From 8a7326a8bb0717e19e4947c6216376c795eea31b Mon Sep 17 00:00:00 2001 From: Carlos Date: Wed, 22 Mar 2023 18:18:37 +0900 Subject: [PATCH 1/5] iceOryx-support --- cyclonedds.xml | 9 ++ roudi_config.toml | 20 ++++ src/tilde/config.yaml | 4 + src/tilde/include/tilde/tilde_publisher.hpp | 44 +++++-- src/tilde/src/tilde_publisher.cpp | 5 +- src/tilde_msg/CMakeLists.txt | 2 +- src/tilde_msg/msg/StaticSize.msg | 3 + src/tilde_sample/CMakeLists.txt | 11 ++ src/tilde_sample/README.md | 78 +++++++++++++ ...lisher_relay_without_header_loan.launch.py | 31 +++++ src/tilde_sample/package.xml | 1 + src/tilde_sample/src/relay_timer_loan.cpp | 110 ++++++++++++++++++ .../src/sample_publisher_loan.cpp | 87 ++++++++++++++ 13 files changed, 396 insertions(+), 9 deletions(-) create mode 100644 cyclonedds.xml create mode 100644 roudi_config.toml create mode 100644 src/tilde/config.yaml create mode 100644 src/tilde_msg/msg/StaticSize.msg create mode 100644 src/tilde_sample/launch/publisher_relay_without_header_loan.launch.py create mode 100644 src/tilde_sample/src/relay_timer_loan.cpp create mode 100644 src/tilde_sample/src/sample_publisher_loan.cpp 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/config.yaml b/src/tilde/config.yaml new file mode 100644 index 00000000..eaaa9b19 --- /dev/null +++ b/src/tilde/config.yaml @@ -0,0 +1,4 @@ +tilde_node: + ros__parameters: + ## if you want to use loaned messages, please turn it on. + is_loaned: false diff --git a/src/tilde/include/tilde/tilde_publisher.hpp b/src/tilde/include/tilde/tilde_publisher.hpp index 1da633e3..c2203225 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,7 +304,8 @@ class TildePublisher : public TildePublisherBase public: RCLCPP_SMART_PTR_DEFINITIONS(TildePublisher) - + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + /// Default constructor TildePublisher( std::shared_ptr info_pub, std::shared_ptr pub, @@ -306,6 +313,20 @@ class TildePublisher : public TildePublisherBase const std::shared_ptr & steady_clock, bool enable) : TildePublisherBase(clock, steady_clock, node_fqn, enable), info_pub_(info_pub), pub_(pub) { + + } + + /// 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) @@ -346,15 +367,22 @@ class TildePublisher : public TildePublisherBase std::cout << "publish SerializedMessage (not supported)" << std::endl; pub_->publish(serialized_msg); } - + /** - * 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 @@ -378,6 +406,7 @@ class TildePublisher : public TildePublisherBase /** * \param has_header_stamp whether main message has header.stamp * \param t header stamp + * \param iceoryx_enable whether iceoryx is enabled */ void publish_info(const std::optional & t) { @@ -411,6 +440,7 @@ class TildePublisher : public TildePublisherBase info_pub_->publish(std::move(msg)); } + }; } // namespace tilde diff --git a/src/tilde/src/tilde_publisher.cpp b/src/tilde/src/tilde_publisher.cpp index 9c7fcb47..6342b7e3 100644 --- a/src/tilde/src/tilde_publisher.cpp +++ b/src/tilde/src/tilde_publisher.cpp @@ -16,6 +16,9 @@ #include "tilde_msg/msg/sub_topic_time_info.hpp" +#include "rclcpp/loaned_message.hpp" +#include "tilde_msg/msg/message_tracking_tag.hpp" + #include #include @@ -30,7 +33,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..cfbb0d76 --- /dev/null +++ b/src/tilde_msg/msg/StaticSize.msg @@ -0,0 +1,3 @@ +int64 id +int64 timestamp +uint8[2147483648] 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..0a50e5e3 100644 --- a/src/tilde_sample/README.md +++ b/src/tilde_sample/README.md @@ -353,3 +353,81 @@ input_infos: # multiple input because explicit API is n sec: 0 nanosec: 0 ``` + +## TILDE_iceoryx-support demo + +### 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. + +``` +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. + +``` +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 +--- +``` \ No newline at end of file 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..d55b708a 100644 --- a/src/tilde_sample/package.xml +++ b/src/tilde_sample/package.xml @@ -14,6 +14,7 @@ sensor_msgs std_msgs tilde + tilde_msg tilde_cmake ament_lint_auto 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..32915e82 --- /dev/null +++ b/src/tilde_sample/src/relay_timer_loan.cpp @@ -0,0 +1,110 @@ +// 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) \ No newline at end of file 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..29525461 --- /dev/null +++ b/src/tilde_sample/src/sample_publisher_loan.cpp @@ -0,0 +1,87 @@ +// 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) \ No newline at end of file From 3f7ab51d44701421467635fd3942f95810558fb8 Mon Sep 17 00:00:00 2001 From: Carlos Date: Wed, 22 Mar 2023 22:18:52 +0900 Subject: [PATCH 2/5] iceOryx-support --- src/tilde/config.yaml | 4 ---- src/tilde/include/tilde/tilde_publisher.hpp | 1 - 2 files changed, 5 deletions(-) delete mode 100644 src/tilde/config.yaml diff --git a/src/tilde/config.yaml b/src/tilde/config.yaml deleted file mode 100644 index eaaa9b19..00000000 --- a/src/tilde/config.yaml +++ /dev/null @@ -1,4 +0,0 @@ -tilde_node: - ros__parameters: - ## if you want to use loaned messages, please turn it on. - is_loaned: false diff --git a/src/tilde/include/tilde/tilde_publisher.hpp b/src/tilde/include/tilde/tilde_publisher.hpp index c2203225..cb70f562 100644 --- a/src/tilde/include/tilde/tilde_publisher.hpp +++ b/src/tilde/include/tilde/tilde_publisher.hpp @@ -406,7 +406,6 @@ class TildePublisher : public TildePublisherBase /** * \param has_header_stamp whether main message has header.stamp * \param t header stamp - * \param iceoryx_enable whether iceoryx is enabled */ void publish_info(const std::optional & t) { From 10bb717aa0164056682fece4b06e3355800c86df Mon Sep 17 00:00:00 2001 From: Carlos Date: Tue, 11 Apr 2023 16:37:57 +0900 Subject: [PATCH 3/5] Update README.md Signed-off-by: Carlos --- src/tilde_sample/README.md | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/tilde_sample/README.md b/src/tilde_sample/README.md index 0a50e5e3..14992737 100644 --- a/src/tilde_sample/README.md +++ b/src/tilde_sample/README.md @@ -381,11 +381,12 @@ Run the publisher and subscriber in the second and third terminal respectively. 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 ``` @@ -397,7 +398,7 @@ ros2 topic echo relay_without_stamp_loan/message_tracking_tag An example of MessageTrackingTag. -``` +```bash header: stamp: sec: 1679292280 @@ -430,4 +431,4 @@ input_infos: sec: 0 nanosec: 0 --- -``` \ No newline at end of file +``` From 8c5b53378274c436e0c6ff2f18ff6747bb2c04c2 Mon Sep 17 00:00:00 2001 From: Carlos Date: Tue, 11 Apr 2023 16:45:53 +0900 Subject: [PATCH 4/5] fix build error(humble) Signed-off-by: Carlos --- .gitignore | 1 + src/tilde/include/tilde/tilde_publisher.hpp | 18 ++++++++---------- src/tilde/src/tilde_publisher.cpp | 3 +-- src/tilde_msg/msg/StaticSize.msg | 2 +- src/tilde_sample/package.xml | 2 +- src/tilde_sample/src/relay_timer_loan.cpp | 16 ++++++++++------ src/tilde_sample/src/sample_publisher_loan.cpp | 11 +++++++---- 7 files changed, 29 insertions(+), 24 deletions(-) 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/src/tilde/include/tilde/tilde_publisher.hpp b/src/tilde/include/tilde/tilde_publisher.hpp index cb70f562..374ccbfc 100644 --- a/src/tilde/include/tilde/tilde_publisher.hpp +++ b/src/tilde/include/tilde/tilde_publisher.hpp @@ -34,7 +34,7 @@ namespace tilde { -template +template class LoanedMessage; /// Internal class to hold input message information @@ -305,7 +305,7 @@ class TildePublisher : public TildePublisherBase public: RCLCPP_SMART_PTR_DEFINITIONS(TildePublisher) using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; - + /// Default constructor TildePublisher( std::shared_ptr info_pub, std::shared_ptr pub, @@ -313,7 +313,6 @@ class TildePublisher : public TildePublisherBase const std::shared_ptr & steady_clock, bool enable) : TildePublisherBase(clock, steady_clock, node_fqn, enable), info_pub_(info_pub), pub_(pub) { - } /// Borrow a loaned ROS message from the middleware. @@ -325,8 +324,7 @@ class TildePublisher : public TildePublisherBase rclcpp::LoanedMessage borrow_loaned_message() { return rclcpp::LoanedMessage( - *pub_, - pub_->get_ros_message_type_allocator()); + *pub_, pub_->get_ros_message_type_allocator()); } void publish(std::unique_ptr msg) @@ -367,20 +365,21 @@ class TildePublisher : public TildePublisherBase std::cout << "publish SerializedMessage (not supported)" << std::endl; pub_->publish(serialized_msg); } - + /** * publish loan message * \param loaned_msg loan message */ void publish(rclcpp::LoanedMessage && loaned_msg) { - if (enable_){ + 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; + // std::cout << "the publish() where defined in tilde_publish.hpp has been called." << + // std::endl; pub_->publish(std::move(loaned_msg)); - }else{ + } else { std::cout << "TILDE is not enabled" << std::endl; } } @@ -439,7 +438,6 @@ class TildePublisher : public TildePublisherBase info_pub_->publish(std::move(msg)); } - }; } // namespace tilde diff --git a/src/tilde/src/tilde_publisher.cpp b/src/tilde/src/tilde_publisher.cpp index 6342b7e3..869be8e0 100644 --- a/src/tilde/src/tilde_publisher.cpp +++ b/src/tilde/src/tilde_publisher.cpp @@ -14,10 +14,9 @@ #include "tilde/tilde_publisher.hpp" -#include "tilde_msg/msg/sub_topic_time_info.hpp" - #include "rclcpp/loaned_message.hpp" #include "tilde_msg/msg/message_tracking_tag.hpp" +#include "tilde_msg/msg/sub_topic_time_info.hpp" #include #include diff --git a/src/tilde_msg/msg/StaticSize.msg b/src/tilde_msg/msg/StaticSize.msg index cfbb0d76..0cd35f39 100644 --- a/src/tilde_msg/msg/StaticSize.msg +++ b/src/tilde_msg/msg/StaticSize.msg @@ -1,3 +1,3 @@ int64 id int64 timestamp -uint8[2147483648] data +uint8[21474] data diff --git a/src/tilde_sample/package.xml b/src/tilde_sample/package.xml index d55b708a..32680f76 100644 --- a/src/tilde_sample/package.xml +++ b/src/tilde_sample/package.xml @@ -14,8 +14,8 @@ sensor_msgs std_msgs tilde - tilde_msg 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 index 32915e82..9592c300 100644 --- a/src/tilde_sample/src/relay_timer_loan.cpp +++ b/src/tilde_sample/src/relay_timer_loan.cpp @@ -16,8 +16,8 @@ #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 @@ -41,7 +41,8 @@ namespace tilde_sample class RelayTimerLoan : public tilde::TildeNode { public: - explicit RelayTimerLoan(const rclcpp::NodeOptions & options) : TildeNode("relay_timer_loan", options) + 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"; @@ -63,14 +64,16 @@ class RelayTimerLoan : public tilde::TildeNode 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); + 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; }); @@ -85,7 +88,8 @@ class RelayTimerLoan : public tilde::TildeNode }; // Create a publisher with a custom Quality of Service profile. - pub_ = this->create_tilde_publisher("relay_without_stamp_loan", qos); + 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); @@ -107,4 +111,4 @@ class RelayTimerLoan : public tilde::TildeNode } // namespace tilde_sample -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::RelayTimerLoan) \ No newline at end of file +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 index 29525461..45ed3708 100644 --- a/src/tilde_sample/src/sample_publisher_loan.cpp +++ b/src/tilde_sample/src/sample_publisher_loan.cpp @@ -16,8 +16,8 @@ #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 @@ -56,7 +56,9 @@ class SamplePublisherWithoutStampLoan : public tilde::TildeNode 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); + RCLCPP_INFO( + this->get_logger(), "(tilde_iceoryx-support) Publishing Message ID: '%ld'", + msg_loan_.get().id); struct timeval tv; gettimeofday(&tv, NULL); @@ -67,7 +69,8 @@ class SamplePublisherWithoutStampLoan : public tilde::TildeNode // 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); + 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); @@ -84,4 +87,4 @@ class SamplePublisherWithoutStampLoan : public tilde::TildeNode } // namespace tilde_sample -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::SamplePublisherWithoutStampLoan) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::SamplePublisherWithoutStampLoan) From 7a98eb5b29b7e3387d66c10fce9498651d79b10f Mon Sep 17 00:00:00 2001 From: Carlos Date: Tue, 11 Apr 2023 17:21:25 +0900 Subject: [PATCH 5/5] Update README.md Signed-off-by: Carlos --- src/tilde_sample/README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/tilde_sample/README.md b/src/tilde_sample/README.md index 14992737..2e563a92 100644 --- a/src/tilde_sample/README.md +++ b/src/tilde_sample/README.md @@ -356,6 +356,9 @@ input_infos: # multiple input because explicit API is n ## 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.