Move from Wiki and Updated Python docs (backport #150) (#152)

* Move Docs From Wiki (#119) Signed-off-by: CursedRock17 <mtglucas1@gmail.com> Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> Co-authored-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
ros2 · Dec 12, 2024 · 457bcd6 · 457bcd6
1 parent fc93cd9
commit 457bcd6
Show file tree

Hide file tree

Showing 13 changed files with 1,086 additions and 99 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -108,7 +108,7 @@ if(BUILD_TESTING)
   if(TARGET ${PROJECT_NAME}-test_approximate_time_policy)
     target_link_libraries(${PROJECT_NAME}-test_approximate_time_policy ${PROJECT_NAME})
   endif()
-  
+
   ament_add_gtest(${PROJECT_NAME}-test_latest_time_policy test/test_latest_time_policy.cpp)
   if(TARGET ${PROJECT_NAME}-test_latest_time_policy)
     target_link_libraries(${PROJECT_NAME}-test_latest_time_policy ${PROJECT_NAME})

diff --git a/doc/Tutorials/Approximate-Synchronizer-Cpp.rst b/doc/Tutorials/Approximate-Synchronizer-Cpp.rst
@@ -0,0 +1,196 @@
+Approximate Time Synchronizer (C++):
+---------------------------------------
+
+Prerequisites
+~~~~~~~~~~~~~
+This tutorial assumes you have a working knowledge of ROS 2
+
+If you have not done so already `create a workspace <https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html>`_ and `create a package <https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html>`_
+
+1. Create a Basic Node with Includes
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. code-block:: C++
+
+  #include "rclcpp/rclcpp.hpp"
+
+  #include <chrono>
+  #include <functional>
+  #include <memory>
+
+  #include "message_filters/subscriber.h"
+  #include "message_filters/synchronizer.h"
+  #include "message_filters/sync_policies/approximate_time.h"
+
+  #include "sensor_msgs/msg/temperature.hpp"
+  #include "sensor_msgs/msg/fluid_pressure.hpp"
+
+  using namespace std::chrono_literals;
+
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+
+  class TimeSyncNode : public rclcpp::Node
+  {
+    public:
+
+    private:
+      rclcpp::Publisher<sensor_msgs::msg::Temperature>::SharedPtr temp_pub;
+      rclcpp::Publisher<sensor_msgs::msg::FluidPressure>::SharedPtr fluid_pub;
+      message_filters::Subscriber<sensor_msgs::msg::Temperature> temp_sub;
+      message_filters::Subscriber<sensor_msgs::msg::FluidPressure> fluid_sub;
+      std::shared_ptr<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<
+        sensor_msgs::msg::Temperature, sensor_msgs::msg::FluidPressure>>> sync;
+      rclcpp::TimerBase::SharedPtr timer;
+      rclcpp::TimerBase::SharedPtr second_timer;
+  };
+
+
+For this example we will be using the ``temperature`` and ``fluid_pressure`` messages found in
+`sensor_msgs <https://github.com/ros2/common_interfaces/tree/humble/sensor_msgs/msg>`_.
+To simulate a working ``Synchronizer`` using the ``ApproximateTime`` Policy. We will be publishing and subscribing to topics of those respective types, to showcase how real sensors would be working.
+
+.. code-block:: C++
+
+    rclcpp::Publisher<sensor_msgs::msg::Temperature>::SharedPtr temp_pub;
+    rclcpp::Publisher<sensor_msgs::msg::FluidPressure>::SharedPtr fluid_pub;
+    message_filters::Subscriber<sensor_msgs::msg::Temperature> temp_sub;
+    message_filters::Subscriber<sensor_msgs::msg::FluidPressure> fluid_sub;
+
+Notice that the ``Subscribers`` are in the ``message_filters`` namespace, while we can utilize ``rclcpp::Publishers``. To simulate them we will also need two ``TimerBases``. Then, we will be utilizing a ``Synchronizer`` to get these messages from the sensor topics aligned.
+
+Next, we can initialize these private elements within a basic ``Node`` constructor
+
+.. code-block:: C++
+
+    public:
+    TimeSyncNode() : Node("sync_node")
+    {
+      rclcpp::QoS qos = rclcpp::QoS(10);
+      temp_pub = this->create_publisher<sensor_msgs::msg::Temperature>("temp", qos);
+      fluid_pub = this->create_publisher<sensor_msgs::msg::FluidPressure>("fluid", qos);
+
+      temp_sub.subscribe(this, "temp", qos.get_rmw_qos_profile());
+      fluid_sub.subscribe(this, "fluid", qos.get_rmw_qos_profile());
+
+      timer = this->create_wall_timer(500ms, std::bind(&TimeSyncNode::TimerCallback, this));
+      second_timer = this->create_wall_timer(550ms, std::bind(&TimeSyncNode::SecondTimerCallback, this));
+
+      uint32_t queue_size = 10;
+      sync = std::make_shared<message_filters::Synchronizer<message_filters::sync_policies::
+        ApproximateTime<sensor_msgs::msg::Temperature, sensor_msgs::msg::FluidPressure>>>(
+        message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Temperature,
+        sensor_msgs::msg::FluidPressure>(queue_size), temp_sub, fluid_sub);
+
+      sync->setAgePenalty(0.50);
+      sync->registerCallback(std::bind(&TimeSyncNode::SyncCallback, this, _1, _2));
+
+     }
+
+It is essential that the QoS is the same for all of the publishers and subscribers, otherwise the Message Filter cannot align the topics together. So, create one ``rclcpp::QoS`` and stick with it, or find out what ``qos`` is being used in the native sensor code, and replicate it. For each private class member, do basic construction of the object relating to the ``Node`` and callback methods that may be used in the future. Both of the two timers we utilize will have different timer values of ``500ms`` and ``550ms`` which causes the timers to off at different points, which is an advantage of using ``ApproximateTime``. This will then work since we called ``setAgePenalty`` to ``0.50`` (50ms)  Notice that we must call ``sync->registerCallback`` to sync up the two (or more) chosen topics.
+
+So, we must create three (or more) private callbacks, one for the ``Synchronizer``, then two for our ``TimerBases`` which are each for a certain ``sensor_msgs``.
+
+.. code-block:: C++
+
+    private:
+
+    void SyncCallback(const sensor_msgs::msg::Temperature::ConstSharedPtr & temp,
+        const sensor_msgs::msg::FluidPressure::ConstSharedPtr & fluid)
+    {
+      RCLCPP_INFO(this->get_logger(), "Sync callback with %u and %u as times",
+        temp->header.stamp.sec, fluid->header.stamp.sec);
+      if (temp->temperature > 2.0)
+      {
+        sensor_msgs::msg::FluidPressure new_fluid;
+        new_fluid.header.stamp = rclcpp::Clock().now();
+        new_fluid.header.frame_id = "test";
+        new_fluid.fluid_pressure = 2.5;
+        fluid_pub->publish(new_fluid);
+      }
+    }
+
+    void TimerCallback()
+    {
+      sensor_msgs::msg::Temperature temp;
+      auto now = this->get_clock()->now();
+      temp.header.stamp = now;
+      temp.header.frame_id = "test";
+      temp.temperature = 1.0;
+      temp_pub->publish(temp);
+    }
+
+    void SecondTimerCallback()
+    {
+      sensor_msgs::msg::FluidPressure fluid;
+      auto now = this->get_clock()->now();
+      fluid.header.stamp = now;
+      fluid.header.frame_id = "test";
+      fluid.fluid_pressure = 2.0;
+      fluid_pub->publish(fluid);
+    }
+
+
+``SyncCallback`` takes ``const shared_ptr references`` relating to both topics becasue they will be taken at the exact time, from here you can compare these topics, set values, etc. This callback is the final goal of synching multiple topics and the reason why the qos and header stamps must be the same. This will be seen with the logging statement as both of the times will be the same. Though, the headers have to have the same ``stamp`` value, they don't have to be triggered at the same time with ``ApproximateTime`` which will be seen in a delay between logging calls. For the ``TimerCallback`` just initialize both the ``Temperature`` and ``FluidPressure`` in whatever way necessary. .
+
+Finally, create a main function and spin the node
+
+.. code-block:: C++
+
+    int main(int argc, char ** argv)
+    {
+      rclcpp::init(argc, argv);
+      auto node = std::make_shared<TimeSyncNode>();
+      rclcpp::spin(node);
+      rclcpp::shutdown();
+
+      return 0;
+    }
+
+
+2. Add the Node to a CMakeLists.txt
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+Now open the ``CMakeLists.txt`` add the executable and name it ``approximate_time_sync``, which you’ll use later with ``ros2 run``.
+
+.. code-block:: C++
+
+   find_package(rclcpp REQUIRED)
+   find_package(sensor_msgs REQUIRED)
+   find_package(message_filters REQUIRED)
+
+   add_executable(approximate_time_sync src/approximate_time_synchronizer.cpp)
+   ament_target_dependencies(approximate_time_sync rclcpp sensor_msgs message_filters)
+
+Finally, add the ``install(TARGETS…)`` section so ``ros2 run`` can find your executable:
+
+.. code-block:: C++
+
+    install(TARGETS
+        approximate_time_sync
+        DESTINATION lib/${PROJECT_NAME})
+
+
+3. Build
+~~~~~~~~
+From the root of your package, build and source.
+
+.. code-block:: bash
+
+    colcon build && . install/setup.zsh
+
+4. Run
+~~~~~~
+Run replacing the package name with whatever you named your workspace.
+
+.. code-block:: bash
+
+   ros2 run pkg_name approximate_time_sync
+
+You should end up with a result similar to the following:
+
+.. code-block:: bash
+
+    [INFO] [1714888439.264005000] [sync_node]: Sync callback with 1714888438 and 1714888438 as times
+    [INFO] [1714888445.263986000] [sync_node]: Sync callback with 1714888444 and 1714888444 as times
+
+* Note the ~0.5 second difference between each callback, this is because the ``ApproximateTime`` calls will be stored in a queue which can seen to trigger once the headers of the two (or more) elements are the same, which makes sense because our longest timer wait is ``550ms``, aligning with our age penalty.
diff --git a/doc/Tutorials/Approximate-Synchronizer-Python.rst b/doc/Tutorials/Approximate-Synchronizer-Python.rst
@@ -0,0 +1,148 @@
+Approximate Time Synchronizer (Python):
+---------------------------------------
+
+Prerequisites
+~~~~~~~~~~~~~
+This tutorial assumes you have a working knowledge of ROS 2
+
+If you have not done so already `create a workspace <https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html>`_ and `create a package <https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html>`_
+
+1. Create a Basic Node with Includes
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. code-block:: Python
+
+    import rclpy
+    from rclpy.node import Node
+    from rclpy.qos import QoSProfile
+    from rclpy.clock import Clock
+
+    from message_filters import Subscriber, ApproximateTimeSynchronizer
+    from sensor_msgs.msg import Temperature, FluidPressure
+
+
+    class TimeSyncNode(Node):
+
+        def __init__(self):
+            super().__init__('sync_node')
+            qos = QoSProfile(depth=10)
+            self.temp_pub = self.create_publisher(Temperature, 'temp', qos)
+            self.fluid_pub = self.create_publisher(FluidPressure, 'fluid', qos)
+            self.temp_sub = Subscriber(self, Temperature, "temp")
+            self.fluid_sub = Subscriber(self, FluidPressure, "fluid")
+
+            self.timer = self.create_timer(1, self.TimerCallback)
+            self.second_timer = self.create_timer(1.05, self.SecondTimerCallback)
+
+            queue_size = 10
+            max_delay = 0.05
+            self.time_sync = ApproximateTimeSynchronizer([self.temp_sub, self.fluid_sub],
+                                                         queue_size, max_delay)
+            self.time_sync.registerCallback(self.SyncCallback)
+
+
+For this example we will be using the ``temperature`` and ``fluid_pressure`` messages found in
+`sensor_msgs <https://github.com/ros2/common_interfaces/tree/humble/sensor_msgs/msg>`_.
+To simulate a working ``ApproximateTimeSynchronizer``. We will be publishing and subscribing to topics of those respective types, to showcase how real sensors would be working. To simulate them we will also need two ``Timers`` on different intervals. Then, we will be utilizing an ``ApproximateTimeSynchronizer`` to get these messages from the sensor topics aligned with a slight delay between messages..
+It is essential that the QoS is the same for all of the publishers and subscribers, otherwise the Message Filter cannot align the topics together. So, create one ``QoSProfile`` and stick with it, or find out what ``qos`` is being used in the native sensor code, and replicate it. Do basic construction of each object relating to the ``Node`` and callback methods that may be used in the future. Both of the two timers we utilize will have different timer values of ``1`` and ``1.05`` which causes the timers to off at different points, which is an advantage of using ``ApproximateTime``. Notice that we must call ``sync->registerCallback`` to sync up the two (or more) chosen topics.
+
+So, we must create three (or more) private callbacks, one for the ``ApproximateTimeSynchronizer``, then two for our ``Timers`` which are each for a certain ``sensor_msg``.
+
+.. code-block:: Python
+
+    def SyncCallback(self, temp, fluid):
+        temp_sec = temp.header.stamp.sec
+        fluid_sec = fluid.header.stamp.sec
+        self.get_logger().info(f'Sync callback with {temp_sec} and {fluid_sec} as times')
+        if (temp.header.stamp.sec > 2.0):
+            new_fluid = FluidPressure()
+            new_fluid.header.stamp = Clock().now().to_msg()
+            new_fluid.header.frame_id = 'test'
+            new_fluid.fluid_pressure = 2.5
+            self.fluid_pub.publish(new_fluid)
+
+    def TimerCallback(self):
+        temp = Temperature()
+        self.now = Clock().now().to_msg()
+
+        temp.header.stamp = self.now
+        temp.header.frame_id = 'test'
+        temp.temperature = 1.0
+        self.temp_pub.publish(temp)
+
+    def SecondTimerCallback(self):
+        fluid = FluidPressure()
+        self.now = Clock().now().to_msg()
+
+        fluid.header.stamp = self.now
+        fluid.header.frame_id = "test"
+        fluid.fluid_pressure = 2.0
+        self.fluid_pub.publish(fluid)
+
+
+``SyncCallback`` takes both messages relating to both topics, then, from here you can compare these topics, set values, etc. This callback is the final goal of synching multiple topics and the reason why the qos must be the same. This will be seen with the logging statement as both of the times will be the same. Though, the headers have to have the same ``stamp`` value, they don't have to be triggered at the same time using an ``ApproximateTimeSynchronizer`` which will be seen in a delay between logging calls. For both ``TimerCallbacks`` just initialize both the ``Temperature`` and ``FluidPressure`` in whatever way necessary. .
+
+Finally, create a main function and spin the node
+
+.. code-block:: Python
+
+    def main(args=None):
+        rclpy.init(args=args)
+
+        time_sync = TimeSyncNode()
+
+        rclpy.spin(time_sync)
+
+        time_sync.destroy_node()
+        rclpy.shutdown()
+
+
+    if __name__ == '__main__':
+        main()
+
+2. Add the Node to Python Setup
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+2.1 Update package.xml
+^^^^^^^^^^^^^^^^^^^^^^
+Navigate to the root of your package's directory, where ``package.xml`` is located, open, and add the following dependencies:
+
+.. code-block:: Python
+
+   <exec_depend>rclpy</exec_depend>
+   <exec_depend>message_filters</exec_depend>
+   <exec_depend>sensor_msgs</exec_depend>
+
+2.2 Add an entry point
+^^^^^^^^^^^^^^^^^^^^^^
+To allow the ``ros2 run`` command to run your node, you must add the entry point to ``setup.py``.
+
+Add the following line between the 'console_scripts': brackets, with the name of your package:
+
+.. code-block:: Python
+
+   'approximate_time_sync = pkg_name.approximate_time_sync:main',
+
+
+3. Build
+~~~~~~~~
+From the root of your package, build and source.
+
+.. code-block:: bash
+
+    colcon build && . install/setup.zsh
+
+4. Run
+~~~~~~
+Run replacing the package name with whatever you named your workspace.
+
+.. code-block:: bash
+
+   ros2 run pkg_name approximate_time_sync
+
+You should end up with a result similar to the following:
+
+.. code-block:: bash
+
+   [INFO] [1714927893.485850000] [sync_node]: Sync callback with 1714927893 and 1714927893 as times
+   [INFO] [1714927894.489608000] [sync_node]: Sync callback with 1714927894 and 1714927894 as times