feat: iceoryx-support

.gitignore

@@ -6,3 +6,4 @@ __pycache__
 /*/*npy
 src/autoware_common
 src/autoware_msg
+.vscode/

cyclonedds.xml

@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="https://cdds.io/config https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/iceoryx/etc/cyclonedds.xsd">
+    <Domain id="any">
+        <SharedMemory>
+            <Enable>true</Enable>
+            <LogLevel>info</LogLevel>
+        </SharedMemory>
+    </Domain>
+</CycloneDDS>

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

src/tilde/include/tilde/tilde_publisher.hpp

@@ -34,6 +34,9 @@
 namespace tilde
 {
 
+template <typename MessageT, typename AllocatorT>
+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<tilde_msg::msg::MessageTrackingTag> & 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<MessageT>::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<ROSMessageType, AllocatorT> borrow_loaned_message()
+  {
+    return rclcpp::LoanedMessage<ROSMessageType, AllocatorT>(
+      *pub_, pub_->get_ros_message_type_allocator());
+  }
+
   void publish(std::unique_ptr<MessageT, MessageDeleter> 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<MessageT, AllocatorT> && loaned_msg)
+  void publish(rclcpp::LoanedMessage<ROSMessageType, AllocatorT> && 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

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 <memory>
@@ -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;
 }
 

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

src/tilde_msg/msg/StaticSize.msg

@@ -0,0 +1,3 @@
+int64 id
+int64 timestamp
+uint8[21474] data

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)

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
+---
+```

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'),
+    ])

src/tilde_sample/package.xml

@@ -15,6 +15,7 @@
   <depend>std_msgs</depend>
   <depend>tilde</depend>
   <depend>tilde_cmake</depend>
+  <depend>tilde_msg</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>caret_lint_common</test_depend>