diff --git a/.gitignore b/.gitignore index 7c3e5fcb..a7a6b4ea 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,4 @@ __pycache__ /*/*npy src/autoware_common src/autoware_msg +.vscode/ diff --git a/cyclonedds.xml b/cyclonedds.xml new file mode 100644 index 00000000..d6077a69 --- /dev/null +++ b/cyclonedds.xml @@ -0,0 +1,9 @@ + + + + + true + info + + + diff --git a/roudi_config.toml b/roudi_config.toml new file mode 100644 index 00000000..3ca8256f --- /dev/null +++ b/roudi_config.toml @@ -0,0 +1,20 @@ +[general] +version = 1 + +[[segment]] + +[[segment.mempool]] +size = 1024 +count =1000 + +[[segment.mempool]] +size = 16384 +count = 1000 + +[[segment.mempool]] +size = 131072 +count = 200 + +[[segment.mempool]] +size = 2200000000 +count = 3 diff --git a/src/tilde/include/tilde/tilde_publisher.hpp b/src/tilde/include/tilde/tilde_publisher.hpp index 1da633e3..374ccbfc 100644 --- a/src/tilde/include/tilde/tilde_publisher.hpp +++ b/src/tilde/include/tilde/tilde_publisher.hpp @@ -34,6 +34,9 @@ namespace tilde { +template +class LoanedMessage; + /// Internal class to hold input message information class InputInfo { @@ -227,6 +230,9 @@ class TildePublisherBase */ void fill_input_info(tilde_msg::msg::MessageTrackingTag & info_msg); + // loan message + void fill_input_info_loaned(rclcpp::LoanedMessage & info_msg); + /// Set how long to hold InputInfo /** * TildePublisher holds InputInfo of every message just a seconds to @@ -298,6 +304,7 @@ class TildePublisher : public TildePublisherBase public: RCLCPP_SMART_PTR_DEFINITIONS(TildePublisher) + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; /// Default constructor TildePublisher( @@ -308,6 +315,18 @@ class TildePublisher : public TildePublisherBase { } + /// Borrow a loaned ROS message from the middleware. + /** + * \sa rclcpp::LoanedMessage for details of the LoanedMessage class. + * + * \return LoanedMessage containing memory for a ROS message of type ROSMessageType + */ + rclcpp::LoanedMessage borrow_loaned_message() + { + return rclcpp::LoanedMessage( + *pub_, pub_->get_ros_message_type_allocator()); + } + void publish(std::unique_ptr msg) { if (enable_) { @@ -348,13 +367,21 @@ class TildePublisher : public TildePublisherBase } /** - * publish() variant - * can send a main message but cannot send the corresponding MessageTrackingTag + * publish loan message + * \param loaned_msg loan message */ - void publish(rclcpp::LoanedMessage && loaned_msg) + void publish(rclcpp::LoanedMessage && loaned_msg) { - std::cout << "publish LoanedMessage (not supported)" << std::endl; - pub_->publish(loaned_msg); + if (enable_) { + // register publish_info + auto stamp = get_timestamp(clock_->now()); + publish_info(stamp); + // std::cout << "the publish() where defined in tilde_publish.hpp has been called." << + // std::endl; + pub_->publish(std::move(loaned_msg)); + } else { + std::cout << "TILDE is not enabled" << std::endl; + } } // TODO(y-okumura-isp) get_allocator diff --git a/src/tilde/src/tilde_publisher.cpp b/src/tilde/src/tilde_publisher.cpp index 9c7fcb47..869be8e0 100644 --- a/src/tilde/src/tilde_publisher.cpp +++ b/src/tilde/src/tilde_publisher.cpp @@ -14,6 +14,8 @@ #include "tilde/tilde_publisher.hpp" +#include "rclcpp/loaned_message.hpp" +#include "tilde_msg/msg/message_tracking_tag.hpp" #include "tilde_msg/msg/sub_topic_time_info.hpp" #include @@ -30,7 +32,7 @@ bool InputInfo::operator==(const InputInfo & rhs) const rclcpp::Time tilde::get_timestamp(rclcpp::Time t, ...) { - std::cout << "get rclcpp::Time t" << std::endl; + // std::cout << "get rclcpp::Time t" << std::endl; return t; } diff --git a/src/tilde_msg/CMakeLists.txt b/src/tilde_msg/CMakeLists.txt index a0064fd2..d2161b72 100644 --- a/src/tilde_msg/CMakeLists.txt +++ b/src/tilde_msg/CMakeLists.txt @@ -27,7 +27,7 @@ set(msg_files "msg/TestTopLevelStamp.msg" "msg/Source.msg" "msg/DeadlineNotification.msg" - + "msg/StaticSize.msg" "msg/SteeSource.msg" # sensor_msgs diff --git a/src/tilde_msg/msg/StaticSize.msg b/src/tilde_msg/msg/StaticSize.msg new file mode 100644 index 00000000..0cd35f39 --- /dev/null +++ b/src/tilde_msg/msg/StaticSize.msg @@ -0,0 +1,3 @@ +int64 id +int64 timestamp +uint8[21474] data diff --git a/src/tilde_sample/CMakeLists.txt b/src/tilde_sample/CMakeLists.txt index 4180bfaa..b5b31374 100644 --- a/src/tilde_sample/CMakeLists.txt +++ b/src/tilde_sample/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(sensor_msgs REQUIRED) find_package(tilde_cmake REQUIRED) tilde_package() find_package(tilde REQUIRED) +find_package(tilde_msg REQUIRED) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -32,6 +33,8 @@ endif() include_directories(include) add_library(tilde_sample SHARED + src/sample_publisher_loan.cpp + src/relay_timer_loan.cpp src/sample_publisher.cpp src/relay_timer.cpp src/relay_timer_with_buffer.cpp @@ -42,8 +45,16 @@ ament_target_dependencies(tilde_sample "rclcpp_components" "std_msgs" "tilde" + "tilde_msg" "sensor_msgs") +rclcpp_components_register_node(tilde_sample + PLUGIN "tilde_sample::SamplePublisherWithoutStampLoan" + EXECUTABLE publisher_without_stamp_loan) +rclcpp_components_register_node(tilde_sample + PLUGIN "tilde_sample::RelayTimerLoan" + EXECUTABLE relay_timer_loan) + rclcpp_components_register_node(tilde_sample PLUGIN "tilde_sample::SamplePublisherWithStamp" EXECUTABLE publisher_with_stamp) diff --git a/src/tilde_sample/README.md b/src/tilde_sample/README.md index ee6788ce..2e563a92 100644 --- a/src/tilde_sample/README.md +++ b/src/tilde_sample/README.md @@ -353,3 +353,85 @@ input_infos: # multiple input because explicit API is n sec: 0 nanosec: 0 ``` + +## TILDE_iceoryx-support demo + +TILDE needs to use `std_msg/header` when tracing messages, but it is a bad news that zero copy does not support handling dynamic size messages. +However the message is supported if there is a stamp field directly below the message even if there is no header. + +### Adjusting the size of the data + +Modify the array size. + +```bash +## path: src/tilde_msg/msg/StaticSize.msg +int64 id +int64 timestamp +uint8[2147483648] data <-- +``` + +### Demo startup + +Launch four terminals, run RouDi daemon in the first terminal. + +```bash +iox-roudi -c roudi_config.toml +``` + +Run the publisher and subscriber in the second and third terminal respectively. + +```bash +CYCLONEDDS_URI=file://$PWD/cyclonedds.xml ros2 run tilde_sample publisher_without_stamp_loan +CYCLONEDDS_URI=file://$PWD/cyclonedds.xml ros2 run tilde_sample relay_timer_loan +``` + +![pic](https://i.328888.xyz/2023/03/20/PNzpL.png) + +Or use the following `ros2 launch` command to run both nodes. + +```bash +CYCLONEDDS_URI=file://$PWD/cyclonedds.xml ros2 launch tilde_sample publisher_relay_without_header_loan.launch.py +``` + +See MessageTrackingTag by execute the following commands. + +```bash +ros2 topic echo relay_without_stamp_loan/message_tracking_tag +``` + +An example of MessageTrackingTag. + +```bash +header: + stamp: + sec: 1679292280 + nanosec: 163271900 + frame_id: '' +output_info: + topic_name: /relay_without_stamp_loan + node_fqn: '' + seq: 164 + pub_time: + sec: 1679292280 + nanosec: 163274465 + pub_time_steady: + sec: 7547 + nanosec: 125041406 + has_header_stamp: false + header_stamp: + sec: 0 + nanosec: 0 +input_infos: +- topic_name: /topic_without_stamp_loan + sub_time: + sec: 1679292280 + nanosec: 152736919 + sub_time_steady: + sec: 7547 + nanosec: 114505558 + has_header_stamp: false + header_stamp: + sec: 0 + nanosec: 0 +--- +``` diff --git a/src/tilde_sample/launch/publisher_relay_without_header_loan.launch.py b/src/tilde_sample/launch/publisher_relay_without_header_loan.launch.py new file mode 100644 index 00000000..7034ea09 --- /dev/null +++ b/src/tilde_sample/launch/publisher_relay_without_header_loan.launch.py @@ -0,0 +1,31 @@ +# Copyright 2022 Research Institute of Systems Planning, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Launch a publisher and a relay.""" + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='tilde_sample', + executable='publisher_without_stamp_loan', + output='screen'), + Node( + package='tilde_sample', + executable='relay_timer_loan', + output='screen'), + ]) diff --git a/src/tilde_sample/package.xml b/src/tilde_sample/package.xml index 1852e335..32680f76 100644 --- a/src/tilde_sample/package.xml +++ b/src/tilde_sample/package.xml @@ -15,6 +15,7 @@ std_msgs tilde tilde_cmake + tilde_msg ament_lint_auto caret_lint_common diff --git a/src/tilde_sample/src/relay_timer_loan.cpp b/src/tilde_sample/src/relay_timer_loan.cpp new file mode 100644 index 00000000..9592c300 --- /dev/null +++ b/src/tilde_sample/src/relay_timer_loan.cpp @@ -0,0 +1,114 @@ +// Copyright 2022 Research Institute of Systems Planning, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde/tilde_publisher.hpp" +#include "tilde_msg/msg/static_size.hpp" + +#include "std_msgs/msg/string.hpp" + +#include +// #include + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +const int64_t TIMER_MS_DEFAULT = 1000; +const int64_t PROC_MS_DEFAULT = 10; + +namespace tilde_sample +{ +// Create a Talker class that subclasses the generic rclcpp::Node base class. +// The main function below will instantiate the class as a ROS node. +class RelayTimerLoan : public tilde::TildeNode +{ +public: + explicit RelayTimerLoan(const rclcpp::NodeOptions & options) + : TildeNode("relay_timer_loan", options) + { + const std::string TIMER_MS = "timer_ms"; + const std::string PROC_MS = "proc_ms"; + const std::string UPDATE_STAMP = "update_stamp"; + + declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); + declare_parameter(PROC_MS, PROC_MS_DEFAULT); + declare_parameter(UPDATE_STAMP, false); + auto timer_ms = get_parameter(TIMER_MS).get_value(); + auto proc_ms = get_parameter(PROC_MS).get_value(); + auto update_stamp = get_parameter(UPDATE_STAMP).get_value(); + + rclcpp::QoS qos(rclcpp::KeepLast(7)); + + sub_loan_ = this->create_tilde_subscription( + "topic_without_stamp_loan", qos, [this](tilde_msg::msg::StaticSize::SharedPtr msg) -> void { + struct timeval tv; + gettimeofday(&tv, NULL); + auto now = tv.tv_sec * 1000 * 1000 + tv.tv_usec; + uint64_t latency = now - msg->timestamp; + + RCLCPP_INFO( + this->get_logger(), "(tilde_iceoryx-support) I heard message ID: '%ld', latency = %ld us", + msg->id, latency); + + // write to .csv file(latency) + // std::ofstream oFile; + // oFile.open("tilde_iceoryx_support_latency.csv", std::ios::app); + // oFile << msg->id << "," << latency << std::endl; + // oFile.close(); + + msg_loan_ = msg; + }); + + // Create a function for when messages are to be sent. + auto proc_dur = std::chrono::duration(proc_ms); + auto timer_callback = [this, proc_dur, update_stamp]() -> void { + std::this_thread::sleep_for(proc_dur); + + if (msg_loan_) { + pub_->publish(*msg_loan_); + } + }; + + // Create a publisher with a custom Quality of Service profile. + pub_ = + this->create_tilde_publisher("relay_without_stamp_loan", qos); + + auto timer_dur = std::chrono::duration(timer_ms); + timer_ = this->create_wall_timer(timer_dur, timer_callback); + } + +private: + std::shared_ptr msg_loan_; + rclcpp::Subscription::SharedPtr sub_loan_; + tilde::TildePublisher::SharedPtr pub_; + + rclcpp::TimerBase::SharedPtr timer_; + + uint64_t nanoseconds(const builtin_interfaces::msg::Time & time_msg) + { + rclcpp::Time time(time_msg); + return time.nanoseconds(); + } +}; + +} // namespace tilde_sample + +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::RelayTimerLoan) diff --git a/src/tilde_sample/src/sample_publisher_loan.cpp b/src/tilde_sample/src/sample_publisher_loan.cpp new file mode 100644 index 00000000..45ed3708 --- /dev/null +++ b/src/tilde_sample/src/sample_publisher_loan.cpp @@ -0,0 +1,90 @@ +// Copyright 2022 Research Institute of Systems Planning, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde/tilde_publisher.hpp" +#include "tilde_msg/msg/static_size.hpp" + +#include "std_msgs/msg/string.hpp" + +#include + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +const int64_t TIMER_MS_DEFAULT = 1000; + +namespace tilde_sample +{ +/** + * This class sends loan messages without header.stamp field. + */ +class SamplePublisherWithoutStampLoan : public tilde::TildeNode +{ +public: + explicit SamplePublisherWithoutStampLoan(const rclcpp::NodeOptions & options) + : TildeNode("talker_without_stamp_loan", options) + { + const std::string TIMER_MS = "timer_ms"; + + declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); + auto timer_ms = get_parameter(TIMER_MS).get_value(); + std::cout << "timer_ms: " << timer_ms << std::endl; + + // Create a function for when messages are to be sent. + auto publish_loan_message = [this]() -> void { + auto time_now = this->now(); + + auto msg_loan_ = pub_loan_->borrow_loaned_message(); + msg_loan_.get().id = count_++; + + RCLCPP_INFO( + this->get_logger(), "(tilde_iceoryx-support) Publishing Message ID: '%ld'", + msg_loan_.get().id); + + struct timeval tv; + gettimeofday(&tv, NULL); + msg_loan_.get().timestamp = tv.tv_sec * 1000 * 1000 + tv.tv_usec; + + pub_loan_->publish(std::move(msg_loan_)); + }; + + // Create a publisher with a custom Quality of Service profile. + rclcpp::QoS qos(rclcpp::KeepLast(7)); + pub_loan_ = + this->create_tilde_publisher("topic_without_stamp_loan", qos); + + // Use a timer to schedule periodic message publishing. + auto dur = std::chrono::duration(timer_ms); + timer_ = this->create_wall_timer(dur, publish_loan_message); + } + +private: + size_t count_ = 0; + + tilde::TildePublisher::SharedPtr pub_loan_; + + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace tilde_sample + +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::SamplePublisherWithoutStampLoan)