diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4f76481f..b632b588 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,92 @@ Changelog for package message_filters ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.3.16 (2026-04-09) +------------------- +* DeltaFilter(C++): Add DeltaFilter class. (backport `#273 `_) (`#276 `_) +* Contributors: mergify[bot] + +4.3.15 (2026-03-24) +------------------- +* Tutorials minor fixers: Replace the TODOs with the actual links to other tutorials as required. Rename Approximate-Tyme tutorial to Approximate-Time (`#266 `_) +* Tutorials: Add LatestTime synchronization policy tutorial (`#266 `_) +* Tutorials: Approximate-Synchronizer: Label CMake code blocks with the right language markings +* Tutorials: Add C++ tutorial for Approximate Epsilon Time Sync policy +* DeltaFilter(Python): Add DeltaFilter for Python. Add tests. Add docstring to filters and comparison handlers (`#252 `_) (`#260 `_) + Co-authored-by: Pavel Esipov <38457822+EsipovPA@users.noreply.github.com> +* Contributors: EsipovPA, mergify[bot] + +4.3.14 (2026-02-18) +------------------- +* `#253 `_ message_filters.chain.python: Remove dataclass. Make FilterInfo a regular class with regular __init_\_ (`#254 `_) +* Contributors: Pavel Esipov + +4.3.13 (2026-02-04) +------------------- +* Add kwargs passing from Subscriber to node.create_subscription (`#247 `_) (`#250 `_) +* Contributors: mergify[bot] + +4.3.12 (2026-01-15) +------------------- +* `#200 `_ fix inconsistensy between cpp and python exact time synchronizer impl (backport `#238 `_) (`#245 `_) +* `#130 `_ add simple filter tutorial for cpp (backport `#239 `_) (`#242 `_) +* Add simple filter tutorials (backport `#226 `_) (`#230 `_) +* Add chain tutorial python (`#219 `_) (`#225 `_) +* Contributors: mergify[bot] + +4.3.11 (2025-10-15) +------------------- +* Add Python implementation for a Chain filter (backport `#213 `_) (`#216 `_) +* Update repository URL in package.xml (`#217 `_) (`#218 `_) +* Contributors: mergify[bot] + +4.3.10 (2025-10-03) +------------------- +* Some fixes to documentation (backport `#208 `_) (`#212 `_) +* Create a Chain class tutorial for C++ (`#203 `_) (`#207 `_) +* Contributors: mergify[bot] + +4.3.9 (2025-09-17) +------------------ +* Add 'Cache (C++)' tutorial (`#196 `_) (`#199 `_) +* Contributors: mergify[bot] + +4.3.8 (2025-07-16) +------------------ +* Fix cache tutorial: added tab extension (backport `#190 `_) (`#193 `_) +* Add tutorial for Cache filter for Python (`#185 `_) (`#189 `_) +* Contributors: mergify[bot] + +4.3.7 (2025-04-08) +------------------ +* Future port hpp files (`#170 `_) (`#172 `_) +* Contributors: mergify[bot] + +4.3.6 (2025-03-25) +------------------ +* Move from Wiki and Updated Python docs (backport `#150 `_) (`#152 `_) +* Contributors: mergify[bot] + +4.3.5 (2024-07-31) +------------------ +* [LatestTimeSync] Fix crash when Synchronizer is started before the messges are available. (`#136 `_) +* Contributors: Dr. Denis + +4.3.4 (2024-05-15) +------------------ +* Fix cache.h std::placeholder namespace (`#87 `_) (`#121 `_) +* Contributors: mergify[bot] + +4.3.3 (2023-04-25) +------------------ +* Add latest time zero-order-hold sync policy (`#73 `_) (`#89 `_) +* Contributors: mergify[bot] + +4.3.2 (2022-06-20) +------------------ +* Adding fix to subscribe() call with raw node pointer and subscriber options (`#76 `_) (`#77 `_) +* Contributors: Steve Macenski + 4.3.1 (2022-03-25) ------------------ * Use RCL_ROS_TIME for message_traits::TimeStamp (`#72 `_) diff --git a/CMakeLists.txt b/CMakeLists.txt index e79687c0..03e68dfe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -109,6 +109,11 @@ if(BUILD_TESTING) 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}) + endif() + ament_add_gtest(${PROJECT_NAME}-test_fuzz test/test_fuzz.cpp SKIP_TEST) if(TARGET ${PROJECT_NAME}-test_fuzz) target_link_libraries(${PROJECT_NAME}-test_fuzz ${PROJECT_NAME}) @@ -131,14 +136,23 @@ if(BUILD_TESTING) endif() endif() + ament_add_gtest(${PROJECT_NAME}-test_message_filters_delta_filter test/test_message_filters_delta_filter.cpp) + if(TARGET ${PROJECT_NAME}-test_message_filters_delta_filter) + target_link_libraries(${PROJECT_NAME}-test_message_filters_delta_filter ${PROJECT_NAME}) + endif() + # python tests with python interfaces of message filters find_package(ament_cmake_pytest REQUIRED) - ament_add_pytest_test(directed.py "test/directed.py" + ament_add_pytest_test(test_time_synchronizer.py "test/test_time_synchronizer.py" PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") ament_add_pytest_test(test_approxsync.py "test/test_approxsync.py" PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") ament_add_pytest_test(test_message_filters_cache.py "test/test_message_filters_cache.py" PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") + ament_add_pytest_test(test_message_filters_chain.py "test/test_message_filters_chain.py" + PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") + ament_add_pytest_test(test_input_aligner.py "test/test_input_aligner.py" + PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") endif() ament_package() diff --git a/doc/Tutorials/Approximate-Epsilon-Time-Synchronizer-Cpp.rst b/doc/Tutorials/Approximate-Epsilon-Time-Synchronizer-Cpp.rst new file mode 100644 index 00000000..a8257ef4 --- /dev/null +++ b/doc/Tutorials/Approximate-Epsilon-Time-Synchronizer-Cpp.rst @@ -0,0 +1,442 @@ +Approximate Epsilon 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 `_ and `create a package `_ + +1. Create a Basic Node with Includes +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Let's assume, you've already created an empty ROS 2 package for C++. +The next step is to create a new C++ file inside your package, e.g., ``approximate_time_sync_tutorial``, and create an example node: + +.. code-block:: C++ + + + #include + #include + #include + + #include "rclcpp/rclcpp.hpp" + + #include + #include + + #include "message_filters/subscriber.hpp" + #include "message_filters/synchronizer.hpp" + #include "message_filters/sync_policies/approximate_epsilon_time.hpp" + + using namespace std::chrono_literals; + + using std::placeholders::_1; + using std::placeholders::_2; + + class EpsTimeSyncNode : public rclcpp::Node { + public: + typedef sensor_msgs::msg::Temperature TemperatureMsg; + typedef sensor_msgs::msg::FluidPressure FluidPressureMsg; + typedef message_filters::sync_policies::ApproximateEpsilonTime< + sensor_msgs::msg::Temperature, + sensor_msgs::msg::FluidPressure + > SyncPolicy; + + EpsTimeSyncNode(): Node("epsilon_time_sync_node") { + rclcpp::QoS qos = rclcpp::QoS(10); + temp_pub = this->create_publisher("temp", qos); + fluid_pub = this->create_publisher("fluid", qos); + + temp_sub.subscribe(this, "temp", qos); + fluid_sub.subscribe(this, "fluid", qos); + + temperature_timer = this->create_wall_timer(500ms, std::bind(&EpsTimeSyncNode::TemperatureTimerCallback, this)); + fluid_pressure_timer = this->create_wall_timer(550ms, std::bind(&EpsTimeSyncNode::FluidPressureTimerCallback, this)); + + uint32_t queue_size = 10; + sync = std::make_shared> ( + SyncPolicy ( + queue_size, + rclcpp::Duration(std::chrono::seconds(1)) + ), + temp_sub, + fluid_sub + ); + + sync->registerCallback(std::bind(&EpsTimeSyncNode::SyncCallback, this, _1, _2)); + } + + private: + + void SyncCallback( + const TemperatureMsg::ConstSharedPtr & temp, + const FluidPressureMsg::ConstSharedPtr & fluid + ) { + RCLCPP_INFO( + this->get_logger(), + "Sync callback with %u.%u and %u.%u as times", + temp->header.stamp.sec, + temp->header.stamp.nanosec, + fluid->header.stamp.sec, + fluid->header.stamp.nanosec + ); + + if (temp->temperature > 2.0) + { + FluidPressureMsg 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 TemperatureTimerCallback() + { + TemperatureMsg 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 FluidPressureTimerCallback() + { + FluidPressureMsg 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); + } + + private: + rclcpp::Publisher::SharedPtr temp_pub; + rclcpp::Publisher::SharedPtr fluid_pub; + + message_filters::Subscriber temp_sub; + message_filters::Subscriber fluid_sub; + + std::shared_ptr> sync; + + rclcpp::TimerBase::SharedPtr temperature_timer; + rclcpp::TimerBase::SharedPtr fluid_pressure_timer; + }; + + int main(int argc, char ** argv) + { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; + } + +1.1 Examine the code +~~~~~~~~~~~~~~~~~~~~ +Now, let's break down this code and examine the details. + +.. code-block:: C++ + + #include + #include + #include + + #include "rclcpp/rclcpp.hpp" + + #include + #include + + #include "message_filters/subscriber.hpp" + #include "message_filters/synchronizer.hpp" + #include "message_filters/sync_policies/approximate_epsilon_time.hpp" + + using namespace std::chrono_literals; + + using std::placeholders::_1; + using std::placeholders::_2; + +We start with the include section. First of all we include the headers from standard library. +The ``chrono`` header is for the duration classes and the ``chrono_literals`` namespace. +The ``functional`` header is included to gain access to the ``std::bind`` function template. +And the ``memory`` header provides us with ``std::shared_ptr`` class and ``std::make_shared`` function template. +The ``rclcpp.hpp`` needed to access the classes from ``rclcpp`` namespace. +Next we add ``temperature.hpp`` and ``fluid_pressure.hpp`` message headers from ``sensor_msgs`` library. +These provide us with ``TemperatureMsg`` and ``FluidPressureMsg`` classes respectively. +These messages will serve as an example of a sensory data. +Also both of them do have a ``Header`` field, that is required by the ``Synchronizer`` filter. +The last includes are ``subscriber.hpp``, ``synchronizer.hpp`` and ``approximate_epsilon_time.hpp``. +The first will provide us with ``Subscriber`` and ``Synchronizer`` filters. +The last one will give us the access to the ``ApproximateEpsilonTime`` sync policy. + +Next we define a tutorial class. +In this case it is the ``EpsTimeSyncNode`` class. +For starters, let's take a look at the class members declared in the ``private`` section of this class: + +.. code-block:: C++ + + rclcpp::Publisher::SharedPtr temp_pub; + rclcpp::Publisher::SharedPtr fluid_pub; + + message_filters::Subscriber temp_sub; + message_filters::Subscriber fluid_sub; + + std::shared_ptr> sync; + + rclcpp::TimerBase::SharedPtr temperature_timer; + rclcpp::TimerBase::SharedPtr fluid_pressure_timer; + +To publish messages we will need two ``Publisher`` objects. +One will be used for publishing ``TemperatureMsg`` and the other for publishing ``FluidPressureMsg``. + +To get inputs for the ``Synchronizer`` filter we add two instances of the ``Subscriber`` filter. +Next is the main star of the show, the ``Synchronizer`` filter. + +And finally, to automate publishing messages and for querying the cache filter we add two timers, +the ``temperature_timer`` and the ``fluid_pressure_timer``. +These are going to invoke publishing of ``TemperatureMsg`` and ``FluidPressureMsg`` messages respectively. + +Next let's take a look at the public interface of this class. + +.. code-block:: C++ + + typedef sensor_msgs::msg::Temperature TemperatureMsg; + typedef sensor_msgs::msg::FluidPressure FluidPressureMsg; + typedef message_filters::sync_policies::ApproximateEpsilonTime< + sensor_msgs::msg::Temperature, + sensor_msgs::msg::FluidPressure + > SyncPolicy; + + EpsTimeSyncNode(): Node("epsilon_time_sync_node") { + rclcpp::QoS qos = rclcpp::QoS(10); + temp_pub = this->create_publisher("temp", qos); + fluid_pub = this->create_publisher("fluid", qos); + + temp_sub.subscribe(this, "temp", qos); + fluid_sub.subscribe(this, "fluid", qos); + + temperature_timer = this->create_wall_timer(500ms, std::bind(&EpsTimeSyncNode::TemperatureTimerCallback, this)); + fluid_pressure_timer = this->create_wall_timer(550ms, std::bind(&EpsTimeSyncNode::FluidPressureTimerCallback, this)); + + uint32_t queue_size = 10; + sync = std::make_shared> ( + SyncPolicy ( + queue_size, + rclcpp::Duration(std::chrono::seconds(1)) + ), + temp_sub, + fluid_sub + ); + + sync->registerCallback(std::bind(&EpsTimeSyncNode::SyncCallback, this, _1, _2)); + } + +Ultimately, the public interface of the ``EpsTimeSyncNode`` class +boils down to a few ``typedef`` aliases and a constructor. +We define aliases for the ``Temperature`` and the ``FluidPressure`` messages +and for the ``ApproximateEpsilonTime`` synchronization policy. + +In the constructor we declare ``QoS`` and message publishers. +Next we initialize subscription to the corresponding topics for ``temp_sub`` and ``fluid_sub`` message filters. +Note that all these subscriptions and publishers utilize the same ``QoS``. +After that the publisher timers are created. +And the last thing to do is to construct the ``Synchronizer`` filter. +The ``Synchronizer`` class constructor accepts a synchronization policy and a set of filters with incoming data. +The synchronization policy requires a ``queue_size`` and an instance of the ``rclcpp::Duration`` class +that will serve as an ``Epsilon`` of the ``ApproximateEpsilonTime``. +We'll touch on the meaning of this ``Epsilon`` a bit later. + +Now let's take a look at the timer and synchronization callbacks defined also in the ``private`` section of this class. + +.. code-block:: C++ + + void SyncCallback( + const TemperatureMsg::ConstSharedPtr & temp, + const FluidPressureMsg::ConstSharedPtr & fluid + ) { + RCLCPP_INFO( + this->get_logger(), + "Sync callback with %u.%u and %u.%u as times", + temp->header.stamp.sec, + temp->header.stamp.nanosec, + fluid->header.stamp.sec, + fluid->header.stamp.nanosec + ); + + if (temp->temperature > 2.0) + { + FluidPressureMsg 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 TemperatureTimerCallback() + { + TemperatureMsg 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 FluidPressureTimerCallback() + { + FluidPressureMsg 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); + } + +These are pretty straightforward. +The ``TemperatureTimerCallback`` is executed by the ``temperature_timer``. +The ``FluidPressureTimerCallback`` is executed by the ``fluid_pressure_timer``. +These two just construct corresponding messages and publish them to corresponding topics. +The ``SyncCallback`` is invoked by the ``Synchronizer`` filter in case if two messages meet the synchronization criterion. + +Finally, we create a main function and spin the node there. + +.. code-block:: C++ + + int main(int argc, char ** argv) + { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; + } + +2. ApproximateEpsilonTime synchronization policy +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Let's talk a bit about the ``ApproximateEpsilonTime`` synchronization policy. +As described `here ` +the ``ApproximateEpsilonTime`` policy requires messages to have the same timestamp within an ``Epsilon`` tolerance in order to match them. +For a synchronization callback to be invoked it is mandatory that messages should be received via all specified channels. +If all the channels have provided messages, but some of them do not fit the ``Epsilon`` +a synchronization callback is not invoked and no messages are passed to the following filters. +We'll demonstrate this further down the line in this tutorial. +Now let's finish setting up the project, build and run it. + +3. Update package.xml +~~~~~~~~~~~~~~~~~~~~~ + +Navigate to your package root and add the following dependencies in ``package.xml``: + +.. code-block:: xml + + message_filters + rclcpp + sensor_msgs + +4. Add the Node to a CMakeLists.txt +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Now open the ``CMakeLists.txt`` add the executable and name it ``approximate_time_sync_tutorial``, which you’ll use later with ``ros2 run``. + +.. code-block:: CMake + + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + add_executable(approximate_time_sync_tutorial src/approximate_time_sync_tutorial.cpp) + +Finally, add the ``install(TARGETS…)`` section so ``ros2 run`` can find your executable: + +.. code-block:: CMake + + install(TARGETS + approximate_time_sync_tutorial + DESTINATION lib/${PROJECT_NAME}) + +5. Build Your Package +~~~~~~~~~~~~~~~~~~~~~ + +From the root of your workspace: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +5. Run the Node +~~~~~~~~~~~~~~~ + +Now run the node using: + +.. code-block:: console + + ros2 run approximate_time_sync_tutorial approximate_time_sync_tutorial + +The console output should look something like this: + +.. code-block:: console + + [INFO] [1773177603.600396546] [epsilon_time_sync_node]: Sync callback with 1773177603.550153015 and 1773177603.600124849 as times + [INFO] [1773177604.151056035] [epsilon_time_sync_node]: Sync callback with 1773177604.50326582 and 1773177604.150421822 as times + [INFO] [1773177604.700414783] [epsilon_time_sync_node]: Sync callback with 1773177604.550350034 and 1773177604.700177922 as times + [INFO] [1773177605.250957771] [epsilon_time_sync_node]: Sync callback with 1773177605.50110559 and 1773177605.250282496 as times + [INFO] [1773177605.800939553] [epsilon_time_sync_node]: Sync callback with 1773177605.550309955 and 1773177605.800346632 as times + [INFO] [1773177606.350348802] [epsilon_time_sync_node]: Sync callback with 1773177606.50311161 and 1773177606.350160222 as times + [INFO] [1773177606.900890167] [epsilon_time_sync_node]: Sync callback with 1773177606.550157175 and 1773177606.900339428 as times + +Let's take a look at a timestamp difference between values from the first line. + +.. code-block:: console + 1773177603.600124849 sec - 1773177603.550153015 sec = 0.04997181892 sec + +The resulting ``0.04997181892`` seconds difference is definitely within the specified ``Epsilon``. +This the synchronization callback is executed. + +.. code-block:: C++ + + rclcpp::Duration(std::chrono::seconds(1)) + +Let's change the ``Epsilon`` value to ``5000000`` nanoseconds +Navigate to the ``EpsTimeSyncNode`` constructor and make the change. + +.. code-block:: C++ + + // replace + rclcpp::Duration(std::chrono::seconds(1)) + + // with + rclcpp::Duration(std::chrono::nanoseconds(5000000)) + +After this change, you'll need to recompile the project and run it again. +The output will look as follows: + +.. code-block:: console + + [INFO] [1773177900.106703711] [epsilon_time_sync_node]: Sync callback with 1773177900.106191759 and 1773177900.105995382 as times + [INFO] [1773177905.606790305] [epsilon_time_sync_node]: Sync callback with 1773177905.606144962 and 1773177905.605920351 as times + +There will be much less message pairs causing an execution of the synchronization callback. +And the time delta between them will be less then the specified ``epsilon``. +For example, for the timestamps from the first line output ``1773177900.106191759`` and ``1773177900.105995382`` +the delta is ``196377`` nanoseconds which is less then ``5000000`` nanoseconds. \ No newline at end of file diff --git a/doc/Tutorials/Approximate-Synchronizer-Cpp.rst b/doc/Tutorials/Approximate-Synchronizer-Cpp.rst new file mode 100644 index 00000000..9a49b2d6 --- /dev/null +++ b/doc/Tutorials/Approximate-Synchronizer-Cpp.rst @@ -0,0 +1,219 @@ +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 `_ and `create a package `_ + +1. Create a Basic Node with Includes +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: C++ + + #include "rclcpp/rclcpp.hpp" + + #include + #include + #include + + #include "message_filters/subscriber.h" + #include "message_filters/synchronizer.h" + #include "message_filters/sync_policies/approximate_time.h" + + #include + #include + + using namespace std::chrono_literals; + + using std::placeholders::_1; + using std::placeholders::_2; + + class TimeSyncNode : public rclcpp::Node + { + public: + + private: + rclcpp::Publisher::SharedPtr temp_pub; + rclcpp::Publisher::SharedPtr fluid_pub; + message_filters::Subscriber temp_sub; + message_filters::Subscriber fluid_sub; + std::shared_ptr>> 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 `_. +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::SharedPtr temp_pub; + rclcpp::Publisher::SharedPtr fluid_pub; + message_filters::Subscriber temp_sub; + message_filters::Subscriber 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("temp", qos); + fluid_pub = this->create_publisher("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::sync_policies::ApproximateTime(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 because 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 syncing 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(); + 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:: CMake + + 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:: CMake + + install(TARGETS + approximate_time_sync + DESTINATION lib/${PROJECT_NAME}) + +3. Build +~~~~~~~~ +From the root of your package, build and source. + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +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 new file mode 100644 index 00000000..35ba776e --- /dev/null +++ b/doc/Tutorials/Approximate-Synchronizer-Python.rst @@ -0,0 +1,156 @@ +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 `_ and `create a package `_ + +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 `_. +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:: xml + + message_filters + rclpy + sensor_msgs + +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 diff --git a/doc/Tutorials/Cache-Cpp.rst b/doc/Tutorials/Cache-Cpp.rst new file mode 100644 index 00000000..656f95ae --- /dev/null +++ b/doc/Tutorials/Cache-Cpp.rst @@ -0,0 +1,381 @@ +Cache (C++): +------------ + +Overview +~~~~~~~~ + +This tutorial demonstrates how to use the ``message_filters::Cache`` class in ROS 2 using C++. +The ``Cache`` filter stores a time history of messages and allows querying based on timestamps. + +We will use ``std_msgs.msg.String`` message for clarity and simplicity. + +Prerequisites +~~~~~~~~~~~~~ + +This tutorial assumes you have a working knowledge of ROS 2. + +If you have not done so already `create a workspace `_ and `create a package `_ + +1. Create a Basic Node with Includes +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Let's assume, you've already created an empty ROS 2 package for C++. +The next step is to create a new C++ file inside your package, e.g., ``cache_tutorial.cpp``, and create an example node: + +.. code-block:: C++ + + #include + #include + #include + #include + + #include + + #include "message_filters/subscriber.hpp" + #include "message_filters/cache.hpp" + + #include + + using namespace std::chrono_literals; + + const std::string TUTORIAL_TOPIC_NAME = "tutorial_topic"; + + class CacheNode : public rclcpp::Node { + public: + CacheNode() + : Node("cache_node") + { + auto qos = rclcpp::QoS(10); + publisher_ = this->create_publisher(TUTORIAL_TOPIC_NAME, qos); + subscriber_filter_.subscribe(this, TUTORIAL_TOPIC_NAME, qos); + + publisher_timer_ = this->create_wall_timer( + 1s, + std::bind(&CacheNode::publisher_timer_callback, this) + ); + + query_timer_ = this->create_wall_timer( + 1s, + std::bind(&CacheNode::query_timer_callback, this) + ); + } + + void publisher_timer_callback() { + auto message = std_msgs::msg::String(); + message.data = "example string"; + publisher_->publish(message); + } + + void query_timer_callback() { + rclcpp::Time latest_time = cache_filter_.getLatestTime(); + + if (latest_time == rclcpp::Time()) { + RCLCPP_INFO( + this->get_logger(), "Cache is empty" + ); + return; + } + + rclcpp::Time oldest_time = cache_filter_.getOldestTime(); + + RCLCPP_INFO( + this->get_logger(), + "oldest_time: %f, latest_time: %f", + latest_time.seconds(), + oldest_time.seconds() + ); + } + + private: + rclcpp::Publisher::SharedPtr publisher_; + + rclcpp::TimerBase::SharedPtr publisher_timer_; + rclcpp::TimerBase::SharedPtr query_timer_; + + message_filters::Subscriber subscriber_filter_; + message_filters::Cache cache_filter_{subscriber_filter_, 10, true}; + }; + + + int main(int argc, char ** argv) + { + rclcpp::init(argc, argv); + auto cache_node = std::make_shared(); + rclcpp::spin(cache_node); + rclcpp::shutdown(); + + return 0; + } + +1.1 Examine the code +~~~~~~~~~~~~~~~~~~~~ +Now, let's break down this code and examine the details. + +.. code-block:: C++ + + #include + #include + #include + #include + + #include + + #include "message_filters/subscriber.hpp" + #include "message_filters/cache.hpp" + + #include + + using namespace std::chrono_literals; + +We start by including ``chrono`` and ``functional`` headers. +The ``chrono`` header is required for the ``chrono_literals`` namespace, necessary for creating timers. +The ``functional`` header is also required to use ``std::bind`` function to bind timer callbacks to timers. +After that we include the ``rclcpp.hpp`` header that provides us with classes from ``rclcpp`` namespace. +To use filters in our code we need corresponding headers as well. +In this case we include ``subscriber.hpp`` and ``cache.hpp``. +And finally we add ``string.hpp`` to get access to ``String`` message class from the ROS standard messages library. + +Next we define a tutorial class. +In this case it is the ``CacheNode`` class. +For starters, let's take a look at the ``private`` section of this class: + +.. code-block:: C++ + + rclcpp::Publisher::SharedPtr publisher_; + + rclcpp::TimerBase::SharedPtr publisher_timer_; + rclcpp::TimerBase::SharedPtr query_timer_; + + message_filters::Subscriber subscriber_filter_; + message_filters::Cache cache_filter_{subscriber_filter_, 10, true}; + +To publish messages we will need a ``Publisher`` object. +To automate publishing messages and for querying the cache filter we add two timers, the ``publisher_timer_`` and the ``query_timer_`` respectively. +After all that we add two filters and chain them together. + +We start with a ``Subscriber`` filter, that is going to be an entry point for the messages into our chain of filters. +After that we create a ``Cache`` filter object, which is going to store and create a history of messages. + +Please note, that when the ``cache_filter_`` is created, the previous filter, the ``subscriber_filter_`` is passed as the first argument. +It is the way to chain these two filters together. +A message is going to pass from a topic, through ``subscriber_filter_`` into ``cache_filter_``, from which it will be processed. + +The second argument of the ``Cache`` constructor is ``cache size``, the maximum allotted messages to be stored in the cache. +The last argument is the ``allow_headerless`` flag, which is required to cache the ``String`` messages, that do not have a ``Header`` field. +In this case, the time the message was received is used, but only in case if the message class does not have a ``Header``. +If there is one, then the time from ``Header`` is used. +If the ``allow_headerless`` flag is set to ``false``, it is impossible to use messages without header. + +There is an option to directly put messages into ``Cache`` filter, by calling the ``add`` method of a ``cache`` filter. +There is, as well, an option to create an instance of the ``Cache`` filter on it's own, and pass messages to it via the ``add`` method. + +Next let's take a look at timer callbacks. + +.. code-block:: C++ + + void publisher_timer_callback() { + auto message = std_msgs::msg::String(); + message.data = "example string"; + publisher_->publish(message); + } + + void query_timer_callback() { + rclcpp::Time latest_time = cache_filter_.getLatestTime(); + + if (latest_time == rclcpp::Time()) { + RCLCPP_INFO( + this->get_logger(), "Cache is empty" + ); + return; + } + + rclcpp::Time oldest_time = cache_filter_.getOldestTime(); + + RCLCPP_INFO( + this->get_logger(), + "oldest_time: %f, latest_time: %f", + latest_time.seconds(), + oldest_time.seconds() + ); + } + +Now it is worthy to draw some attention to the following line of code. + +.. code-block:: C++ + + if (latest_time == rclcpp::Time()) + +Since we use the headerless ``String`` message in this tutorial, the time source for this message is the default ``RCL_SYSTEM_TIME``. +If we would use messages with headers, the expected time source for them would be the ``RCL_ROS_TIME``. + +Finally, let's take a look at the class constructor. + +.. code-block:: C++ + + CacheNode() + : Node("cache_node") + { + auto qos = rclcpp::QoS(10); + publisher_ = this->create_publisher(TUTORIAL_TOPIC_NAME, qos); + subscriber_filter_.subscribe(this, TUTORIAL_TOPIC_NAME, qos); + + publisher_timer_ = this->create_wall_timer( + 1s, + std::bind(&CacheNode::publisher_timer_callback, this) + ); + + query_timer_ = this->create_wall_timer( + 1s, + std::bind(&CacheNode::query_timer_callback, this) + ); + } + +Here we create a ``publisher_``, that is going to publish messages to some topic. + +.. code-block:: C++ + + publisher_ = this->create_publisher(TUTORIAL_TOPIC_NAME, qos); + +When the publisher is created we subscribe to a topic via ``subscriber_filter_``. + +.. code-block:: C++ + + subscriber_filter_.subscribe(this, TUTORIAL_TOPIC_NAME, qos); + +After that all what's left to be done is to create timers and we are good to go. + +.. code-block:: C++ + + publisher_timer_ = this->create_wall_timer( + 1s, + std::bind(&CacheNode::publisher_timer_callback, this) + ); + + query_timer_ = this->create_wall_timer( + 1s, + std::bind(&CacheNode::query_timer_callback, this) + ); + +The ``main`` function in this case is pretty straightforward. + +.. code-block:: C++ + + int main(int argc, char ** argv) + { + rclcpp::init(argc, argv); + auto cache_node = std::make_shared(); + rclcpp::spin(cache_node); + rclcpp::shutdown(); + + return 0; + } + +2. Update package.xml +~~~~~~~~~~~~~~~~~~~~~ + +Navigate to your package root and add the following dependencies in ``package.xml``: + +.. code-block:: xml + + message_filters + rclcpp + std_msgs + +3. Add the Node to a CMakeLists.txt +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Now open the ``CMakeLists.txt`` add the executable and name it ``cache_tutorial``, which you’ll use later with ``ros2 run``. + +.. code-block:: CMake + + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + ament_auto_add_executable(cache_tutorial src/cache_tutorial.cpp) + +Finally, add the ``install(TARGETS…)`` section so ros2 run can find your executable: + +.. code-block:: CMake + + install(TARGETS cache_tutorial + DESTINATION lib/${PROJECT_NAME}) + +4. Build Your Package +~~~~~~~~~~~~~~~~~~~~~ + +From the root of your workspace: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +5. Run the Node +~~~~~~~~~~~~~~~ + +Now run the node using: + +.. code-block:: console + + ros2 run cache_tutorial cache_tutorial + +The first message in the output is going to be + +.. code-block:: console + + [INFO] [1752701571.845039452] [cache_node]: Cache is empty + +As there were no messages published yet, and the cache is empty. +After that, the publisher will start populate the cache with messages: + +.. code-block:: console + + [INFO] [1752701572.845232157] [cache_node]: oldest_time: 1752701571.846233, latest_time: 1752701571.846233 + [INFO] [1752701573.845208906] [cache_node]: oldest_time: 1752701572.846269, latest_time: 1752701571.846233 + [INFO] [1752701574.844841757] [cache_node]: oldest_time: 1752701573.846131, latest_time: 1752701571.846233 + [INFO] [1752701575.844989998] [cache_node]: oldest_time: 1752701574.845164, latest_time: 1752701571.846233 + [INFO] [1752701576.845013484] [cache_node]: oldest_time: 1752701575.845885, latest_time: 1752701571.846233 + [INFO] [1752701577.844898272] [cache_node]: oldest_time: 1752701576.845787, latest_time: 1752701571.846233 + [INFO] [1752701578.844905995] [cache_node]: oldest_time: 1752701577.845648, latest_time: 1752701571.846233 + [INFO] [1752701579.844954514] [cache_node]: oldest_time: 1752701578.845697, latest_time: 1752701571.846233 + [INFO] [1752701580.844988219] [cache_node]: oldest_time: 1752701579.845718, latest_time: 1752701571.846233 + [INFO] [1752701581.844955759] [cache_node]: oldest_time: 1752701580.845818, latest_time: 1752701571.846233 + [INFO] [1752701582.845005794] [cache_node]: oldest_time: 1752701581.845692, latest_time: 1752701572.846269 <-- drop old msgs + [INFO] [1752701583.844966965] [cache_node]: oldest_time: 1752701582.845980, latest_time: 1752701573.846131 + [INFO] [1752701584.844954452] [cache_node]: oldest_time: 1752701583.845715, latest_time: 1752701574.845164 + +Note as the oldest time is starting to update after the 5'th message is added to the cache. +The cache size for the ``Cache`` in this example is 10. So as the 10'th message is added to +the cache, the oldest messages are being removed from it, thus updating oldest time. + +6. Other methods of the Cache filter interface +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The ``Cache`` filter stores the last N messages (in this case, 5), and allows querying: + +- Entire history: ``getInterval(const rclcpp::Time& start, const rclcpp::Time& end)`` +- Oldest rclcpp::Time: ``getOldestTime()`` +- Newest rclcpp::Time: ``getLatestTime()`` +- Messages after a certain time: ``getElemAfterTime(const rclcpp::Time& time)`` +- Messages before a certain time: ``getElemBeforeTime(const rclcpp::Time& time)`` +- A vector of messages that occur between a start and end time (inclusive) ``getInterval(const rclcpp::Time & start, const rclcpp::Time & end)`` +- The smallest interval of messages that surrounds an interval from start to end ``getSurroundingInterval(const rclcpp::Time & start, const rclcpp::Time & end)`` +- Set new cache size. The actual cache size will change when new message is added ``setCacheSize`` + +This is especially useful when you need to look back in time (e.g., align with previous sensor data). \ No newline at end of file diff --git a/doc/Tutorials/Cache-Python.rst b/doc/Tutorials/Cache-Python.rst new file mode 100644 index 00000000..846baa0c --- /dev/null +++ b/doc/Tutorials/Cache-Python.rst @@ -0,0 +1,317 @@ +Cache (Python): +--------------------------------------- + +Overview +~~~~~~~~ + +This tutorial demonstrates how to use the ``message_filters.Cache`` class in ROS 2 using Python. +The ``Cache`` filter stores a time history of messages and allows querying based on timestamps. + +We will use ``std_msgs.msg.String`` message for clarity and simplicity. + +Prerequisites +~~~~~~~~~~~~~ +This tutorial assumes you have a working knowledge of ROS 2 + +If you have not done so already `create a workspace `_ and `create a package `_ + +1. Create an Example Node with Includes +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Let's assume, you've already created an empty ros package for Python. +The next step is to create a new Python file inside your package, e.g., ``cache_tutorial.py``, and create an example node: + +.. code-block:: python + + import rclpy + from rclpy.node import Node + from rclpy.qos import QoSProfile + + from message_filters import Subscriber, Cache + from std_msgs.msg import String + + + class CacheNode(Node): + + def __init__(self): + super().__init__('cache_node') + + qos = QoSProfile(depth=10) + + self.publisher = self.create_publisher(String, 'input', qos_profile) + self.subscriber = Subscriber( + self, + String, + "/example/topic", + qos_profile=qos_profile, + ) + self.cache = Cache( + self.subscriber, + cache_size=5, + allow_headerless=True, # To allow caching basic String message + ) + + # Simulate publishing via timer + self.publisher_timer = self.create_timer( + timer_period_sec=1.0, + callback=self.publisher_timer_callback, + ) + + # Check on cached data + self.query_timer = self.create_timer( + timer_period_sec=1.0, + callback=self.query_timer_callback, + ) + + self.counter = 0 + + def publisher_timer_callback(self): + self.publisher.publish( + String( + data=f"Message {self.counter}" + ) + ) + self.counter += 1 + + def query_timer_callback(self): + latest_time = self.cache.getLatestTime() + if latest_time is None: + self.get_logger().info("Cache is empty.") + return + + oldest_time = self.cache.getOldestTime() + + self.get_logger().info(f"oldest_time: {oldest_time.seconds_nanoseconds()[0]}," + f"latest_time: {latest_time.seconds_nanoseconds()[0]}") + + cached_messages = self.cache.getInterval(oldest_time, latest_time) + + for msg in cached_messages: + self.get_logger().info(f"Cached: {msg.data}") + + + def main(args=None): + rclpy.init(args=args) + + cache_node = CacheNode() + rclpy.spin(cache_node) + + cache_node.destroy_node() + rclpy.shutdown() + + + if __name__ == '__main__': + main() + + +1.1 Examine the code +~~~~~~~~~~~~~~~~~~~~ +Now, let's break down this code and examine the details. + +.. code-block:: python + + import rclpy + from rclpy.node import Node + from rclpy.qos import QoSProfile + + from message_filters import Subscriber, Cache + from std_msgs.msg import String + +We start with importing ``rclpy``, ``Node`` and ``QoSProfile`` classes +that are required for constructing node and it's subscriptions and publishers, +and for running the node itself. After that we import message filters: +``Subscriber`` and ``Cache``. And in the end we import the ``String`` +message class that we are going to use for this example. + +.. code-block:: python + + class CacheNode(Node): + + def __init__(self): + super().__init__('cache_node') + + qos = QoSProfile(depth=10) + + self.publisher = self.create_publisher(String, 'input', qos_profile) + +After declaring imports, we create a class for this example, declare a Quality of Service profile +that we are going to use for all our interfaces in this example, and create a publisher +that is going to populate the example topic with messages to cache. + +.. code-block:: python + + self.subscriber = Subscriber( + self, + String, + "/example/topic", + qos_profile=qos_profile, + ) + self.cache = Cache( + self.subscriber, + cache_size=5, + allow_headerless=True, # To allow caching basic String message + ) + +The next step is to create filters and to chain them together. +We start with a Subscriber filter, that is going to be and entry point for +the messages into our chain of filters. And after that we create a +cache filter object, that is going to cache the messages, passing down +the filters chain. Please note, that when the ``cache`` is created, +the previous filter, the ``subscriber`` is passed as the first argument. +It is the way to chain these two filters together. Message is going to pass through +``subscriber`` into ``cache``, and in some other filter if it is added +down the chain. + +It may be useful to point out that the ``Subscriber`` filter is not the only +way to start a chain of filters. One may consider using ``SimpleFilter``. +It does not create a new subscription on it's own and may be used directly +in a subscription callback instead. + +In this case, we set the argument ``allow_headerless`` value to ``true``, to allow caching +``std_msgs/String`` message, as it does not have a ``Header``. In case we've decided +to set this value to ``False``, the filter would log a corresponding error message, +when trying to store message in cache. + +What is left to be done is to set timers + +.. code-block:: python + + # Simulate publishing via timer + self.publisher_timer = self.create_timer( + timer_period_sec=1.0, + callback=self.publisher_timer_callback, + ) + + # Check on cached data + self.query_timer = self.create_timer( + timer_period_sec=1.0, + callback=self.query_timer_callback, + ) + +And define the timer callbacks + +.. code-block:: python + + def publisher_timer_callback(self): + self.publisher.publish( + String( + data=f"Message {self.counter}" + ) + ) + self.counter += 1 + + def query_timer_callback(self): + latest_time = self.cache.getLatestTime() + if latest_time is None: + self.get_logger().info("Cache is empty.") + return + + oldest_time = self.cache.getOldestTime() + + self.get_logger().info(f"oldest_time: {oldest_time.seconds_nanoseconds()[0]}," + f"latest_time: {latest_time.seconds_nanoseconds()[0]}") + + cached_messages = self.cache.getInterval(oldest_time, latest_time) + + for msg in cached_messages: + self.get_logger().info(f"Cached: {msg.data}") + + + +2. Update package.xml +~~~~~~~~~~~~~~~~~~~~~ + +Navigate to your package root and add the following dependencies in ``package.xml``: + +.. code-block:: xml + + message_filters + rclpy + std_msgs + +3. Add Entry Point in setup.py +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Update the ``console_scripts`` section of your ``setup.py``: + +.. code-block:: python + + entry_points={ + 'console_scripts': [ + 'cache_tutorial = pkg_name.cache_tutorial:main', + ], + }, + +Replace ``pkg_name`` with your actual package name. + +4. Build Your Package +~~~~~~~~~~~~~~~~~~~~~ + +From the root of your workspace: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +5. Run the Node +~~~~~~~~~~~~~~~ + +Now run the node using: + +.. code-block:: bash + + ros2 run pkg_name cache_tutorial + +The first message in the output is going to be + +.. code-block:: bash + + [INFO] [1750884527.235426721] [cache_node]: Cache filters cache is empty + +As there were no messages published yet, and the cache is empty. +After that, the publisher will start populate the cache with messages: + +.. code-block:: bash + + [INFO] [1750887122.590581767] [cache_node]: oldest_time: 1750887121, latest_time: 1750887121 + [INFO] [1750887123.593117081] [cache_node]: oldest_time: 1750887121, latest_time: 1750887122 + [INFO] [1750887124.593130934] [cache_node]: oldest_time: 1750887121, latest_time: 1750887123 + [INFO] [1750887125.592839265] [cache_node]: oldest_time: 1750887121, latest_time: 1750887124 + [INFO] [1750887126.592716962] [cache_node]: oldest_time: 1750887121, latest_time: 1750887125 + [INFO] [1750887127.592824186] [cache_node]: oldest_time: 1750887122, latest_time: 1750887126 <-- drop old msgs + [INFO] [1750887128.590810767] [cache_node]: oldest_time: 1750887123, latest_time: 1750887127 + + +Note as the oldest time is starting to update after the 5'th message is added to the cache. +The cache size for the ``cache`` in this example is 5. So as the 5'th message is added to +the cache, the oldest messages are being removed from it, thus updating oldest time. + +6. Other methods of the Cache filter interface +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The ``Cache`` filter stores the last N messages (in this case, 5), and allows querying: + +- Entire history: ``getInterval(start_time, end_time)`` +- Most recent message: ``getLast()`` +- Oldest timestamp: ``getOldestTime()`` +- Newest timestamp: ``getLatestTime()`` +- Messages after a certain time: ``getElemAfterTime(time)`` +- Messages before a certain time: ``getElemBeforeTime(time)`` + +This is especially useful when you need to look back in time (e.g., align with previous sensor data). diff --git a/doc/Tutorials/Chain-Cpp.rst b/doc/Tutorials/Chain-Cpp.rst new file mode 100644 index 00000000..3ef7d891 --- /dev/null +++ b/doc/Tutorials/Chain-Cpp.rst @@ -0,0 +1,544 @@ +Chain (C++): +------------ + +Overview +~~~~~~~~ + +This tutorial demonstrates how to use the ``message_filters::Chain`` class in ROS 2 using C++. +The ``Chain`` filter provides a container for simple filters. +It allows you to store an N-long set of filters inside a single structure, making it much easier to manage them. + +To demonstrate the functionality of the ``Chain`` filter in a more clear manner, we are going to add a custom filter to this tutorial. +This is going to be the ``CounterFilter`` that will be counting the number of messages passing through it. +This filter class will be a successor to the ``SimpleFilter`` class, but this is a topic for another tutorial. + +For more information on this succession mechanism, please refer to the `SimpleFilter for C++ tutorial `_. + +Prerequisites +~~~~~~~~~~~~~ + +This tutorial assumes you have a working knowledge of ROS 2. + +If you have not done so already `create a workspace `_ and `create a package `_ + +1. Create a Basic Node with Includes +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Let's assume, you've already created an empty ROS 2 package for C++. +The next step is to create a new C++ file inside your package, e.g., ``chain_tutorial.cpp``, and write an example code: + +.. code-block:: C++ + + #include + #include + #include + #include + #include + + #include + + #include + #include + #include + #include + + #include + + using namespace std::chrono_literals; + + const std::string TUTORIAL_TOPIC_NAME = "tutorial_topic"; + + class CounterFilter : public message_filters::SimpleFilter { + public: + CounterFilter() : message_filters::SimpleFilter() {} + + void add(const message_filters::MessageEvent& e) { + ++counter_; + signalMessage(e); + } + + template + void connectInput(F& f) { + incoming_connection_ = f.registerCallback( + std::bind( + &CounterFilter::callback, + this, + std::placeholders::_1 + ) + ); + } + + size_t getCounterValue() { + return counter_; + } + private: + size_t counter_ = 0; + + message_filters::Connection incoming_connection_; + + void callback(const std_msgs::msg::String::ConstSharedPtr& evt) { + add(evt); + } + }; + + class ChainNode : public rclcpp::Node { + public: + ChainNode() : + Node("ChainNode"), + subscriber_filter_(), + first_counter_(std::make_shared()), + second_counter_(std::make_shared()), + chain_filter_(subscriber_filter_) + { + auto qos = rclcpp::QoS(10); + + subscriber_filter_.subscribe(this, TUTORIAL_TOPIC_NAME, qos); + + // Set up the chain of filters + chain_filter_.addFilter(first_counter_); + chain_filter_.addFilter(second_counter_); + + chain_filter_.registerCallback( + std::bind( + &ChainNode::chain_exit_callback, + this, + std::placeholders::_1 + ) + ); + + publisher_ = create_publisher(TUTORIAL_TOPIC_NAME, qos); + + publisher_timer_ = this->create_wall_timer( + 1s, + std::bind(&ChainNode::publisher_timer_callback, this) + ); + + query_timer_ = this->create_wall_timer( + 1s, + std::bind(&ChainNode::query_timer_callback, this) + ); + } + + void chain_exit_callback(const std_msgs::msg::String::ConstSharedPtr& _) { + RCLCPP_INFO( + get_logger(), + "%zu messages have reached the end of this chain", + ++chain_counter_ + ); + } + + void publisher_timer_callback() { + auto message = std_msgs::msg::String(); + message.data = "example string"; + publisher_->publish(message); + } + + void query_timer_callback() { + RCLCPP_INFO( + get_logger(), + "First counter messages count: %zu, Second counter messages count: %zu", + chain_filter_.getFilter(0)->getCounterValue(), + chain_filter_.getFilter(1)->getCounterValue() + ); + } + private: + message_filters::Subscriber subscriber_filter_; + + std::shared_ptr first_counter_; + std::shared_ptr second_counter_; + + size_t chain_counter_ = 0; + + message_filters::Chain chain_filter_; + + rclcpp::Publisher::SharedPtr publisher_; + + rclcpp::TimerBase::SharedPtr publisher_timer_; + rclcpp::TimerBase::SharedPtr query_timer_; + }; + + int main([[maybe_unused]] int argc, [[maybe_unused]] char ** argv) + { + rclcpp::init(argc, argv); + + auto chain_node = std::make_shared(); + + rclcpp::spin(chain_node); + rclcpp::shutdown(); + + return 0; + } + +1.1 Examine the code +~~~~~~~~~~~~~~~~~~~~ + +Now, let's break down this code and examine the details. + +.. code-block:: C++ + + #include + #include + #include + #include + #include + + #include + + #include + #include + #include + #include + + #include + + using namespace std::chrono_literals; + +We start by including ``chrono`` and ``functional`` headers. +The ``chrono`` header is required for the ``chrono_literals`` namespace, necessary for creating timers. +The ``cstddef`` header provides us with some basic types such as ``size_t``. +The ``memory`` header is required as we are going to utilize ``shared_ptr`` class. +The ``functional`` header is also required to use ``std::bind`` function to bind timer callbacks to timers. +After that we include the ``rclcpp.hpp`` header that provides us with classes from ``rclcpp`` namespace. +For the ``CounterFilter`` we need a ``message_filters::MessageEvent`` class from the ``message_event.hpp``, so it is included. +To use filters in our code we need corresponding headers as well. +In this case we include ``subscriber.hpp``, ``chain.hpp`` and ``simple_filter.hpp``. +And finally we add ``string.hpp`` to get access to ``String`` message class from the ROS standard messages library. + +Next we define a ``CounterFilter`` class. + +.. code-block:: C++ + + class CounterFilter : public message_filters::SimpleFilter { + public: + CounterFilter() : message_filters::SimpleFilter() {} + + void add(const message_filters::MessageEvent& e) { + ++counter_; + signalMessage(e); + } + + template + void connectInput(F& f) { + incoming_connection_ = f.registerCallback( + std::bind( + &CounterFilter::callback, + this, + std::placeholders::_1 + ) + ); + } + + size_t getCounterValue() { + return counter_; + } + private: + size_t counter_ = 0; + + message_filters::Connection incoming_connection_; + + void callback(const std_msgs::msg::String::ConstSharedPtr& evt) { + add(evt); + } + }; + +This filter counts the number of messages passing through it. +The ``add`` method increases messages count, and passes message to the following filter via ``signalMessage``. +The ``connectInput`` connects this filter to a previous filter's output. +The ``getCounterValue`` grants access to the current messages count. + +More on this succession mechanism may be found in the corresponding tutorial: `SimpleFilter for C++ tutorial `_. + +And now we can turn our attention to the main tutorial class, that is the ``ChainNode`` class. +For starters, let's take a look at the ``private`` section of this class: + +.. code-block:: C++ + + message_filters::Subscriber subscriber_filter_; + + std::shared_ptr first_counter_; + std::shared_ptr second_counter_; + + message_filters::Chain chain_filter_; + + size_t chain_counter_ = 0; + + rclcpp::Publisher::SharedPtr publisher_; + + rclcpp::TimerBase::SharedPtr publisher_timer_; + rclcpp::TimerBase::SharedPtr query_timer_; + +We start with a ``Subscriber`` filter, that is going to be an entry point for the messages into our chain of filters. +After that we add two ``CounterFilter`` objects. +Next up is the ``Chain`` filter itself. It is going to connect all the filters. + +When a message passes through all the filters in the chain, it is passed to a ``Chain`` filter's callback. +This callback is where ``chain_counter_`` is updated. +The ``chain_counter_`` field, as it says, keeps the count of messages, that have reached the end of the chain. + +To publish messages we will need a ``Publisher`` object. +To automate publishing messages and for querying the chain filter we add two timers, the ``publisher_timer_`` and the ``query_timer_`` respectively. + +Now let's take a look at the ``ChainNode`` constructor + +.. code-block:: C++ + + ChainNode() : + Node("ChainNode"), + subscriber_filter_(), + first_counter_(std::make_shared()), + second_counter_(std::make_shared()), + chain_filter_(subscriber_filter_) + { + auto qos = rclcpp::QoS(10); + + subscriber_filter_.subscribe(this, TUTORIAL_TOPIC_NAME, qos); + + // Set up the chain of filters + chain_filter_.addFilter(first_counter_); + chain_filter_.addFilter(second_counter_); + + chain_filter_.registerCallback( + std::bind( + &ChainNode::chain_exit_callback, + this, + std::placeholders::_1 + ) + ); + + publisher_ = create_publisher(TUTORIAL_TOPIC_NAME, qos); + + publisher_timer_ = this->create_wall_timer( + 1s, + std::bind(&ChainNode::publisher_timer_callback, this) + ); + + query_timer_ = this->create_wall_timer( + 1s, + std::bind(&ChainNode::query_timer_callback, this) + ); + } + +We start with constructing the basic ``Node`` class and all the filters required in this example. + +.. code-block:: C++ + + Node("ChainNode"), + subscriber_filter_(), + first_counter_(std::make_shared()), + second_counter_(std::make_shared()), + chain_filter_(subscriber_filter_) + +After that we subscrie to the example topic via ``subscriber_filter_``. +Following it we create ``first_counter_`` and ``second_counter_`` ``CounterFilte`` instances. +Finally we create an instance of the ``Chain`` filter, that is stored in the ``chain_filter_`` field. +Please note that the ``subscriber_filter_`` is passed as a constructor argument. +This helps to set this filter as a first in a chain. +The other wat to do so is to call ``connectInput`` method after the ``chain_filter_`` is constructed. +Like so + +.. code-block:: C++ + + message_filters::Subscriber subscriber_filter_(); + message_filters::Chain chain_filter_(); + chain_filter_.connectInput(subscriber_filter_); + +The body of the constructor starts with initializing the ``subscriber_filter_``'s subscription. + +.. code-block:: C++ + + auto qos = rclcpp::QoS(10); + + subscriber_filter_.subscribe(this, TUTORIAL_TOPIC_NAME, qos); + +The next step is to chain together two instances of a ``CounterFilter``. +We add both of this filters to the ``Chain`` filter via ``addFilter`` method call. + +.. code-block:: C++ + + // Set up the chain of filters + chain_filter_.addFilter(first_counter_); + chain_filter_.addFilter(second_counter_); + +The last thing to do with the ``Chain`` filter is to add a callback. +This callback is called every time, when a message passes through all other filters in the chain. + +.. code-block:: C++ + + chain_filter_.registerCallback( + std::bind( + &ChainNode::chain_exit_callback, + this, + std::placeholders::_1 + ) + ); + +Now, when all filters are set up, we need to publish some messages to the example topic. +For this purpose we set up a publisher and a publish timer, and a query timer to see the results. + +.. code-block:: C++ + + publisher_ = create_publisher(TUTORIAL_TOPIC_NAME, qos); + + publisher_timer_ = this->create_wall_timer( + 1s, + std::bind(&ChainNode::publisher_timer_callback, this) + ); + + query_timer_ = this->create_wall_timer( + 1s, + std::bind(&ChainNode::query_timer_callback, this) + ); + +Please notice the ``query_timer_callback``. +It demonstrates one of the ``Chain`` filter main methods. +The ``getFilter`` template method provides an access to any filter in the chain, given it's position. +The return value of this method is a shared pointer to the required filter. + +.. code-block:: C++ + + void query_timer_callback() { + RCLCPP_INFO( + get_logger(), + "First counter messages count: %zu, Second counter messages count: %zu", + chain_filter_.getFilter(0)->getCounterValue(), + chain_filter_.getFilter(1)->getCounterValue() + ); + + +The ``main`` function as usual in this tutorials is pretty straightforward. + +.. code-block:: C++ + + int main([[maybe_unused]] int argc, [[maybe_unused]] char ** argv) + { + rclcpp::init(argc, argv); + + auto chain_node = std::make_shared(); + + rclcpp::spin(chain_node); + rclcpp::shutdown(); + + return 0; + } + +2. Update package.xml +~~~~~~~~~~~~~~~~~~~~~ + +Navigate to your package root and add the following dependencies in ``package.xml``: + +.. code-block:: xml + + message_filters + rclcpp + std_msgs + +3. Add the Node to a CMakeLists.txt +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Now open the ``CMakeLists.txt`` add the executable and name it ``chain_tutorial``, which you’ll use later with ``ros2 run``. + +.. code-block:: CMake + + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + ament_auto_add_executable(chain_tutorial src/chain_tutorial.cpp) + +Finally, add the ``install(TARGETS…)`` section so ros2 run can find your executable: + +.. code-block:: CMake + + install(TARGETS chain_tutorial + DESTINATION lib/${PROJECT_NAME}) + +4. Build Your Package +~~~~~~~~~~~~~~~~~~~~~ + +From the root of your workspace: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +5. Run the Node +~~~~~~~~~~~~~~~ + +Now run the node using: + +.. code-block:: console + + ros2 run chain_tutorial chain_tutorial + +The output is going to look something like this + +.. code-block:: console + + [INFO] [1758299415.877982943] [ChainNode]: First counter messages count: 0, Second counter messages count: 0 + [INFO] [1758299415.879089260] [ChainNode]: 1 messages have reached the end of this chain + [INFO] [1758299416.877867549] [ChainNode]: First counter messages count: 1, Second counter messages count: 1 + [INFO] [1758299416.878531764] [ChainNode]: 2 messages have reached the end of this chain + [INFO] [1758299417.877748421] [ChainNode]: First counter messages count: 2, Second counter messages count: 2 + [INFO] [1758299417.877956617] [ChainNode]: 3 messages have reached the end of this chain + [INFO] [1758299418.877758202] [ChainNode]: First counter messages count: 3, Second counter messages count: 3 + [INFO] [1758299418.878093589] [ChainNode]: 4 messages have reached the end of this chain + [INFO] [1758299419.877953871] [ChainNode]: First counter messages count: 4, Second counter messages count: 4 + [INFO] [1758299419.878610243] [ChainNode]: 5 messages have reached the end of this chain + [INFO] [1758299420.878002318] [ChainNode]: First counter messages count: 5, Second counter messages count: 5 + [INFO] [1758299420.878685845] [ChainNode]: 6 messages have reached the end of this chain + [INFO] [1758299421.877966429] [ChainNode]: First counter messages count: 6, Second counter messages count: 6 + [INFO] [1758299421.878590495] [ChainNode]: 7 messages have reached the end of this chain + [INFO] [1758299422.877695774] [ChainNode]: First counter messages count: 7, Second counter messages count: 7 + [INFO] [1758299422.878073509] [ChainNode]: 8 messages have reached the end of this chain + [INFO] [1758299423.877839197] [ChainNode]: First counter messages count: 8, Second counter messages count: 8 + [INFO] [1758299423.878292931] [ChainNode]: 9 messages have reached the end of this chain + +Note that when the first query to the both counter filters executed, they both report that there was no messages, passing through before. + +.. code-block:: console + + [INFO] [1758299415.877982943] [ChainNode]: First counter messages count: 0, Second counter messages count: 0 + +After that, as we can see from the output of the ``chain_exit_callback``, the first message passes through the chain. +The next time we request the number of messages, every counter filter reports that one message has passed through. + +.. code-block:: console + + [INFO] [1758299415.879089260] [ChainNode]: 1 messages have reached the end of this chain + [INFO] [1758299416.877867549] [ChainNode]: First counter messages count: 1, Second counter messages count: 1 + +From this point on, all three counters increase their values as more messages are passed down the filter chain. + +6. Other methods of the Chain filter interface +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +In this example we've passed the ``subscriber_filter_`` object to the ``chain_filter_`` as a constructor argument. +In this case, the ``subscriber_filter_`` was used as an input filter for the ``chain_filter_``. + +.. code-block:: C++ + + chain_filter_(subscriber_filter_) + +The other way to do it is by calling ``connectInput`` method of the ``Chain`` class. + +.. code-block:: C++ + + message_filters::Subscriber subscriber_filter_(); + message_filters::Chain chain_filter_(); + chain_filter_.connectInput(subscriber_filter_); \ No newline at end of file diff --git a/doc/Tutorials/Chain-Python.rst b/doc/Tutorials/Chain-Python.rst new file mode 100644 index 00000000..46d371f4 --- /dev/null +++ b/doc/Tutorials/Chain-Python.rst @@ -0,0 +1,384 @@ +Chain (Python): +--------------- + +Overview +~~~~~~~~ + +This tutorial demonstrates how to use the ``message_filters.Chain`` class in ROS 2 using Python. +The ``Chain`` filter provides a container for simple filters. +It allows you to store an N-long set of filters inside a single structure, making it much easier to manage them. + +To demonstrate the functionality of the ``Chain`` filter in a more clear manner, we are going to add a custom filter to this tutorial. +This is going to be the ``CounterFilter`` that will be counting the number of messages passing through it. +This filter class will be a successor to the ``SimpleFilter`` class, but this is a topic for another tutorial. +For more information on this topic, please refer to the `SimpleFilter for Python tutorial `. + +Prerequisites +~~~~~~~~~~~~~ + +This tutorial assumes you have a working knowledge of ROS 2. + +If you have not done so already `create a workspace `_ and `create a package `_ + +1. Create a Basic Node +~~~~~~~~~~~~~~~~~~~~~~ + +Let's assume, you've already created an empty ROS 2 package for Python. +The next step is to create a new Python file inside your package, e.g., ``chain_tutorial.py``, and write an example code: + +.. code-block:: python + + import rclpy + from rclpy.node import Node + from rclpy.qos import QoSProfile + + from message_filters import Subscriber, Chain, SimpleFilter + from std_msgs.msg import String + + + TUTORIAL_TOPIC = "/example/topic" + + + class CounterFilter(SimpleFilter): + def __init__(self): + SimpleFilter.__init__(self) + + self.incoming_connection = None + self._counter: int = 0 + + def connectInput(self, message_filter): + if self.incoming_connection is not None: + raise RuntimeError('Already connected') + self.incoming_connection = message_filter.registerCallback(self.add) + + def add(self, message): + self._counter += 1 + self.signalMessage(message) + + @property + def counter(self): + return self._counter + + + class ChainNode(Node): + + def __init__(self): + super().__init__('chain_node') + + qos_profile = QoSProfile(depth=10) + + self.publisher = self.create_publisher(String, TUTORIAL_TOPIC, qos_profile) + self.subscriber = Subscriber( + self, + String, + TUTORIAL_TOPIC, + qos_profile=qos_profile, + ) + self.first_counter = CounterFilter() + self.second_counter = CounterFilter() + self.chain_counter = 0 + + self.chain_filter = Chain(self.subscriber) + self.chain_filter.addFilter(self.first_counter) + self.chain_filter.addFilter(self.second_counter) + self.chain_filter.registerCallback(self.chain_callback) + + self.publisher_timer = self.create_timer( + timer_period_sec=1.0, + callback=self.publisher_timer_callback, + ) + + self.query_timer = self.create_timer( + timer_period_sec=1.0, + callback=self.querty_timer_callback, + ) + + def chain_callback(self, message): + self.chain_counter += 1 + + def publisher_timer_callback(self): + self.publisher.publish(String(data='example message')) + + def querty_timer_callback(self): + first_filter_count = self.chain_filter.getFilter(0).counter + second_filter_count = self.chain_filter.getFilter(1).counter + + print(f"first counter messages count: {first_filter_count}, second counter messages count: {second_filter_count}") + print(f"messages reached the end of chain: {self.chain_counter}\n") + + + def main(): + rclpy.init() + + chain_node = ChainNode() + rclpy.spin(chain_node) + + chain_node.destroy_node() + rclpy.shutdown() + + + if __name__ == '__main__': + main() + +1.1 Examine the code +~~~~~~~~~~~~~~~~~~~~ + +Now, let's break down this code and examine the details. + +.. code-block:: python + + import rclpy + from rclpy.node import Node + from rclpy.qos import QoSProfile + + from message_filters import Subscriber, Chain, SimpleFilter + from std_msgs.msg import String + + + TUTORIAL_TOPIC = "/example/topic" + +We start with importing ``rclpy``, ``Node`` and ``QoSProfile`` classes +that are required for constructing node and it's subscriptions and publishers, +and for running the node itself. After that we import message filters: +``Subscriber``, ``Chain`` and ``SimpleFilter``. +And in the end we import the ``String`` message class that we are going to use for this example. +In the end of this section we define a ``TUTORIAL_TOPIC`` constant. +It will be convenient when defining publishers and subscribers later in this tutorial. +Next we define a ``CounterFilter`` class. + +.. code-block:: python + + class CounterFilter(SimpleFilter): + def __init__(self): + SimpleFilter.__init__(self) + + self.incoming_connection = None + self._counter: int = 0 + + def connectInput(self, message_filter): + if self.incoming_connection is not None: + raise RuntimeError('Already connected') + self.incoming_connection = message_filter.registerCallback(self.add) + + def add(self, message): + self._counter += 1 + self.signalMessage(message) + + @property + def counter(self): + return self._counter + +This filter counts the number of messages passing through it. +The ``add`` method increases messages count, and passes messages to the following filter via ``signalMessage`` call. +The ``connectInput`` connects this filter to a previous filter's output. +The ``counter`` property grants access to the current messages count. + +For more information on this succession mechanism, please refer to the `SimpleFilter for Python tutorial `. + +And now we can turn our attention to the main tutorial class, that is the ``ChainNode`` class. +For starters, let's take a look at the ``__init__`` method of this class: + +.. code-block:: python + + class ChainNode(Node): + + def __init__(self): + super().__init__('chain_node') + + qos_profile = QoSProfile(depth=10) + + self.publisher = self.create_publisher(String, TUTORIAL_TOPIC, qos_profile) + self.subscriber = Subscriber( + self, + String, + TUTORIAL_TOPIC, + qos_profile=qos_profile, + ) + self.first_counter = CounterFilter() + self.second_counter = CounterFilter() + self.chain_counter = 0 + +First we declare a Quality of Service profile. +After that we initialize a basic ``ros2`` ``publisher`` that will generate the input for our filters chain. +The chain is giong to contain three filters. +A ``Subscriber`` filter and two instances of a ``CounterFilter`` that is defined earlier. +We initialize all of those. +In the end of this section we create the ``chain_counter`` field that is going to count all the messages that have passed through all the filters in the ``Chain``. +When a message passes through all the filters in the chain, it is passed to a ``Chain`` filter's callback. +This callback is where ``chain_counter`` is updated. + +.. code-block:: python + + self.chain_filter = Chain(self.subscriber) + self.chain_filter.addFilter(self.first_counter) + self.chain_filter.addFilter(self.second_counter) + self.chain_filter.registerCallback(self.chain_callback) + +The next step is to build the ``Chain`` message filter and populate it with other filters. +We initialize the ``Chain`` filter itself and pass the ``Subscriber`` filter as an argument to the constructor function to create an entry point for messages into the chain. +At this moment the ``Chain`` filter holds only one filter - the ``Subscriber`` filter. +We add two instances of ``CounterFilter`` to the chain. wia ``addFilter`` method call. +Filnally we register the ``chain_callback`` method of our ``ChainNode`` class as a final callback for the ``Chain`` filter object. + +Now, when all filters are set up, we need to publish some messages to the example topic. +For this purpose we set up a publisher and a publish timer, and a query timer to see the results. + +.. code-block:: python + + self.publisher_timer = self.create_timer( + timer_period_sec=1.0, + callback=self.publisher_timer_callback, + ) + + self.query_timer = self.create_timer( + timer_period_sec=1.0, + callback=self.querty_timer_callback, + ) + +And define the timer callbacks and the ``chain_callback``. + +.. code-block:: python + + def chain_callback(self, message): + self.chain_counter += 1 + + def publisher_timer_callback(self): + self.publisher.publish(String(data='example message')) + + def querty_timer_callback(self): + first_filter_count = self.chain_filter.getFilter(0).counter + second_filter_count = self.chain_filter.getFilter(1).counter + + print(f"first counter messages count: {first_filter_count}, second counter messages count: {second_filter_count}") + print(f"messages reached the end of chain: {self.chain_counter}\n") + +Please notice the ``query_timer_callback``. +It demonstrates one of the ``Chain`` filter main methods. +The ``getFilter`` method provides an access to any filter in the chain, given it's position. +The return value of this method is a shared pointer to the required filter. + +The ``main`` function as usual in this tutorials is pretty straightforward. + +.. code-block:: python + + def main(): + rclpy.init() + + chain_node = ChainNode() + rclpy.spin(chain_node) + + chain_node.destroy_node() + rclpy.shutdown() + + + if __name__ == '__main__': + main() + +2. Update package.xml +~~~~~~~~~~~~~~~~~~~~~ + +Navigate to your package root and add the following dependencies in ``package.xml``: + +.. code-block:: xml + + message_filters + rclpy + std_msgs + +3. Add Entry Point in setup.py +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Update the ``console_scripts`` section of your ``setup.py``: + +.. code-block:: python + + entry_points={ + 'console_scripts': [ + 'chain_tutorial = pkg_name.chain_tutorial:main', + ], + }, + +Replace ``pkg_name`` with your actual package name. + +4. Build Your Package +~~~~~~~~~~~~~~~~~~~~~ + +From the root of your workspace: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +5. Run the Node +~~~~~~~~~~~~~~~ + +Now run the node using: + +.. code-block:: console + + ros2 run pkg_name chain_tutorial + +The output of the node is going to look something like this + +.. code-block:: console + + first counter messages count: 0, second counter messages count: 0 + messages reached the end of chain: 0 + + first counter messages count: 1, second counter messages count: 1 + messages reached the end of chain: 1 + + first counter messages count: 2, second counter messages count: 2 + messages reached the end of chain: 2 + + first counter messages count: 3, second counter messages count: 3 + messages reached the end of chain: 3 + + first counter messages count: 4, second counter messages count: 4 + messages reached the end of chain: 4 + +Note that when the first query to the both counter filters executed, they both report that there was no messages, passing through before. +And there were no messages that have reached the end of the chain. +After that the first message passes through the all filters in chain as indicated by increased counter values + +.. code-block:: console + + first counter messages count: 1, second counter messages count: 1 + messages reached the end of chain: 1 + +From this point on, all three counters increase their values as more messages are passed down the filter chain. + +6. Other methods of the Chain filter interface +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +In this example we've passed the ``subscriber_filter`` object to the ``chain_filter`` as a constructor argument. +In this case, the ``subscriber_filter`` was used as an input filter for the ``chain_filter``. + +.. code-block:: python + + self.chain_filter = Chain(self.subscriber) + +The other way to do it is by calling ``connectInput`` method of the ``Chain`` class. + +.. code-block:: python + + self.chain_filter = Chain() + self.subscriber = Subscriber() + self.chain_filter.connectInput(self.subscriber) diff --git a/doc/Tutorials/Latest-Time-Cpp.rst b/doc/Tutorials/Latest-Time-Cpp.rst new file mode 100644 index 00000000..a9c10fbd --- /dev/null +++ b/doc/Tutorials/Latest-Time-Cpp.rst @@ -0,0 +1,456 @@ +LatestTime Synchronizer (C++): +------------------------------ + +Overview +~~~~~~~~ + +This tutorial demonstrates how to use the ``Synchronizer`` filter with the ``LatestTime`` synchronization policy. + +To demonstrate this policy we will implement a node that will be publishing messages to +three different topics with different time intervals. +At the same time this node will create subscriptions to these topics and synchronize +messages coming from these topics using ``LatestTime`` policy. + +We are going to use ``std_msgs::msg::String`` message to make this tutorial clearer. + +Prerequisites +~~~~~~~~~~~~~ + +This tutorial assumes you have a working knowledge of ROS 2. + +If you have not done so already `create a workspace `_ and `create a package `_ + +1. Create a Basic Node with Includes +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Let's assume, you've already created an empty ROS 2 package for C++. +The next step is to create a new C++ file inside your package, e.g., ``latest_time_tutorial.cpp``, and create an example node: + +.. code-block:: C++ + + #include + #include + #include + #include + + #include "rclcpp/rclcpp.hpp" + + #include + + #include "message_filters/subscriber.hpp" + #include "message_filters/synchronizer.hpp" + #include "message_filters/sync_policies/latest_time.hpp" + + using namespace std::chrono_literals; + + class LatestTimeExampleNode: public rclcpp::Node { + public: + + typedef std_msgs::msg::String StringMsg; + typedef message_filters::sync_policies::LatestTime< + StringMsg, + StringMsg, + StringMsg + > SyncPolicy; + + LatestTimeExampleNode(): Node("latest_time_example_node") { + rclcpp::QoS qos = rclcpp::QoS(10); + + fast_publisher = this->create_publisher("fast_topic", qos); + slow_publisher = this->create_publisher("slow_topic", qos); + very_slow_publisher = this->create_publisher("very_slow_topic", qos); + + fast_sub.subscribe(this, "fast_topic", qos); + slow_sub.subscribe(this, "slow_topic", qos); + very_slow_sub.subscribe(this, "very_slow_topic", qos); + + fast_timer = this->create_wall_timer(500ms, std::bind(&LatestTimeExampleNode::fast_timer_callback, this)); + slow_timer = this->create_wall_timer(1000ms, std::bind(&LatestTimeExampleNode::slow_timer_callback, this)); + very_slow_timer = this->create_wall_timer(2000ms, std::bind(&LatestTimeExampleNode::very_slow_timer_callback, this)); + + sync = std::make_shared> ( + SyncPolicy(), + fast_sub, + slow_sub, + very_slow_sub + ); + + sync->registerCallback( + std::bind( + &LatestTimeExampleNode::sync_callback, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3 + ) + ); + } + + private: + void sync_callback( + const StringMsg::ConstSharedPtr & msg_1, + const StringMsg::ConstSharedPtr & msg_2, + const StringMsg::ConstSharedPtr & msg_3 + ) { + RCLCPP_INFO( + this->get_logger(), + "Sync callback. Msgs: %s, %s, %s", + msg_1->data.c_str(), + msg_2->data.c_str(), + msg_3->data.c_str() + ); + } + + void fast_timer_callback() { + fast_publisher->publish( + get_message( + "Fast msg. Count: " + std::to_string(++fast_counter) + ) + ); + } + + void slow_timer_callback() { + slow_publisher->publish( + get_message( + "Slow msg. Count: " + std::to_string(++slow_counter) + ) + ); + } + + void very_slow_timer_callback() { + very_slow_publisher->publish( + get_message( + "Very slow msg. Count: " + std::to_string(++very_slow_counter) + ) + ); + } + + StringMsg get_message(const std::string& msg_data) { + auto message = StringMsg(); + message.data = msg_data; + return message; + } + + private: + rclcpp::Publisher::SharedPtr fast_publisher; + rclcpp::Publisher::SharedPtr slow_publisher; + rclcpp::Publisher::SharedPtr very_slow_publisher; + + message_filters::Subscriber fast_sub; + message_filters::Subscriber slow_sub; + message_filters::Subscriber very_slow_sub; + + std::shared_ptr> sync; + + rclcpp::TimerBase::SharedPtr fast_timer; + rclcpp::TimerBase::SharedPtr slow_timer; + rclcpp::TimerBase::SharedPtr very_slow_timer; + + size_t fast_counter { 0 }; + size_t slow_counter { 0 }; + size_t very_slow_counter { 0 }; + }; + + int main(int argc, char ** argv) + { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; + } + +1.1 Examine the code +~~~~~~~~~~~~~~~~~~~~ +Now, let's break down this code and examine the details. + +.. code-block:: C++ + + #include + #include + #include + #include + + #include "rclcpp/rclcpp.hpp" + + #include + + #include "message_filters/subscriber.hpp" + #include "message_filters/synchronizer.hpp" + #include "message_filters/sync_policies/latest_time.hpp" + + using namespace std::chrono_literals; + +We start by including ``chrono`` and ``functional`` headers. +The ``chrono`` header is required for the ``chrono_literals`` namespace, necessary for creating timers. +The ``functional`` header is also required to use ``std::bind`` function to bind timer callbacks to timers. +And the ``memory`` header provides us with ``std::shared_ptr`` class and ``std::make_shared`` function template. +Also we are going to need the ``std::string`` class, so we add that via including ``string`` header. +After that we include the ``rclcpp.hpp`` header that provides us with classes from ``rclcpp`` namespace. +To use filters in our code we need corresponding headers as well. +Then we add ``string.hpp`` to get access to ``String`` message class from the ROS standard messages library. +The last includes are ``subscriber.hpp``, ``synchronizer.hpp`` and ``latest_time.hpp``. +The first will provide us with ``Subscriber`` and ``Synchronizer`` filters. +The last one will give us the access to the ``LatestTime`` sync policy. + +Next we define a ``LatestTimeExampleNode`` class. +First of all let's take a look at the type definitions that will be useful for making our code clearer. + +.. code-block:: C++ + + typedef std_msgs::msg::String StringMsg; + typedef message_filters::sync_policies::LatestTime< + StringMsg, + StringMsg, + StringMsg + > SyncPolicy; + +Here we define an alias for the ``std_msgs::msg::String`` message class that will be the ``StringMsg``. +Next we define an alias for the synchronization policy. +We will address it as ``SyncPolicy`` from now on. + +Now let's take a look at the class members declared in the ``private`` section of this class: + +.. code-block:: C++ + + private: + rclcpp::Publisher::SharedPtr fast_publisher; + rclcpp::Publisher::SharedPtr slow_publisher; + rclcpp::Publisher::SharedPtr very_slow_publisher; + + message_filters::Subscriber fast_sub; + message_filters::Subscriber slow_sub; + message_filters::Subscriber very_slow_sub; + + std::shared_ptr> sync; + + rclcpp::TimerBase::SharedPtr fast_timer; + rclcpp::TimerBase::SharedPtr slow_timer; + rclcpp::TimerBase::SharedPtr very_slow_timer; + + size_t fast_counter { 0 }; + size_t slow_counter { 0 }; + size_t very_slow_counter { 0 }; + +To publish messages we will need three ``Publisher`` objects. +These will be the ``fast_publisher``, the ``slow_publisher`` and the ``very_slow_publisher``. +All of these will work with ``StringMsg`` messages. +To subscribe to these topics we are going to need a few ``Subscriber`` filter objects as well. +The ``fast_sub`` the ``slow_sub`` and the ``very_slow_sub`` will play this role. +Next we declare a ``Synchronizer`` filter object that will be using ``SyncPolicy``. +And to automate the publishing process we add three timers. +That will be the ``fast_timer``, the ``slow_timer``, and the ``very_slow_timer``. +The last group of objects is the group of counters. +The ``fast_counter``, the ``slow_counter``, and the ``very_slow_counter``. +These will be useful to add some context to the final output. + +Nest let's take a look at the ``LatestTimeExampleNode`` class constructor. + +.. code-block:: C++ + + LatestTimeExampleNode(): Node("latest_time_example_node") { + rclcpp::QoS qos = rclcpp::QoS(10); + + fast_publisher = this->create_publisher("fast_topic", qos); + slow_publisher = this->create_publisher("slow_topic", qos); + very_slow_publisher = this->create_publisher("very_slow_topic", qos); + + fast_sub.subscribe(this, "fast_topic", qos); + slow_sub.subscribe(this, "slow_topic", qos); + very_slow_sub.subscribe(this, "very_slow_topic", qos); + + fast_timer = this->create_wall_timer(500ms, std::bind(&LatestTimeExampleNode::fast_timer_callback, this)); + slow_timer = this->create_wall_timer(1000ms, std::bind(&LatestTimeExampleNode::slow_timer_callback, this)); + very_slow_timer = this->create_wall_timer(2000ms, std::bind(&LatestTimeExampleNode::very_slow_timer_callback, this)); + + sync = std::make_shared> ( + SyncPolicy(), + fast_sub, + slow_sub, + very_slow_sub + ); + + sync->registerCallback( + std::bind( + &LatestTimeExampleNode::sync_callback, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3 + ) + ); + } + +We start the class construction by creating the publishers. +The ``fast_publisher``, the ``slow_publisher`` and the ``very_slow_publisher``. +Then the subscriptions as and entrypoint for the data flow. +The ``fast_sub`` the ``slow_sub`` and the ``very_slow_sub``. +For automation we add the ``fast_timer``, the ``slow_timer``, and the ``very_slow_timer``. +Now it is time for the main purpose of this tutorial. +The ``Synchronizer`` filter with ``LatestTime`` synchronization policy. +Finally we register a callback that will be invoked when any message comes into the filter. +This will be the ``sync_callback`` that is declared later. +The final part of this class that have not been addressed yet is the callbacks sections. +Let's take a look at them as well. + +.. code-block:: C++ + + void sync_callback( + const StringMsg::ConstSharedPtr & msg_1, + const StringMsg::ConstSharedPtr & msg_2, + const StringMsg::ConstSharedPtr & msg_3 + ) { + RCLCPP_INFO( + this->get_logger(), + "Sync callback. Msgs: %s, %s, %s", + msg_1->data.c_str(), + msg_2->data.c_str(), + msg_3->data.c_str() + ); + } + + void fast_timer_callback() { + fast_publisher->publish( + get_message( + "Fast msg. Count: " + std::to_string(++fast_counter) + ) + ); + } + + void slow_timer_callback() { + slow_publisher->publish( + get_message( + "Slow msg. Count: " + std::to_string(++slow_counter) + ) + ); + } + + void very_slow_timer_callback() { + very_slow_publisher->publish( + get_message( + "Very slow msg. Count: " + std::to_string(++very_slow_counter) + ) + ); + } + + StringMsg get_message(const std::string& msg_data) { + auto message = StringMsg(); + message.data = msg_data; + return message; + } + +The synchronization callback, the ``sync_callback`` outputs the contents of the messages. +Following are the timer callbacks. +The ``fast_timer_callback``, the ``slow_timer_callback``, and the ``very_slow_timer_callback``. +Every callback generates a message that tells the subscriber which publisher it comes from. +Finally there is the ``get_message`` function that just creates a message object +with the contents specified by caller. +It is added mainly to avoid some of the code duplication. + +To run the node and all the publishers, subscribers, timers and a filter we add the ``main`` function. + +.. code-block:: C++ + + int main(int argc, char ** argv) + { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; + } + +2. LatestTime synchronization policy +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The ``Synchronizer`` class provides an option to use different policies +to synchronize messages from multiple topics. +The ``LatestTime`` policy synchronizes up to N incoming channels by the rates they are received. +The callback with all the messages will be triggered whenever the fastest message is received. +The slower messages will be repeated at the rate of the fastest message and will be updated +whenever a new one is received. + +3. Update package.xml +~~~~~~~~~~~~~~~~~~~~~ + +Navigate to your package root and add the following dependencies in ``package.xml``: + +.. code-block:: xml + + message_filters + rclcpp + std_msgs + +4. Add the Node to a CMakeLists.txt +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Now open the ``CMakeLists.txt`` add the executable and name it ``latest_time_tutorial``, which you'll use later with ``ros2 run``. + +.. code-block:: CMake + + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + add_executable(latest_time_tutorial src/latest_time_tutorial.cpp) + +Finally, add the ``install(TARGETS…)`` section so ``ros2 run`` can find your executable: + +.. code-block:: CMake + + install(TARGETS + latest_time_tutorial + DESTINATION lib/${PROJECT_NAME}) + +5. Build Your Package +~~~~~~~~~~~~~~~~~~~~~ + +From the root of your workspace: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +6. Run the Node +~~~~~~~~~~~~~~~ + +Now run the node using: + +.. code-block:: console + + ros2 run latest_time_tutorial latest_time_tutorial + +The console output should look something like this: + +.. code-block:: console + + [INFO] [1773694323.514600409] [latest_time_example_node]: Sync callback. Msgs: Fast msg. Count: 4, Slow msg. Count: 2, Very slow msg. Count: 1 + [INFO] [1773694324.014240647] [latest_time_example_node]: Sync callback. Msgs: Fast msg. Count: 5, Slow msg. Count: 2, Very slow msg. Count: 1 + [INFO] [1773694324.514252533] [latest_time_example_node]: Sync callback. Msgs: Fast msg. Count: 6, Slow msg. Count: 3, Very slow msg. Count: 1 + [INFO] [1773694325.014487832] [latest_time_example_node]: Sync callback. Msgs: Fast msg. Count: 7, Slow msg. Count: 3, Very slow msg. Count: 1 + [INFO] [1773694325.514573661] [latest_time_example_node]: Sync callback. Msgs: Fast msg. Count: 8, Slow msg. Count: 4, Very slow msg. Count: 2 + [INFO] [1773694326.014433118] [latest_time_example_node]: Sync callback. Msgs: Fast msg. Count: 9, Slow msg. Count: 4, Very slow msg. Count: 2 + +Note that on each line the ``Fast msg`` counter increases it's value by 1. +At the same time, the ``Slow msg`` counter increments once for every two lines. +And it takes four lines to increase ``Very slow msg`` counter by one. +This means that it takes ``very_slow_publisher`` almost four times more time to publish one message than that of the ``fast_publisher``. +But the ``sync_callback`` is invoked for every incoming fast message with the previous values of all the other messages. +This is essentially an upsampling of slower messages. \ No newline at end of file diff --git a/doc/Tutorials/SimpleFilter-Cpp.rst b/doc/Tutorials/SimpleFilter-Cpp.rst new file mode 100644 index 00000000..48144167 --- /dev/null +++ b/doc/Tutorials/SimpleFilter-Cpp.rst @@ -0,0 +1,504 @@ +SimpleFilter (C++): +------------------- + +Overview +~~~~~~~~ + +This tutorial demonstrates how to create a custom filter that is going to be a successor to the ``SimpleFilter`` class. +The ``SimpleFilter`` is a base class for almost all of message filters implemented in ``C++``. +It provides the basic functionality for building filters. +To demonstrate the functionality of this filter we are going to create a ``CounterWithLastMessageCache`` filter class. + +Prerequisites +~~~~~~~~~~~~~ + +This tutorial assumes you have a working knowledge of ROS 2. + +If you have not done so already `create a workspace `_ and `create a package `_ + +1. Create a Basic Node +~~~~~~~~~~~~~~~~~~~~~~ + +Let's assume, you've already created an empty ROS 2 package for ``C++``. +The next step is to create a new C++ file inside your package, e.g., ``simple_filter_tutorial.cpp``, and write an example code: + +.. code-block:: C++ + + #include + #include + #include + + #include + + #include + #include + #include + + #include + + using namespace std::chrono_literals; + + const std::string TUTORIAL_TOPIC_NAME = "tutorial_topic"; + + namespace message_filters + { + template + class CounterWithLastMessageCache : public SimpleFilter { + typedef typename SimpleFilter::MConstPtr MConstPtr; + typedef typename SimpleFilter::Callback Callback; + typedef MessageEvent EventType; + + public: + virtual ~CounterWithLastMessageCache() {} + + CounterWithLastMessageCache() {} + + template + explicit CounterWithLastMessageCache(F & filter) + { + connectInput(filter); + } + + template + void connectInput(F & filter) + { + incoming_connection_.disconnect(); + incoming_connection_ = filter.registerCallback( + typename SimpleFilter::EventCallback( + std::bind( + &CounterWithLastMessageCache::add, + this, + std::placeholders::_1 + ) + ) + ); + } + + size_t getCounter() const { + return counter_; + } + + const MConstPtr getLastMsgCache() const { + return last_msg_cache_; + } + + void add(const EventType & evt) + { + counter_ += 1; + last_msg_cache_ = evt.getMessage(); + + signalMessage(evt); + } + + private: + MConstPtr last_msg_cache_; + size_t counter_ = 0; + + Connection incoming_connection_; + }; + + } + + class SimpleFilterExampleNode : public rclcpp::Node { + public: + SimpleFilterExampleNode() : + Node("SimpleFilterExampleNode"), + subscriber_filter_(), + counter_filter_(subscriber_filter_) + { + auto qos = rclcpp::QoS(10); + + subscriber_filter_.subscribe(this, TUTORIAL_TOPIC_NAME, qos); + + publisher_ = create_publisher(TUTORIAL_TOPIC_NAME, qos); + + publisher_timer_ = create_wall_timer( + 1s, + std::bind(&SimpleFilterExampleNode::publisher_timer_callback, this) + ); + + query_timer_ = create_wall_timer( + 1s, + std::bind(&SimpleFilterExampleNode::query_timer_callback, this) + ); + } + + void publisher_timer_callback() { + auto message = std_msgs::msg::String(); + message.data = "Pub count: " + std::to_string(++pub_counter_); + publisher_->publish(message); + } + + void query_timer_callback() { + if (counter_filter_.getCounter() != 0) { + RCLCPP_INFO( + get_logger(), + "Published messages count: %zu. Last message: %s", + counter_filter_.getCounter(), + counter_filter_.getLastMsgCache()->data.c_str() + ); + } else { + RCLCPP_INFO( + get_logger(), + "No messages published yet" + ); + } + } + + private: + message_filters::Subscriber subscriber_filter_; + message_filters::CounterWithLastMessageCache counter_filter_; + + rclcpp::Publisher::SharedPtr publisher_; + + rclcpp::TimerBase::SharedPtr publisher_timer_; + rclcpp::TimerBase::SharedPtr query_timer_; + + std::size_t pub_counter_ = 0; + }; + + int main([[maybe_unused]] int argc, [[maybe_unused]] char ** argv) + { + rclcpp::init(argc, argv); + + auto example_node = std::make_shared(); + + rclcpp::spin(example_node); + rclcpp::shutdown(); + + return 0; + } + +1.1 Examine the code +~~~~~~~~~~~~~~~~~~~~ + +Now, let's break down this code and examine the details. + +.. code-block:: C++ + + #include + #include + #include + + #include + + #include + #include + #include + + #include + + using namespace std::chrono_literals; + + const std::string TUTORIAL_TOPIC_NAME = "tutorial_topic"; + +We start by including ``C++`` standard library headers such as ``chrono``, ``cstddef`` and ``string``. +The ``chrono`` header is required for the ``chrono_literals`` namespace, necessary for creating timers. +The ``cstddef`` header provides us with some basic types such as ``size_t``. +The ``string`` header gives us access to the ``std::string`` class and ``std::to_string`` function. +After that we include the ``rclcpp.hpp`` header that provides us with classes from ``rclcpp`` namespace. +To use message filters and some other classes from ``message_filters`` library we need to include corresponding headers. +In this case we include ``simple_filter.hpp``, ``subscriber.hpp`` and ``connection.hpp``. + +Next we defne ``CounterWithLastMessageCache`` and make it a part of the ``message_filters`` namespace. + +.. code-block:: C++ + + namespace message_filters + { + template + class CounterWithLastMessageCache : public SimpleFilter { + typedef typename SimpleFilter::MConstPtr MConstPtr; + typedef typename SimpleFilter::Callback Callback; + typedef MessageEvent EventType; + +We start with a few ``typedef declarations`` for ``MConstPtr``, ``Callback`` and ``EventType`` to make the code cleaner. +Now let's take a look at the ``public`` section of the class. + +.. code-block:: C++ + + public: + virtual ~CounterWithLastMessageCache() {} + + CounterWithLastMessageCache() {} + + template + explicit CounterWithLastMessageCache(F & filter) + { + connectInput(filter); + } +`` +It starts with a default destructor and constructor for the class, and one constructor that receives a reference to another filter. +The latter gives an option to create an instance of the ``CounterWithLastMessageCache`` filter that is already connected to another filter's output. +Following the last constructor is the ``connectInput`` method, which removes the previous connection with another filter, if there was any. +This is done by the ``incoming_connection_.disconnect()`` call. +This way, another filter, if there was any, stops passing messages to this filter. +Any previously registered callbacks are no longer executed. + +.. code-block:: C++ + + template + void connectInput(F & filter) + { + incoming_connection_.disconnect(); + incoming_connection_ = filter.registerCallback( + typename SimpleFilter::EventCallback( + std::bind( + &CounterWithLastMessageCache::add, + this, + std::placeholders::_1 + ) + ) + ); + } + +To create a connection with a parent class filter, ``connectInput`` registers the ``add`` method of this class as a callback with another filter. +Thus, when a message arrives to this parent filter and calls the ``signalMessage`` method, +a message is passed to this ``CounterWithLastMessageCache`` filter's ``add`` method. +And from there, to other filters if there are any. + +The ``signalMessage`` method is of the base ``SimpleFilter`` class. +Every instance of the ``SimpleFilter`` class may have a collection of callbacks. +A callback may be added to this collection via ``registerCallback`` call. +When the ``signalMessage`` method of a ``SimpleFilter`` class is called, +every callback from this collection is executed. +So it is important to note that for your filter to be able pass messages to other filters, +it has to call the ``signalMessage`` method at some point in it's workflow. + +Now let's turn to the public interface of the class. +The ``getCounter`` method returns the current value of the counter. +The ``getLastMsgCache`` method provides access to the last message, that has passed through this filter. +And the ``add`` method does all the message processing work. +It increases the ``counter_`` for every passing method, as well as it stores the last message data to the ``last_message_cache_``. + +.. code-block:: C++ + + size_t getCounter() const { + return counter_; + } + + const MConstPtr getLastMsgCache() const { + return last_msg_cache_; + } + + void add(const EventType & evt) + { + counter_ += 1; + last_msg_cache_ = evt.getMessage(); + + signalMessage(evt); + } + +The private section in this case is rather simple. +It consists of the ``counter_`` and the ``last_message_cache_`` fields and the ``incoming_connection_`` field. +First two are the part of the business logic of this class. +The last one manages the connection with a filter that passes messages to this one. + +.. code-block:: C++ + + private: + size_t counter_ = 0; + MConstPtr last_msg_cache_; + + Connection incoming_connection_; + }; + + } + +This is it for the ``CounterWithLastMessageCache`` class. +Now let's take a look at the ``SimpleFilterExampleNode``. +The public interface of the node consists of three methods. +The node's constructor, the ``publisher_timer_callback`` and the ``query_timer_callback``. + +.. code-block:: C++ + + class SimpleFilterExampleNode : public rclcpp::Node { + public: + SimpleFilterExampleNode() : + Node("SimpleFilterExampleNode"), + subscriber_filter_(), + counter_filter_(subscriber_filter_) + { + auto qos = rclcpp::QoS(10); + + subscriber_filter_.subscribe(this, TUTORIAL_TOPIC_NAME, qos); + + publisher_ = create_publisher(TUTORIAL_TOPIC_NAME, qos); + + publisher_timer_ = create_wall_timer( + 1s, + std::bind(&SimpleFilterExampleNode::publisher_timer_callback, this) + ); + + query_timer_ = create_wall_timer( + 1s, + std::bind(&SimpleFilterExampleNode::query_timer_callback, this) + ); + } + + void publisher_timer_callback() { + auto message = std_msgs::msg::String(); + message.data = "Pub count: " + std::to_string(++pub_counter_); + publisher_->publish(message); + } + + void query_timer_callback() { + if (counter_filter_.getCounter() != 0) { + RCLCPP_INFO( + get_logger(), + "Published messages count: %zu. Last message: %s", + counter_filter_.getCounter(), + counter_filter_.getLastMsgCache()->data.c_str() + ); + } else { + RCLCPP_INFO( + get_logger(), + "No messages published yet" + ); + } + } + +The constructor initializes the node itself, and the two filters. +The instance of the ``SubscriberFilter`` receives messages from the ``TUTORIAL_TOPIC``. +The instance of the ``CounterWithLastMessageCache`` filter is immediately connected to the ``subscriber_filter_``'s output. +The ``subscriber_filter_`` subscribes to the ``TUTORIAL_TOPIC``. +After that the ``publisher_`` is created to populate the topic with messages. + +Two timers are added to automate the work: +The ``publisher_timer_`` automates message publishing. +The ``query_timer_`` automates the introspection of the node filter's state. + +The ``private`` section of the node holds the declarations of all the fields, required for the work, described above. + +.. code-block:: C++ + + private: + message_filters::Subscriber subscriber_filter_; + message_filters::CounterWithLastMessageCache counter_filter_; + + rclcpp::Publisher::SharedPtr publisher_; + + rclcpp::TimerBase::SharedPtr publisher_timer_; + rclcpp::TimerBase::SharedPtr query_timer_; + + std::size_t pub_counter_ = 0; + }; + +The main section is rather simple in this example. + +.. code-block:: C++ + + int main([[maybe_unused]] int argc, [[maybe_unused]] char ** argv) + { + rclcpp::init(argc, argv); + + auto example_node = std::make_shared(); + + rclcpp::spin(example_node); + rclcpp::shutdown(); + + return 0; + } + +2. Update package.xml +~~~~~~~~~~~~~~~~~~~~~ + +Navigate to your package root and add the following dependencies in ``package.xml``: + +.. code-block:: xml + + message_filters + rclcpp + std_msgs + +3. Add the Node to a CMakeLists.txt +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Now open the ``CMakeLists.txt`` add the executable and name it ``simple_filter_tutorial``, which you’ll use later with ``ros2 run``. + +.. code-block:: CMake + + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + ament_auto_add_executable(simple_filter_tutorial src/simple_filter_tutorial.cpp) + +Finally, add the ``install(TARGETS…)`` section so ros2 run can find your executable: + +.. code-block:: CMake + + install(TARGETS simple_filter_tutorial + DESTINATION lib/${PROJECT_NAME}) + +4. Build Your Package +~~~~~~~~~~~~~~~~~~~~~ + +From the root of your workspace: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +5. Run the Node +~~~~~~~~~~~~~~~ + +Now run the node using: + +.. code-block:: console + + ros2 run simple_filter_tutorial simple_filter_tutorial + +The output is going to look something like this + +.. code-block:: console + + [INFO] [1766958211.195602859] [SimpleFilterExampleNode]: No messages published yet + [INFO] [1766958212.195809044] [SimpleFilterExampleNode]: Published messages count: 1. Last message: Pub count: 1 + [INFO] [1766958213.195618995] [SimpleFilterExampleNode]: Published messages count: 2. Last message: Pub count: 2 + [INFO] [1766958214.195599466] [SimpleFilterExampleNode]: Published messages count: 3. Last message: Pub count: 3 + [INFO] [1766958215.195890964] [SimpleFilterExampleNode]: Published messages count: 4. Last message: Pub count: 4 + [INFO] [1766958216.195910443] [SimpleFilterExampleNode]: Published messages count: 5. Last message: Pub count: 5 + [INFO] [1766958217.195906785] [SimpleFilterExampleNode]: Published messages count: 6. Last message: Pub count: 6 + [INFO] [1766958218.195652168] [SimpleFilterExampleNode]: Published messages count: 7. Last message: Pub count: 7 + +Note that when the first query is executed, there were no messages that have passed the filter, as is indicated by the console output. +After that we see that the count of the messages starts to increase. + +.. code-block:: console + + ... Published messages count: 1. ... + ... Published messages count: 2. ... + ... Published messages count: 3. ... + ... Published messages count: 4. ... + ... Published messages count: 5. ... + ... Published messages count: 6. ... + ... Published messages count: 7. ... + +as well as the last message cache starts to update + +.. code-block:: console + + ... Last message: Pub count: 1 + ... Last message: Pub count: 2 + ... Last message: Pub count: 3 + ... Last message: Pub count: 4 + ... Last message: Pub count: 5 + ... Last message: Pub count: 6 + ... Last message: Pub count: 7 diff --git a/doc/Tutorials/SimpleFilter-Python.rst b/doc/Tutorials/SimpleFilter-Python.rst new file mode 100644 index 00000000..354d9b8b --- /dev/null +++ b/doc/Tutorials/SimpleFilter-Python.rst @@ -0,0 +1,461 @@ +SimpleFilter (Python): +---------------------- + +Overview +~~~~~~~~ + +This tutorial demonstrates how to create a custom filter that is going to be a successor to the ``SimpleFilter`` class. +The ``SimpleFilter`` is a base class for almost all of message filters implemented in ``Python``. +It provides the basic functionality for building filters. +To demonstrate the functionality of this filter we are going to create a ``CounterWithCallback`` filter class. + +Prerequisites +~~~~~~~~~~~~~ + +This tutorial assumes you have a working knowledge of ROS 2. + +If you have not done so already `create a workspace `_ and `create a package `_ + +1. Create a Basic Node +~~~~~~~~~~~~~~~~~~~~~~ + +Let's assume, you've already created an empty ROS 2 package for Python. +The next step is to create a new Python file inside your package, e.g. ``simple_filter_tutorial.py``, and write an example code: + +.. code-block:: python + + import typing as tp + + import rclpy + from rclpy.node import Node + from rclpy.qos import QoSProfile + + from message_filters import SimpleFilter, Subscriber + from std_msgs.msg import String + from rclpy.type_support import MsgT + + + TUTORIAL_TOPIC = "/example/topic" + QOS_PROFILE = QoSProfile(depth=10) + TIMER_PERIOD_SECONDS = 1 + + + class CounterWithCallback(SimpleFilter): + + def __init__( + self, + message_filter = None, + ): + super().__init__() + + self.count = 0 + + self.incoming_connection = None + + self.connectInput(message_filter=message_filter) + self.registerCallback(self.counter_callback) + + def connectInput(self, message_filter): + if self.incoming_connection is not None: + raise RuntimeError('Already connected') + self.incoming_connection = message_filter.registerCallback(self.add) + + def registerCallback(self, callback, *args): + super().registerCallback(callback, *args) + + def add(self, message: MsgT): + # Some other work may be done here with a message + super().signalMessage(message) + + def counter_callback(self, _: MsgT): + self.count += 1 + + def print_counter_value(self): + print(f"Filter counter value: {self.count}") + + + class LastMessageCache: + def __init__(self): + self.last_message = None + + def new_message_callback(self, message: MsgT): + self.last_message = message + + def print_last_msg_data(self): + if self.last_message: + print(f"Last cached message: {self.last_message.data}") + else: + print(f"Cache is empty") + + + class SimpleFilterExampleNode(Node): + + def __init__(self): + super().__init__("simple_filter_example_node") + + self.pub_count = 0 + self.publisher = self.create_publisher( + String, + TUTORIAL_TOPIC, + QOS_PROFILE, + ) + + self.subscriber_filter = Subscriber( + node=self, + msg_type=String, + topic=TUTORIAL_TOPIC, + qos_profile=QOS_PROFILE, + ) + + self.cache = LastMessageCache() + + self.filter = CounterWithCallback( + message_filter=self.subscriber_filter, + ) + self.filter.registerCallback( + self.cache.new_message_callback + ) + + self.publisher_timer = self.create_timer( + TIMER_PERIOD_SECONDS, + self.publisher_timer_callback, + ) + + self.query_timer = self.create_timer( + TIMER_PERIOD_SECONDS, + self.counter_query_timer_callback, + ) + + def publisher_timer_callback(self): + msg = String() + msg.data = f"Example string data № {self.pub_count}" + self.publisher.publish(msg) + self.pub_count += 1 + + def counter_query_timer_callback(self): + self.filter.print_counter_value() + self.cache.print_last_msg_data() + print("") + + + def main(): + rclpy.init() + + example_node = SimpleFilterExampleNode() + rclpy.spin(example_node) + + example_node.destroy_node() + rclpy.shutdown() + + + if __name__ == '__main__': + main() + +1.1 Examine the code +~~~~~~~~~~~~~~~~~~~~ + +Now, let's break down this code and examine the details. + +.. code-block:: python + + import typing as tp + + import rclpy + from rclpy.node import Node + from rclpy.qos import QoSProfile + + from message_filters import SimpleFilter, Subscriber + from std_msgs.msg import String + from rclpy.type_support import MsgT + + + TUTORIAL_TOPIC = "/example/topic" + QOS_PROFILE = QoSProfile(depth=10) + TIMER_PERIOD_SECONDS = 1 + +We start with importing ``rclpy``, ``Node`` and ``QoSProfile`` classes +that are required for constructing node and it's subscriptions and publishers, +and for running the node itself. +After that we import ``SimpleFilter`` and ``Subscriber`` filter classes. +The first one will serve as a base class for the example filter. +The second will serve as an entry point for messages from an example topic. +Next the ``String`` message class is imported. +We are giong to create a publisher and subscriber for this message type. +And finally the message type ``MsgT`` that will be used for type hints. +In the end of this section the ``TUTORIAL_TOPIC``, ``QOS_PROFILE`` and ``TIMER_PERIOD_SECONDS`` constants are declared. +These will be used for configuring publisher and subscriber as well as for publisher automation later on. + +Next let's take a look at the ``CounterWithCallback`` class. +It implements a synthetic scenario to demonstrate all the main features of the ``SimpleFilter`` class in a clearer manner. + +.. code-block:: python + + class CounterWithCallback(SimpleFilter): + + def __init__( + self, + message_filter = None, + ): + super().__init__() + + self.count = 0 + + self.incoming_connection = None + + self.connectInput(message_filter=message_filter) + self.registerCallback(self.counter_callback) + + def connectInput(self, message_filter): + if self.incoming_connection is not None: + raise RuntimeError('Already connected') + self.incoming_connection = message_filter.registerCallback(self.add) + + def registerCallback(self, callback, *args): + super().registerCallback(callback, *args) + + def add(self, message: MsgT): + # Some other work may be done here with message + super().signalMessage(message) + + def counter_callback(self, _: MsgT): + self.count += 1 + + def print_counter_value(self): + print(f"Filter counter value: {self.count}") + +This filter accepts other filter as an argument of the ``__init__`` method. +If any is provided, this filter registers it's own ``add`` method as a callback with that other filter. +Thus, when a message arrives to this other filter and that filter calls the ``signalMessage``, +a message is passed to this ``CounterWithCallback`` filter's ``add`` method and from there, down the chain of filters. +The ``signalMessage`` method is of the base ``SimpleFilter`` class. + +Every instance of the ``SimpleFilter`` class may have a collection of callbacks. +A callback may be added to this collection via ``registerCallback`` call. + +.. code-block:: python + + def callback(message: MsgT): + # Some work done here + + ... + + def callback_with_args(message: MsgT, *args): + # Some work done here + + ... + + # register callback *without* arguments + simple_filter.registerCallback(callback) + + # register callback *with* arguments + simple_filter.registerCallback(callback_with_args, arguments_list) + +When the ``signalMessage`` method of a ``SimpleFilter`` class is called, +all callbacks from this collection are executed with the corresponding arguments if any. + +So it is important to note that for your filter to be able pass messages to other filters, +it has to call the ``signalMessage`` method at some point in it's workflow. + +The example ``CounterWithCallback`` class, as the name suggests counts the messages coming through. +Also it gives an option to register a callback to be called for every message. +To be as precise as possible, evetry subclass of the ``SimpleFilter`` class provides this option expressly prohibited. +To demonstrate this, there is the ``registerCallback`` method in the class interface, +that does nothing except calling ``super().registerCallback``. + +Next up is the ``LastMessageCache`` class. + +.. code-block:: python + + class LastMessageCache: + def __init__(self): + self.last_message = None + + def new_message_callback(self, message: MsgT): + self.last_message = message + + def print_last_msg_data(self): + if self.last_message: + print(f"Last cached message: {self.last_message.data}") + else: + print(f"Cache is empty") + +This class, as the name suggests, stores the last message, and provides a callback for the ``CounterWithCallback`` class. + +What is left to discuss is the ``SimpleFilterExampleNode`` that does all the management of classes defined earlier. + +.. code-block:: python + + class SimpleFilterExampleNode(Node): + + def __init__(self): + super().__init__("simple_filter_example_node") + + self.pub_count = 0 + self.publisher = self.create_publisher( + String, + TUTORIAL_TOPIC, + QOS_PROFILE, + ) + +First of all we initialize a basic ``ros2`` ``publisher`` that will generate the input for our filters. + +.. code-block:: python + + self.subscriber_filter = Subscriber( + node=self, + msg_type=String, + topic=TUTORIAL_TOPIC, + qos_profile=QOS_PROFILE, + ) + +Next the ``Subscriber`` filter for the messages coming from publisher to enter a chain of filters. + +.. code-block:: python + + self.cache = LastMessageCache() + + self.filter = CounterWithCallback( + message_filter=self.subscriber_filter, + ) + self.filter.registerCallback( + self.cache.new_message_callback + ) + +After the ``publisher`` and ``Subscruber`` are set up, we initialize ``LastMessageCache`` and ``CounterWithCallback`` filter. +We register the ``new_message_callback`` as a callback with the ``registerCallback`` call. + +.. code-block:: python + + self.publisher_timer = self.create_timer( + TIMER_PERIOD_SECONDS, + self.publisher_timer_callback, + ) + + self.query_timer = self.create_timer( + TIMER_PERIOD_SECONDS, + self.counter_query_timer_callback, + ) + + def publisher_timer_callback(self): + msg = String() + msg.data = f"Example string data № {self.pub_count}" + self.publisher.publish(msg) + self.pub_count += 1 + + def counter_query_timer_callback(self): + self.filter.print_counter_value() + self.cache.print_last_msg_data() + print("") + +What is left to do is to create timers and timer callbacks. + +The ``main`` function as usual in this tutorials is pretty straightforward. + +.. code-block:: python + + def main(): + rclpy.init() + + example_node = SimpleFilterExampleNode() + rclpy.spin(example_node) + + example_node.destroy_node() + rclpy.shutdown() + + + if __name__ == '__main__': + main() + +2. Update package.xml +~~~~~~~~~~~~~~~~~~~~~ + +Navigate to your package root and add the following dependencies in ``package.xml``: + +.. code-block:: xml + + message_filters + rclpy + std_msgs + +3. Add Entry Point in setup.py +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Update the ``console_scripts`` section of your ``setup.py``: + +.. code-block:: python + + entry_points={ + 'console_scripts': [ + 'simple_filter_tutorial = pkg_name.simple_filter_tutorial:main', + ], + }, + +Replace ``pkg_name`` with your actual package name. + +4. Build Your Package +~~~~~~~~~~~~~~~~~~~~~ + +From the root of your workspace: + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +5. Run the Node +~~~~~~~~~~~~~~~ + +Now run the node using: + +.. code-block:: console + + ros2 run pkg_name simple_filter_tutorial + +The output of the node is going to look something like this + +.. code-block:: console + + Filter counter value: 0 + Cache is empty + + Filter counter value: 1 + Last cached message: Example string data № 0 + + Filter counter value: 2 + Last cached message: Example string data № 1 + + Filter counter value: 3 + Last cached message: Example string data № 2 + + Filter counter value: 4 + Last cached message: Example string data № 3 + + Filter counter value: 5 + Last cached message: Example string data № 4 + + Filter counter value: 6 + Last cached message: Example string data № 5 + +Note that when the first query to the filter and the ``LastMessageCache`` is executed, +they both report that there was no messages, passing through before. +The following queries show that the count of messages passed through the filter is increasing +and that the last cached message is constantly updated. +That means that both ``CounterWithCallback.counter_callback`` +and ``LastMessageCache.new_message_callback`` are executed for every new message. +As designed. \ No newline at end of file diff --git a/doc/Tutorials/Writing-A-Time-Synchronizer-Cpp.rst b/doc/Tutorials/Writing-A-Time-Synchronizer-Cpp.rst new file mode 100644 index 00000000..87f9039e --- /dev/null +++ b/doc/Tutorials/Writing-A-Time-Synchronizer-Cpp.rst @@ -0,0 +1,203 @@ +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 `_ and `create a package `_ + +1. Create a Basic Node with Includes +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: C++ + + #include + + #include + #include + #include + + #include "message_filters/subscriber.h" + #include "message_filters/time_synchronizer.h" + + #include + #include + + using namespace std::chrono_literals; + + using std::placeholders::_1; + using std::placeholders::_2; + + class TimeSyncNode : public rclcpp::Node + { + public: + + private: + rclcpp::Publisher::SharedPtr temp_pub; + rclcpp::Publisher::SharedPtr fluid_pub; + message_filters::Subscriber temp_sub; + message_filters::Subscriber fluid_sub; + std::shared_ptr> sync; + rclcpp::TimerBase::SharedPtr timer; + }; + + +For this example we will be using the ``temperature`` and ``fluid_pressure`` messages found in +`sensor_msgs `_. +To simulate a working ``TimeSynchronizer`` 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::SharedPtr temp_pub; + rclcpp::Publisher::SharedPtr fluid_pub; + message_filters::Subscriber temp_sub; + message_filters::Subscriber 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 some sort of ``TimerBase``. Then, we will be utilizing a ``TimeSynchronizer`` 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("temp", qos); + fluid_pub = this->create_publisher("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(1000ms, std::bind(&TimeSyncNode::TimerCallback, this)); + + uint32_t queue_size = 10; + sync = std::make_shared>(temp_sub, fluid_sub, queue_size); + + 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. Notice that we must call ``sync->registerCallback`` to sync up the two (or more) chosen topics. + +So, we must create some private callbacks. + +.. 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; + sensor_msgs::msg::FluidPressure fluid; + auto now = rclcpp::Clock().now(); + + temp.header.stamp = now; + temp.header.frame_id = "test"; + temp.temperature = 1.0; + temp_pub->publish(temp); + + 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 because 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. +For the ``TimerCallback`` just initialize both the ``Temperature`` and ``FluidPressure`` in whatever way necessary, but make sure the header stamp of both have the same exact time, otherwise the ``TimeSynchronizer`` will be misaligned and won't do anything. +This is because the ``TimeSynchronizer`` has an ``ExactTime`` sync policy. + +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(); + 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 ``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(time_sync src/time_synchronizer.cpp) + ament_target_dependencies(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 + time_sync + DESTINATION lib/${PROJECT_NAME}) + +3. Build +~~~~~~~~ +From the root of your package, build and source. + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +4. Run +~~~~~~ +Run replacing the package name with whatever you named your workspace. + +.. code-block:: bash + + ros2 run pkg_name time_sync + +You should end up with a result similar to the following: + +.. code-block:: bash + + [INFO] [1714504937.157035000] [sync_node]: Sync callback with 1714504937 and 1714504937 as times diff --git a/doc/Tutorials/Writing-A-Time-Synchronizer-Python.rst b/doc/Tutorials/Writing-A-Time-Synchronizer-Python.rst new file mode 100644 index 00000000..e5001dca --- /dev/null +++ b/doc/Tutorials/Writing-A-Time-Synchronizer-Python.rst @@ -0,0 +1,167 @@ +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 `_ and `create a package `_ + + +1. Create a Basic Node with Imports +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. 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, TimeSynchronizer + 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) + + queue_size = 10 + self.sync = TimeSynchronizer([self.temp_sub, self.fluid_sub], queue_size) + self.sync.registerCallback(self.SyncCallback) + + +For this example we will be using the ``temperature`` and ``fluid_pressure`` messages found in +`sensor_msgs `_. +To simulate a working ``TimeSynchronizer`` 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 some sort of ``Timer``. +Then, we will be utilizing said ``TimeSynchronizer`` to get these messages from the sensor topics aligned, seen with the two ``Subscribers`` conjoined in the ``TimeSynchronizer`` initialization. + +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. +For each class member, do basic construction of the object relating to the ``Node`` and callback methods that may be used in the future. +Notice that we must call ``sync.registerCallback`` to sync up the two (or more) chosen topics. + +So, we must create some callbacks. + +.. 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() + fluid = FluidPressure() + 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) + + fluid.header.stamp = self.now + fluid.header.frame_id = "test" + fluid.fluid_pressure = 2.0 + self.fluid_pub.publish(fluid) + +``SyncCallback`` takes a fluid_pressure and a temperature relating to both topics because 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. +For the ``TimerCallback`` just initialize both the ``Temperature`` and ``FluidPressure`` in whatever way necessary, but make sure the header stamp of both have the same exact time, otherwise the ``TimeSynchronizer`` will be misaligned and won't do anything. + +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:: xml + + rclpy + message_filters + sensor_msgs + +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 + + 'time_sync = pkg_name.time_sync:main', + +3. Build +~~~~~~~~ +From the root of your package, build and source. + + +.. tabs:: + + .. group-tab:: Linux + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: macOS + + .. code-block:: console + + $ colcon build && . install/setup.bash + + .. group-tab:: Windows + + .. code-block:: console + + $ colcon build + $ call C:\dev\ros2\local_setup.bat + +4. Run +~~~~~~ +Run replacing the package name with whatever you named your workspace. + +.. code-block:: bash + + ros2 run pkg_name time_sync + +You should end up with a result similar to the following: + +.. code-block:: bash + + [INFO] [1714504937.157035000] [sync_node]: Sync callback with 1714504937 and 1714504937 as times diff --git a/conf.py b/doc/conf.py similarity index 81% rename from conf.py rename to doc/conf.py index 97d16fd9..e7e6dc46 100644 --- a/conf.py +++ b/doc/conf.py @@ -11,8 +11,7 @@ # All configuration values have a default; values that are commented out # serve to show the default. -import sys -import os +import sys, os # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the @@ -23,7 +22,8 @@ # Add any Sphinx extension module names here, as strings. They can be extensions # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. -extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.imgmath'] +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', + 'sphinx.ext.imgmath', 'sphinx_rtd_theme', 'sphinx_tabs.tabs'] # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] @@ -38,17 +38,22 @@ master_doc = 'index' # General information about the project. -project = u'message_filters' -copyright = u'2009, Willow Garage, Inc.' +from datetime import datetime +now = datetime.now() # current date and time +year = now.strftime("%Y") + +project = 'message_filters' +copyright = '2008-' + year + ', Open Source Robotics Foundation, Inc.' # noqa +author = 'Open Source Robotics Foundation, Inc.' # The version info for the project you're documenting, acts as replacement for # |version| and |release|, also used in various other places throughout the # built documents. # # The short X.Y version. -version = '0.1' +version = '4.12.0' # The full version, including alpha/beta/rc tags. -release = '0.1.0' +release = '4.12.0' # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -63,8 +68,6 @@ # List of documents that shouldn't be included in the build. #unused_docs = [] -exclude_patterns = ['CHANGELOG.rst'] - # List of directories, relative to source directory, that shouldn't be searched # for source files. exclude_trees = ['_build'] @@ -94,7 +97,7 @@ # The theme to use for HTML and HTML Help pages. Major themes that come with # Sphinx are currently 'default' and 'sphinxdoc'. -html_theme = 'default' +html_theme = 'sphinx_rtd_theme' # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -123,7 +126,7 @@ # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = [] +#html_static_path = ['_static'] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. @@ -161,7 +164,7 @@ #html_file_suffix = '' # Output file base name for HTML help builder. -htmlhelp_basename = 'tfdoc' +htmlhelp_basename = 'message_filters_doc' # -- Options for LaTeX output -------------------------------------------------- @@ -176,7 +179,7 @@ # (source start file, target name, title, author, documentclass [howto/manual]). latex_documents = [ ('index', 'message_filters.tex', u'stereo\\_utils Documentation', - u'James Bowman', 'manual'), + [copyright], 'manual'), ] # The name of an image file (relative to this directory) to place at the top of @@ -196,8 +199,30 @@ # If false, no module index is generated. #latex_use_modindex = True -# Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = { - 'http://docs.python.org/': None, - 'http://docs.scipy.org/doc/numpy' : None - } +# -- Options for manual page output ------------------------------------------ + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + (master_doc, 'message_filters', 'message_filters Documentation', + [copyright], 1) +] + +# -- Options for Texinfo output ---------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + (master_doc, 'message_filters', 'message_filters Documentation', + copyright, 'message_filters', 'ROS 2 ', + 'Miscellaneous'), +] + + +autoclass_content = "both" + +autodoc_default_options = { + 'members': True, # document members + 'undoc-members': True, # also document members without documentation +} diff --git a/doc/index.rst b/doc/index.rst new file mode 100644 index 00000000..7c63a0e7 --- /dev/null +++ b/doc/index.rst @@ -0,0 +1,271 @@ +Message Filters +=============== + +:mod:`message_filters` is a collection of message "filters" which take messages in, +either from a ROS 2 subscription or another filter, +and may or may not output the message +at some time in the future, depending on a policy defined for that filter. + +``message_filters`` also defines a common interface for these filters, allowing you to chain them together. + +The filters currently implemented in this package are: + + * :class:`message_filters.Subscriber` A source filter, which wraps a ROS 2 subscription. Most filter chains will begin with a Subscriber. + * :class:`message_filters.Cache` Caches messages which pass through it, allowing later lookup by time stamp. + * :class:`message_filters.TimeSynchronizer` Synchronizes multiple messages by their timestamps, only passing them through when all have arrived. + * :class:`message_filters.TimeSequencer` Tries to pass messages through ordered by their timestamps, even if some arrive out of order. + +1. Filter Pattern +----------------- +All message filters follow the same pattern for connecting inputs and outputs. Inputs are connected either through the filter's constructor or through the ``connectInput()`` method. Outputs are connected through the ``registerCallback()`` method. + +Note that the input and output types are defined perfilter, so not all filters are directly interconnectable. + +Filter Construction (C++):: + + FooFilter foo; + BarFilter bar; + BazFilter baz(foo); + bar.connectInput(foo); + +Filter Construction (Python):: + + bar(foo) + bar.connectInput(foo) + +1.1 registerCallback() +~~~~~~~~~~~~~~~~~~~~~~ +You can register multiple callbacks with the ``registerCallbacks()`` method. They will get called in the order they are registered. The signature of the callback depends on the definition of the filter + +In C++ ``registerCallback()`` returns a ``message_filters::Connection`` object that allows you to disconnect the callback by calling its ``disconnect()`` method. You do not need to store this connection object if you do not need to manually disconnect the callback. + +2. Subscriber +------------- +The Subscriber filter is simply a wrapper around a ROS 2 subscription that provides a source for other filters. The Subscriber filter cannot connect to another filter's output, instead it uses a ROS 2 topic as its input. + +2.1 Connections +~~~~~~~~~~~~~~~ +Input: + No input connections +Output: + * C++: ``void callback(const std::shared_ptr&)`` + * Python: ``callback(msg)`` + +2.2 Example (C++) +~~~~~~~~~~~~~~~~~ +.. code-block:: C++ + + message_filters::Subscriber sub(node, "my_topic", 1); + sub.registerCallback(myCallback); + +or + +.. code-block:: C++ + + message_filters::Subscriber sub = node.subscribe("my_topic", 1, myCallback); + +2.3 Example (Python) +~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: Python + + sub = message_filters.Subscriber(node_object, "pose_topic", robot_msgs.msg.Pose) + sub.registerCallback(myCallback) + +3. Time Synchronizer +-------------------- +The TimeSynchronizer filter synchronizes incoming channels by the timestamps contained in their headers, and outputs them in the form of a single callback that takes the same number of channels. The C++ implementation can synchronize up to 9 channels. + +3.1 Connections +~~~~~~~~~~~~~~~ +Input: + * C++: Up to 9 separate filters, each of which is of the form ``void callback(const std::shared_ptr&)``. The number of filters supported is determined by the number of template arguments the class was created with. + * Python: N separate filters, each of which has signature ``callback(msg)``. + +Output: + * C++: For message types M0..M8, ``void callback(const std::shared_ptr&, ..., const std::shared_ptr&)``. The number of parameters is determined by the number of template arguments the class was created with. + * Python: ``callback(msg0.. msgN)``. The number of parameters is determined by the number of template arguments the class was created with. + +4. Time Sequencer +----------------- +* Python: the TimeSequencer filter is not yet implemented. + +The TimeSequencer filter guarantees that messages will be called in temporal order according to their header's timestamp. The TimeSequencer is constructed with a specific delay which specifies how long to queue up messages before passing them through. A callback for a message is never invoked until the messages' time stamp is out of date by at least delay. However, for all messages which are out of date by at least the delay, their callback are invoked and guaranteed to be in temporal order. If a message arrives from a time prior to a message which has already had its callback invoked, it is thrown away. + +4.1 Connections +~~~~~~~~~~~~~~~ +Input: + * C++: ``void callback(const std::shared_ptr&)`` +Output: + * C++: ``void callback(const std::shared_ptr&)`` + +4.2 Example (C++) +~~~~~~~~~~~~~~~~~ +.. code-block:: C++ + + message_filters::Subscriber sub(node, "my_topic", 1); + message_filters::TimeSequencer seq( + sub, rclcpp::Duration(0.1), rclcpp::Duration(0.01), 10); + seq.registerCallback(myCallback); + +5. Cache +-------- +Stores a time history of messages. + +Given a stream of messages, the most recent N messages are cached in a ring buffer, from which time intervals of the cache can then be retrieved by the client. The timestamp of a message is determined from its header field. + +If the message type doesn't contain a header, see below for workaround. + +The Cache immediately passes messages through to its output connections. + +5.1 Connections +~~~~~~~~~~~~~~~ +Input: + * C++: ``void callback(const std::shared_ptr&)`` + * Python: ``callback(msg)`` +Output: + * C++: ``void callback(const std::shared_ptr&)`` + * Python: ``callback(msg)`` + +5.2 Example (C++) +~~~~~~~~~~~~~~~~~ +.. code-block:: C++ + + message_filters::Subscriber sub(node, "my_topic", 1); + message_filters::Cache cache(sub, 100); + cache.registerCallback(myCallback); + +5.3 Example (Python) +~~~~~~~~~~~~~~~~~~~~ +.. code-block:: Python + + sub = message_filters.Subscriber(node_object, 'my_topic', sensor_msgs.msg.Image) + cache = message_filters.Cache(sub, 100) + +In this example, the Cache stores the last 100 messages received on ``my_topic``, and ``myCallback`` is called on the addition of every new message. The user can then make calls like ``cache.getInterval(start, end)`` to extract part of the cache. +If the message type does not contain a header field that is normally used to determine its timestamp, and the Cache is contructed with ``allow_headerless=True``, the current ROS 2 time is used as the timestamp of the message. This is currently only available in Python. + +6. PolicyBased Synchronizers +---------------------------- +The Synchronizer filter synchronizes incoming channels by the timestamps contained in their headers, and outputs them in the form of a single callback that takes the same number of channels. The C++ implementation can synchronize up to 9 channels. + +The Synchronizer filter is templated on a policy that determines how to synchronize the channels. There are currently three policies: ExactTime, ApproximateEpsilonTime and ApproximateTime. + +6.1 Connections +~~~~~~~~~~~~~~~ +Input: + * C++: Up to 9 separate filters, each of which is of the form ``void callback(const std::shared_ptr&)``. The number of filters supported is determined by the number of template arguments the class was created with. + * Python: N separate filters, each of which has signature ``callback(msg)``. +Output: + * C++: For message types M0..M8, ``void callback(const std::shared_ptr&, ..., const std::shared_ptr&)``. The number of parameters is determined by the number of template arguments the class was created with. + * Python: ``callback(msg0.. msgN)``. The number of parameters is determined by the number of template arguments the class was created with. + +6.2 ExactTime Policy +~~~~~~~~~~~~~~~~~~~~ +The ``message_filters::sync_policies::ExactTime`` policy requires messages to have exactly the same timestamp in order to match. Your callback is only called if a message has been received on all specified channels with the same exact timestamp. The timestamp is read from the header field of all messages (which is required for this policy). + +6.3 ApproximateEpsilonTime Policy +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +The ``message_filters::sync_policies::ApproximateEpsilonTime`` policy requires messages to have exactly the same timestamp within an "epsilon" tolerance in order to match. Your callback is only called if a message has been received on all specified channels with that same exact timestamp within the tolerance. The timestamp is read from the header field of all messages (which is required for this policy). + +6.4 ApproximateTime Policy +~~~~~~~~~~~~~~~~~~~~~~~~~~ +The ``message_filters::sync_policies::ApproximateTime`` policy uses an adaptive algorithm to match messages based on their timestamp. + +If not all messages have a header field from which the timestamp could be determined, see below for a workaround. If some messages are of a type that doesn't contain the header field, ``ApproximateTimeSynchronizer`` refuses by default adding such messages. However, its Python version can be constructed with ``allow_headerless=True``, which uses current ROS 2 time in place of any missing header.stamp field: + +7. Chain +-------- +**Python:** the Chain filter is not yet implemented. +The Chain filter allows you to dynamically chain together multiple singleinput/single-output (simple) filters. As filters are added to it they are automatically connected together in the order they were added. It also allows you to retrieve added filters by index. + +Chain is most useful for cases where you want to determine which filters to apply at runtime rather than compiletime. + +7.1 Connections +~~~~~~~~~~~~~~~ +Input: + * C++: ``void callback(const std::shared_ptr&)`` +Output: + * C++: ``void callback(const std::shared_ptr&)`` + +7.2 Examples (C++) +~~~~~~~~~~~~~~~~~~ +**Simple Example** + +.. code-block:: C++ + + void myCallback(const MsgConstPtr& msg) + { + } + + Chain c; + c.addFilter(std::shared_ptr >(new Subscriber)); + c.addFilter(std::shared_ptr >(new TimeSequencer)); + c.registerCallback(myCallback); + +**Bare Pointers** +It is possible to pass bare pointers in. These will not be automatically deleted when Chain is destructed. + +.. code-block:: C++ + + Chain c; + Subscriber s; + c.addFilter(&s); + c.registerCallback(myCallback); + +**Retrieving a Filter** + +.. code-block:: C++ + + Chain c; + size_t sub_index = c.addFilter(std::shared_ptr >(new Subscriber)); + std::shared_ptr > sub = c.getFilter >(sub_index); + + +The message filter interface +---------------------------- + +For an object to be usable as a message filter, it needs to have one method, +``registerCallback``. To collect messages from a message filter, register a callback with:: + + anyfilter.registerCallback(my_callback) + +The signature of ``my_callback`` varies according to the message filter. For many filters it is simply:: + + def my_callback(msg): + +where ``msg`` is the message. + +Message filters that accept input from an upstream +message filter (e.g. :class:`message_filters.Cache`) register their own +message handler as a callback. + +Output connections are registered through the ``registerCallback()`` function. + +Here's a simple example of using a Subscriber with a Cache:: + + def myCallback(posemsg): + print(posemsg) + + sub = message_filters.Subscriber(node_object, robot_msgs.msg.Pose, "pose_topic") + cache = message_filters.Cache(sub, 10) + cache.registerCallback(myCallback) + +The Subscriber here acts as the source of messages. Each message is passed to the cache, +which then passes it through to the user's callback ``myCallback``. + +**Note**: It is **VERY IMPORTANT** that each subscriber has the same ``qos_profile`` as the one specified in the corresponding publisher code for each topic you want to subscribe to. +If they don't match, the callback won't be executed (without any warning) and you will be very frustrated. + +.. toctree:: + :maxdepth: 2 + + self + tutorials + references + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`search` diff --git a/doc/references.rst b/doc/references.rst new file mode 100644 index 00000000..f3c1e06f --- /dev/null +++ b/doc/references.rst @@ -0,0 +1,8 @@ +References +========== + +.. toctree:: + :maxdepth: 2 + + C++ API + Python Modules diff --git a/doc/tutorials.rst b/doc/tutorials.rst new file mode 100644 index 00000000..9d594fb5 --- /dev/null +++ b/doc/tutorials.rst @@ -0,0 +1,31 @@ +.. _Tutorials: + +Tutorials +========= + +Various tutorials for ``message_filters`` in both C++ and Python. +The tutorials will provide a swell of knowledge from Basic Understanding to advanced innerworkings + +Workspace setup +--------------- + +If you have not yet created a workspace in which to complete the tutorials, `follow this tutorial `_ . + +Basic Tutorials +--------------- + +.. toctree:: + :maxdepth: 1 + + Tutorials/Writing-A-Time-Synchronizer-Cpp + Tutorials/Writing-A-Time-Synchronizer-Python + Tutorials/Approximate-Synchronizer-Cpp + Tutorials/Approximate-Synchronizer-Python + Tutorials/Cache-Python + Tutorials/Cache-Cpp + Tutorials/Chain-Cpp + Tutorials/Chain-Python + Tutorials/SimpleFilter-Python + Tutorials/SimpleFilter-Cpp + Tutorials/Approximate-Epsilon-Time-Synchronizer-Cpp.rst + Tutorials/Latest-Time-Cpp.rst diff --git a/include/message_filters/cache.h b/include/message_filters/cache.h index 43a3b5a2..f4e886c5 100644 --- a/include/message_filters/cache.h +++ b/include/message_filters/cache.h @@ -47,7 +47,6 @@ namespace message_filters { -using namespace std::placeholders; /** * \brief Stores a time history of messages * @@ -90,7 +89,8 @@ class Cache : public SimpleFilter template void connectInput(F& f) { - incoming_connection_ = f.registerCallback(typename SimpleFilter::EventCallback(std::bind(&Cache::callback, this, _1))); + incoming_connection_ = f.registerCallback(typename SimpleFilter::EventCallback( + std::bind(&Cache::callback, this, std::placeholders::_1))); } ~Cache() diff --git a/include/message_filters/cache.hpp b/include/message_filters/cache.hpp new file mode 100644 index 00000000..1bca1942 --- /dev/null +++ b/include/message_filters/cache.hpp @@ -0,0 +1,34 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__CACHE_HPP_ +#define MESSAGE_FILTERS__CACHE_HPP_ + +#include "cache.h" + +#endif // MESSAGE_FILTERS__CACHE_HPP_ diff --git a/include/message_filters/chain.hpp b/include/message_filters/chain.hpp new file mode 100644 index 00000000..c14656f3 --- /dev/null +++ b/include/message_filters/chain.hpp @@ -0,0 +1,34 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__CHAIN_HPP_ +#define MESSAGE_FILTERS__CHAIN_HPP_ + +#include "chain.h" + +#endif // MESSAGE_FILTERS__CHAIN_HPP_ diff --git a/include/message_filters/connection.hpp b/include/message_filters/connection.hpp new file mode 100644 index 00000000..cb59a5f9 --- /dev/null +++ b/include/message_filters/connection.hpp @@ -0,0 +1,34 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__CONNECTION_HPP_ +#define MESSAGE_FILTERS__CONNECTION_HPP_ + +#include "connection.h" + +#endif // MESSAGE_FILTERS__CONNECTION_HPP_ diff --git a/include/message_filters/delta_filter.hpp b/include/message_filters/delta_filter.hpp new file mode 100644 index 00000000..44c06cce --- /dev/null +++ b/include/message_filters/delta_filter.hpp @@ -0,0 +1,294 @@ +// Copyright 2026, Open Source Robotics Foundation, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__DELTA_FILTER_HPP_ +#define MESSAGE_FILTERS__DELTA_FILTER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace message_filters +{ + +/** + * \brief Base class for comparison handlers. + * + * Provides class interface but nothing more. + */ +template +class ComparisonHandler +{ +public: + typedef std::shared_ptr MConstPtr; + typedef MessageEvent EventType; + + /** + * Comparator function type: takes two messages (cached and current), + * returns true if the fields being observed satisfy the comparison condition. + */ + typedef std::function FieldComparatorFunctionType; + + virtual ~ComparisonHandler() = default; + + virtual bool message_fits(const EventType & message) = 0; + + virtual bool do_fields_fit( + const FieldComparatorFunctionType & comparator, + const MConstPtr & cached, + const MConstPtr & current) const = 0; +}; + +/** + * \brief CachedComparisonHandler implements messages comparison field by field. + * + * A successor to this class should implement 'do_fields_fit' method. + * If any of the comparators provided to 'do_fields_fit' satisfies the comparison + * conditions the message is accepted. That means that the message is stored in + * the 'message_cache_' and the 'message_fits' method returns 'true'. + */ +template +class CachedComparisonHandler : public ComparisonHandler +{ +public: + typedef std::shared_ptr MConstPtr; + typedef MessageEvent EventType; + typedef std::function FieldComparatorFunctionType; + + /** + * Initialize handler. + * \param field_comparators A list of callable objects that take the cached and + * current message and return true if the comparison condition is satisfied. + * If any comparator returns true the current message is accepted. + */ + explicit CachedComparisonHandler( + std::forward_list field_comparators + ) + : ComparisonHandler(), + field_comparators_(field_comparators) + {} + + virtual ~CachedComparisonHandler() = default; + + bool message_fits(const EventType & message) + { + std::lock_guard lock(message_cache_mutex_); + + if (!message_cache_) { + message_cache_.reset(new EventType(message)); + return true; + } + + const MConstPtr & cached_msg = message_cache_->getConstMessage(); + const MConstPtr & current_msg = message.getConstMessage(); + + const bool any_field_fits = std::any_of( + field_comparators_.begin(), + field_comparators_.end(), + [&] (const FieldComparatorFunctionType & comparator) + { + return do_fields_fit(comparator, cached_msg, current_msg); + } + ); + + if (any_field_fits) { + message_cache_.reset(new EventType(message)); + } + return any_field_fits; + } + + virtual bool do_fields_fit( + const FieldComparatorFunctionType & comparator, + const MConstPtr & cached, + const MConstPtr & current) const = 0; + +private: + std::mutex message_cache_mutex_; + + std::unique_ptr message_cache_; + + std::forward_list field_comparators_; +}; + +/** + * \brief DeltaCompare implements delta comparison. + * + * If any of the comparators returns true when applied to the previously cached + * and current message, the current message is accepted. + */ +template +class DeltaCompare : public CachedComparisonHandler +{ +public: + typedef std::shared_ptr MConstPtr; + typedef std::function FieldComparatorFunctionType; + + /** + * Initialize handler. + * + * \param field_comparators A list of callable objects that take the cached and + * current message and return true if the fields being observed differ. + * If any comparator returns true the current message is accepted. + */ + explicit DeltaCompare( + std::forward_list field_comparators + ) + : CachedComparisonHandler(field_comparators) + {} + + virtual ~DeltaCompare() = default; + + bool do_fields_fit( + const FieldComparatorFunctionType & comparator, + const MConstPtr & cached, + const MConstPtr & current) const + { + return comparator(cached, current); + } +}; + +/** + * \brief ROS 2 Comparison filter. + * + * Given a stream of messages, the message is passed down to the next filter + * if 'comparison_handler' 'message_fits' method returns 'true' for that message. + * + * \param HandlerType [template] Specification + * of the CachedComparisonHandler successor class. + * Is expected implement a ``message_fits`` method. If ``message_fits`` returns ``True`` + * for a provided message, that message is considered valid and is passed + * to a next filter if any. + */ +template class HandlerType> +class ComparisonFilter : public SimpleFilter +{ +public: + typedef std::shared_ptr MConstPtr; + typedef MessageEvent EventType; + typedef std::function FieldComparatorFunctionType; + + /** + * Initialize ComparisonFilter. + * + * \param filter An instance of the 'SimpleFilter' successor class. + * The input filter to connect to. + * + * \param field_comparators A list of callable objects that take the cached and + * current message and return true if the comparison condition is satisfied. + * If any comparator returns true the current message is accepted. + */ + template + explicit ComparisonFilter( + FilterType & filter, + std::forward_list field_comparators + ) + :SimpleFilter(), + comparison_handler_(field_comparators) + { + connectInput(filter); + } + + virtual ~ComparisonFilter() = default; + + template + void connectInput(FilterType & filter) + { + incoming_connection_.disconnect(); + incoming_connection_ = filter.registerCallback( + typename SimpleFilter::EventCallback( + std::bind( + &ComparisonFilter::add, + this, + std::placeholders::_1 + ) + ) + ); + } + + void add(const EventType & evt) + { + if (comparison_handler_.message_fits(evt)) { + this->signalMessage(evt); + } + } + +private: + Connection incoming_connection_; + + HandlerType comparison_handler_; +}; + + +/** + * \brief ROS 2 Delta filter. + * + * Given a stream of messages, the message is passed down to the next filter + * if any of the comparators returns true when applied to the previously accepted + * message and the current message. + */ +template +class DeltaFilter : public ComparisonFilter +{ +public: + typedef std::shared_ptr MConstPtr; + typedef std::function FieldComparatorFunctionType; + + /** + * \brief Initialize DeltaFilter + * + * \param filter An instance of the 'SimpleFilter' successor class. + * The input filter to connect to. + * + * \param field_comparators A list of callable objects that take the cached and + * current message and return true if the observed fields differ. + * If any comparator returns true the current message is accepted. + */ + template + explicit DeltaFilter( + FilterType & filter, + std::forward_list field_comparators + ) + : ComparisonFilter( + filter, + field_comparators + ) {} + + virtual ~DeltaFilter() = default; +}; + +} // namespace message_filters + +#endif // MESSAGE_FILTERS__DELTA_FILTER_HPP_ diff --git a/include/message_filters/message_event.hpp b/include/message_filters/message_event.hpp new file mode 100644 index 00000000..585426b2 --- /dev/null +++ b/include/message_filters/message_event.hpp @@ -0,0 +1,34 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__MESSAGE_EVENT_HPP_ +#define MESSAGE_FILTERS__MESSAGE_EVENT_HPP_ + +#include "message_event.h" + +#endif // MESSAGE_FILTERS__MESSAGE_EVENT_HPP_ diff --git a/include/message_filters/message_traits.hpp b/include/message_filters/message_traits.hpp new file mode 100644 index 00000000..5b4b8cb2 --- /dev/null +++ b/include/message_filters/message_traits.hpp @@ -0,0 +1,34 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__MESSAGE_TRAITS_HPP_ +#define MESSAGE_FILTERS__MESSAGE_TRAITS_HPP_ + +#include "message_traits.h" + +#endif // MESSAGE_FILTERS__MESSAGE_TRAITS_HPP_ diff --git a/include/message_filters/null_types.hpp b/include/message_filters/null_types.hpp new file mode 100644 index 00000000..2ba27aee --- /dev/null +++ b/include/message_filters/null_types.hpp @@ -0,0 +1,34 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__NULL_TYPES_HPP_ +#define MESSAGE_FILTERS__NULL_TYPES_HPP_ + +#include "null_types.h" + +#endif // MESSAGE_FILTERS__NULL_TYPES_HPP_ diff --git a/include/message_filters/parameter_adapter.hpp b/include/message_filters/parameter_adapter.hpp new file mode 100644 index 00000000..aa66b5f1 --- /dev/null +++ b/include/message_filters/parameter_adapter.hpp @@ -0,0 +1,34 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__PARAMETER_ADAPTER_HPP_ +#define MESSAGE_FILTERS__PARAMETER_ADAPTER_HPP_ + +#include "parameter_adapter.h" + +#endif // MESSAGE_FILTERS__PARAMETER_ADAPTER_HPP_ diff --git a/include/message_filters/pass_through.hpp b/include/message_filters/pass_through.hpp new file mode 100644 index 00000000..2cffaa8d --- /dev/null +++ b/include/message_filters/pass_through.hpp @@ -0,0 +1,34 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__PASS_THROUGH_HPP_ +#define MESSAGE_FILTERS__PASS_THROUGH_HPP_ + +#include "pass_through.h" + +#endif // MESSAGE_FILTERS__PASS_THROUGH_HPP_ diff --git a/include/message_filters/signal1.hpp b/include/message_filters/signal1.hpp new file mode 100644 index 00000000..8b0da284 --- /dev/null +++ b/include/message_filters/signal1.hpp @@ -0,0 +1,34 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SIGNAL1_HPP_ +#define MESSAGE_FILTERS__SIGNAL1_HPP_ + +#include "signal1.h" + +#endif // MESSAGE_FILTERS__SIGNAL1_HPP_ diff --git a/include/message_filters/signal9.hpp b/include/message_filters/signal9.hpp new file mode 100644 index 00000000..0b3cf07c --- /dev/null +++ b/include/message_filters/signal9.hpp @@ -0,0 +1,34 @@ +// Copyright 2010, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SIGNAL9_HPP_ +#define MESSAGE_FILTERS__SIGNAL9_HPP_ + +#include "signal9.h" + +#endif // MESSAGE_FILTERS__SIGNAL9_HPP_ diff --git a/include/message_filters/simple_filter.hpp b/include/message_filters/simple_filter.hpp new file mode 100644 index 00000000..64e08373 --- /dev/null +++ b/include/message_filters/simple_filter.hpp @@ -0,0 +1,34 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SIMPLE_FILTER_HPP_ +#define MESSAGE_FILTERS__SIMPLE_FILTER_HPP_ + +#include "simple_filter.h" + +#endif // MESSAGE_FILTERS__SIMPLE_FILTER_HPP_ diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index dd4d9efe..bb17c706 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -109,11 +109,7 @@ class SubscriberBase const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) { - (void) options; - RCLCPP_ERROR( - node->get_logger(), - "SubscriberBase::subscribe with four arguments has not been overridden"); - this->subscribe(node, topic, qos); + this->subscribe(node, topic, qos, options); } /** diff --git a/include/message_filters/subscriber.hpp b/include/message_filters/subscriber.hpp new file mode 100644 index 00000000..fbce0c1a --- /dev/null +++ b/include/message_filters/subscriber.hpp @@ -0,0 +1,34 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SUBSCRIBER_HPP_ +#define MESSAGE_FILTERS__SUBSCRIBER_HPP_ + +#include "subscriber.h" + +#endif // MESSAGE_FILTERS__SUBSCRIBER_HPP_ diff --git a/include/message_filters/sync_policies/approximate_epsilon_time.hpp b/include/message_filters/sync_policies/approximate_epsilon_time.hpp new file mode 100644 index 00000000..a98af1a9 --- /dev/null +++ b/include/message_filters/sync_policies/approximate_epsilon_time.hpp @@ -0,0 +1,3 @@ +#pragma once + +#include "approximate_epsilon_time.h" diff --git a/include/message_filters/sync_policies/approximate_time.hpp b/include/message_filters/sync_policies/approximate_time.hpp new file mode 100644 index 00000000..701b439c --- /dev/null +++ b/include/message_filters/sync_policies/approximate_time.hpp @@ -0,0 +1,3 @@ +#pragma once + +#include "approximate_time.h" diff --git a/include/message_filters/sync_policies/exact_time.hpp b/include/message_filters/sync_policies/exact_time.hpp new file mode 100644 index 00000000..0a36b6cf --- /dev/null +++ b/include/message_filters/sync_policies/exact_time.hpp @@ -0,0 +1,3 @@ +#pragma once + +#include "exact_time.h" diff --git a/include/message_filters/sync_policies/latest_time.h b/include/message_filters/sync_policies/latest_time.h new file mode 100644 index 00000000..95d4b52b --- /dev/null +++ b/include/message_filters/sync_policies/latest_time.h @@ -0,0 +1,396 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2022, Open Source Robotics Foundation, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/** + * \brief Synchronizes up to 9 messages by their rates with upsampling via zero-order-hold. + * + * LatestTime policy synchronizes up to 9 incoming channels by the rates they are received. + * The callback with all the messages will be triggered whenever the fastest message is received. + * The slower messages will be repeated at the rate of the fastest message and will be updated + * whenever a new one is received. This is essentially an upsampling of slower messages using a + * zero-order hold (no interpolation). + + * \section usage USAGE + * Example usage would be: +\verbatim +typedef LatestTime latest_policy; +Synchronizer sync_policies(latest_policy(), caminfo_sub, limage_sub, rimage_sub); +sync_policies.registerCallback(callback); +\endverbatim + + * May also take an instance of a `rclcpp::Clock::SharedPtr` from `rclpp::Node::get_clock()` + * to use the node's time source (e.g. sim time) as in: +\verbatim +typedef LatestTime latest_policy; +Synchronizer sync_policies(latest_policy(node->get_clock()), caminfo_sub, limage_sub, rimage_sub); +sync_policies.registerCallback(callback); +\endverbatim + + * The callback is then of the form: +\verbatim +void callback(const sensor_msgs::CameraInfo::ConstPtr&, const sensor_msgs::Image::ConstPtr&, const sensor_msgs::Image::ConstPtr&); +\endverbatim + * + */ + +#ifndef MESSAGE_FILTERS__SYNC_LATEST_TIME_H_ +#define MESSAGE_FILTERS__SYNC_LATEST_TIME_H_ + +#include +#include +#include +#include +#include + +#include + +#include "message_filters/message_traits.h" +#include "message_filters/null_types.h" +#include "message_filters/signal9.h" +#include "message_filters/synchronizer.h" + + +namespace message_filters +{ +namespace sync_policies +{ + +template +struct LatestTime : public PolicyBase +{ + typedef Synchronizer Sync; + typedef PolicyBase Super; + typedef typename Super::Messages Messages; + typedef typename Super::Signal Signal; + typedef typename Super::Events Events; + typedef typename Super::RealTypeCount RealTypeCount; + + /// \brief filter coeffs and error margin factor: + /// + typedef std::tuple RateConfig; + + LatestTime() + : LatestTime(rclcpp::Clock::SharedPtr(new rclcpp::Clock(RCL_ROS_TIME))) + { + } + + LatestTime(rclcpp::Clock::SharedPtr clock) + : parent_(0), + ros_clock_{clock} + { + } + + LatestTime(const LatestTime& e) + { + *this = e; + } + + LatestTime & operator=(const LatestTime & rhs) + { + parent_ = rhs.parent_; + events_ = rhs.events_; + rates_ = rhs.rates_; + ros_clock_ = rhs.ros_clock_; + + return *this; + } + + void initParent(Sync * parent) + { + parent_ = parent; + } + + void setRateConfigPerMessage(const std::vector & configs) + { + rate_configs_.assign(configs.begin(), configs.end()); + } + + void setRateConfig(const RateConfig & config) + { + rate_configs_.assign(1U, config); + } + + void setClock(rclcpp::Clock::SharedPtr clock) + { + ros_clock_ = clock; + } + + template + void add(const typename std::tuple_element::type & evt) + { + RCUTILS_ASSERT(parent_); + + std::lock_guard lock(data_mutex_); + + if(!received_msg()) + { + initialize_rate(); + // wait until we get each message once to publish + // then wait until we got each message twice to compute rates + // NOTE: this will drop a few messages of the faster topics until + // we get one of the slowest so we can sync + std::get(events_) = evt; // adding here ensures we see even the slowest + // message twice before computing rate + return; + } + + std::get(events_) = evt; + rclcpp::Time now = ros_clock_->now(); + bool valid_rate = rates_[i].compute_hz(now); + if(valid_rate && (i == find_pivot(now)) && is_full()) + { + publish(); + } + } + +private: + // assumed data_mutex_ is locked + template + void initialize_rate() + { + if (rate_configs_.size() > 0U) { + double rate_ema_alpha{Rate::DEFAULT_RATE_EMA_ALPHA}; + double error_ema_alpha{Rate::DEFAULT_ERROR_EMA_ALPHA}; + double rate_step_change_margin_factor{Rate::DEFAULT_MARGIN_FACTOR}; + if (rate_configs_.size() == RealTypeCount::value) { + std::tie ( + rate_ema_alpha, + error_ema_alpha, + rate_step_change_margin_factor) = rate_configs_[i]; + } else if (rate_configs_.size() == 1U) { + std::tie ( + rate_ema_alpha, + error_ema_alpha, + rate_step_change_margin_factor) = rate_configs_[0U]; + } + rates_.push_back( + Rate( + ros_clock_->now(), + rate_ema_alpha, + error_ema_alpha, + rate_step_change_margin_factor)); + } else { + rates_.push_back(Rate(ros_clock_->now())); + } + } + + // assumed data_mutex_ is locked + void publish() + { + parent_->signal(std::get<0>(events_), std::get<1>(events_), std::get<2>(events_), + std::get<3>(events_), std::get<4>(events_), std::get<5>(events_), + std::get<6>(events_), std::get<7>(events_), std::get<8>(events_)); + } + + struct Rate + { + static constexpr double DEFAULT_RATE_EMA_ALPHA{0.9}; + static constexpr double DEFAULT_ERROR_EMA_ALPHA{0.3}; + static constexpr double DEFAULT_MARGIN_FACTOR{10.0}; + + rclcpp::Time prev; + double hz{0.0}; + double error{0.0}; + double rate_ema_alpha{DEFAULT_RATE_EMA_ALPHA}; + double error_ema_alpha{DEFAULT_ERROR_EMA_ALPHA}; + double rate_step_change_margin_factor{DEFAULT_MARGIN_FACTOR}; + bool do_hz_init{true}; + bool do_error_init{true}; + + Rate(const rclcpp::Time & start) + : Rate(start, DEFAULT_RATE_EMA_ALPHA, DEFAULT_ERROR_EMA_ALPHA, DEFAULT_MARGIN_FACTOR) + { + } + + Rate(const rclcpp::Time & start, + const double & rate_ema_alpha, const double & error_ema_alpha, + const double & rate_step_change_margin_factor) + : prev{start}, + rate_ema_alpha{rate_ema_alpha}, + error_ema_alpha{error_ema_alpha}, + rate_step_change_margin_factor{rate_step_change_margin_factor} + { + } + + bool operator>(const Rate & that) const + { + return this->hz > that.hz; + } + + bool compute_hz(const rclcpp::Time & now) + { + bool step_change_detected = false; + do { + double period = 0.0; + try { + period = (now-prev).seconds(); + } catch (const std::runtime_error & /*e*/) { + // Different time sources that might happen on initialization if the messages are not yet available. + // std::cout << "Exception: " << e.what() << std::endl; + return false; + } + if (period <= 0.0) { + // multiple messages and time isn't updating + return false; + } + + if (do_hz_init) { + hz = 1.0/period; + do_hz_init = false; + step_change_detected = false; + } else { + if (do_error_init) { + error = fabs(hz - 1.0/period); + do_error_init = false; + } else { + // check if rate is some multiple of mean error from mean + if (fabs(hz - 1.0/period) > rate_step_change_margin_factor*error) { + // detected step change in rate so reset + do_hz_init = true; + do_error_init = true; + step_change_detected = true; + continue; + } + // on-line mean error from mean + error = error_ema_alpha*fabs(hz - 1.0/period) + (1.0 - error_ema_alpha)*error; + } + hz = rate_ema_alpha/period + (1.0 - rate_ema_alpha)*hz; + } + } while (step_change_detected); + prev = now; + return true; + } + }; + + // assumed data_mutex_ is locked + template + std::vector sort_indices(const std::vector & v) + { + // initialize original index locations + std::vector idx(v.size()); + std::iota(idx.begin(), idx.end(), 0U); + + // sort indexes based on comparing values in v + // using std::stable_sort instead of std::sort + // to avoid unnecessary index re-orderings + // when v contains elements of equal values + std::stable_sort(idx.begin(), idx.end(), + [&v](std::size_t i1, std::size_t i2) {return v[i1] > v[i2];}); + + return idx; + } + + // assumed data_mutex_ is locked + template + bool received_msg() + { + return (RealTypeCount::value > i ? (bool)std::get(events_).getMessage() : true); + } + + // assumed data_mutex_ is locked + bool is_full() + { + bool full = received_msg<0>(); + full = full && received_msg<1>(); + full = full && received_msg<2>(); + full = full && received_msg<3>(); + full = full && received_msg<4>(); + full = full && received_msg<5>(); + full = full && received_msg<6>(); + full = full && received_msg<7>(); + full = full && received_msg<8>(); + + return full; + } + + // assumed data_mutex_ is locked + int find_pivot(const rclcpp::Time & now) + { + // find arg max rate + std::vector sorted_idx = sort_indices(rates_); + + // use fastest message that isn't late as pivot + for (size_t pivot : sorted_idx) { + double period = (now-rates_[pivot].prev).seconds(); + if (period == 0.0) { + if (rates_[pivot].hz > 0.0) { + // we just updated updated this one, + // and it's fastest, so use as pivot + return static_cast(pivot); + } else { + // haven't calculated rate for this message yet + continue; + } + } + + if (!rates_[pivot].do_error_init) { + // can now check if new messages are late + double rate_delta = rates_[pivot].hz - 1.0/period; + double margin = rates_[pivot].rate_step_change_margin_factor * rates_[pivot].error; + if (rate_delta > margin) { + // this pivot is late + continue; + } + } + + if (rates_[pivot].hz > 0.0) { + // found fastest message with a calculated rate + return static_cast(pivot); + } else { + // haven't calculated rate for this message yet + continue; + } + } + return NO_PIVOT; + } + + Sync* parent_; + Events events_; + std::vector rates_; + std::mutex data_mutex_; // Protects all of the above + + std::vector rate_configs_; + + const int NO_PIVOT{9}; + + rclcpp::Clock::SharedPtr ros_clock_{nullptr}; +}; + +} // namespace sync +} // namespace message_filters + +#endif // MESSAGE_FILTERS__SYNC_LATEST_TIME_H_ diff --git a/include/message_filters/sync_policies/latest_time.hpp b/include/message_filters/sync_policies/latest_time.hpp new file mode 100644 index 00000000..e8ae0365 --- /dev/null +++ b/include/message_filters/sync_policies/latest_time.hpp @@ -0,0 +1,3 @@ +#pragma once + +#include "latest_time.h" diff --git a/include/message_filters/synchronizer.hpp b/include/message_filters/synchronizer.hpp new file mode 100644 index 00000000..51e24570 --- /dev/null +++ b/include/message_filters/synchronizer.hpp @@ -0,0 +1,34 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__SYNCHRONIZER_HPP_ +#define MESSAGE_FILTERS__SYNCHRONIZER_HPP_ + +#include "synchronizer.h" + +#endif // MESSAGE_FILTERS__SYNCHRONIZER_HPP_ diff --git a/include/message_filters/time_sequencer.hpp b/include/message_filters/time_sequencer.hpp new file mode 100644 index 00000000..53d76139 --- /dev/null +++ b/include/message_filters/time_sequencer.hpp @@ -0,0 +1,34 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__TIME_SEQUENCER_HPP_ +#define MESSAGE_FILTERS__TIME_SEQUENCER_HPP_ + +#include "time_sequencer.h" + +#endif // MESSAGE_FILTERS__TIME_SEQUENCER_HPP_ diff --git a/include/message_filters/time_synchronizer.hpp b/include/message_filters/time_synchronizer.hpp new file mode 100644 index 00000000..a88597c9 --- /dev/null +++ b/include/message_filters/time_synchronizer.hpp @@ -0,0 +1,34 @@ +// Copyright 2009, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MESSAGE_FILTERS__TIME_SYNCHRONIZER_HPP_ +#define MESSAGE_FILTERS__TIME_SYNCHRONIZER_HPP_ + +#include "time_synchronizer.h" + +#endif // MESSAGE_FILTERS__TIME_SYNCHRONIZER_HPP_ diff --git a/include/message_filters/visibility_control.hpp b/include/message_filters/visibility_control.hpp new file mode 100644 index 00000000..d984e984 --- /dev/null +++ b/include/message_filters/visibility_control.hpp @@ -0,0 +1,20 @@ +// Copyright 2018 Open Source Robotics Foundation, 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. + +#ifndef MESSAGE_FILTERS__VISIBILITY_CONTROL_HPP_ +#define MESSAGE_FILTERS__VISIBILITY_CONTROL_HPP_ + +#include "visibility_control.h" + +#endif // MESSAGE_FILTERS__VISIBILITY_CONTROL_HPP_ diff --git a/index.rst b/index.rst deleted file mode 100644 index 9273421e..00000000 --- a/index.rst +++ /dev/null @@ -1,73 +0,0 @@ -Message Filters --- chained message processing -============================================== - -:mod:`message_filters` is a collection of message "filters" which take messages in, -either from a ROS subscription or another filter, -and may or may not output the message -at some time in the future, depending on a policy defined for that filter. - -message_filters also defines a common interface for these filters, allowing you to chain them together. - -The filters currently implemented in this package are: - - * :class:`message_filters.Subscriber` - A source filter, which wraps a ROS subscription. Most filter chains will begin with a Subscriber. - * :class:`message_filters.Cache` - Caches messages which pass through it, allowing later lookup by time stamp. - * :class:`message_filters.TimeSynchronizer` - Synchronizes multiple messages by their timestamps, only passing them through when all have arrived. - * :class:`message_filters.TimeSequencer` - Tries to pass messages through ordered by their timestamps, even if some arrive out of order. - -Here's a simple example of using a Subscriber with a Cache:: - - def myCallback(posemsg): - print posemsg - - sub = message_filters.Subscriber("pose_topic", robot_msgs.msg.Pose) - cache = message_filters.Cache(sub, 10) - cache.registerCallback(myCallback) - -The Subscriber here acts as the source of messages. Each message is passed to the cache, which then passes it through to the -user's callback ``myCallback``. - - -Using the time synchronizer:: - - from message_filters import TimeSynchronizer, Subscriber - - def gotimage(image, camerainfo): - assert image.header.stamp == camerainfo.header.stamp - print "got an Image and CameraInfo" - - tss = TimeSynchronizer(Subscriber("/wide_stereo/left/image_rect_color", sensor_msgs.msg.Image), - Subscriber("/wide_stereo/left/camera_info", sensor_msgs.msg.CameraInfo)) - tss.registerCallback(gotimage) - - -The message filter interface ----------------------------- - -For an object to be usable as a message filter, it needs to have one method, -``registerCallback``. To collect messages from a message filter, register a callback with:: - - anyfilter.registerCallback(my_callback) - -The signature of ``my_callback`` varies according to the message filter. For many filters it is simply:: - - def my_callback(msg): - -where ``msg`` is the message. - -Message filters that accept input from an upstream -message filter (e.g. :class:`message_filters.Cache`) register their own -message handler as a callback. - -Output connections are registered through the ``registerCallback()`` function. - -.. automodule:: message_filters - :members: Subscriber, Cache, TimeSynchronizer, TimeSequencer - :inherited-members: - -Indices and tables -================== - -* :ref:`genindex` -* :ref:`search` - diff --git a/package.xml b/package.xml index 8e57b4b8..0b0afab5 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ message_filters - 4.3.1 + 4.3.16 A set of ROS2 message filters which take in messages and may output those messages at a later time, based on the conditions that filter needs met. @@ -11,7 +11,7 @@ Geoffrey Biggs BSD - https://github.com/intel/ros2_message_filters + https://github.com/ros2/message_filters Dirk Thomas Jing Wang @@ -39,5 +39,6 @@ ament_cmake + rosdoc2.yaml diff --git a/rosdoc.yaml b/rosdoc.yaml deleted file mode 100644 index d243f31a..00000000 --- a/rosdoc.yaml +++ /dev/null @@ -1,7 +0,0 @@ - - builder: sphinx - name: Python API - output_dir: python - - builder: doxygen - name: C++ API - output_dir: c++ - file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' diff --git a/rosdoc2.yaml b/rosdoc2.yaml new file mode 100644 index 00000000..2ea2a9ba --- /dev/null +++ b/rosdoc2.yaml @@ -0,0 +1,67 @@ +## Default configuration, generated by rosdoc2. + +## This 'attic section' self-documents this file's type and version. +type: 'rosdoc2 config' +version: 1 + +--- + +settings: + ## If this is true, a standard index page is generated in the output directory. + ## It uses the package information from the 'package.xml' to show details + ## about the package, creates a table of contents for the various builders + ## that were run, and may contain links to things like build farm jobs for + ## this package or links to other versions of this package. + + ## If false, you can still include content that would have been in the index + ## into one of your '.rst' files from your Sphinx project, using the + ## '.. include::' directive in Sphinx. + ## For example, you could include it in a custom 'index.rst' so you can have + ## the standard information followed by custom content. + + ## TODO(wjwwood): provide a concrete example of this (relative path?) + + ## If this is not specified explicitly, it defaults to 'true'. + generate_package_index: true + + ## This setting is relevant mostly if the standard Python package layout cannot + ## be assumed for 'sphinx-apidoc' invocation. The user can provide the path + ## (relative to the 'package.xml' file) where the Python modules defined by this + ## package are located. + python_source: 'message_filters' + + ## This setting, if true, attempts to run `doxygen` and the `breathe`/`exhale` + ## extensions to `sphinx` regardless of build type. This is most useful if the + ## user would like to generate C/C++ API documentation for a package that is not + ## of the `ament_cmake/cmake` build type. + always_run_doxygen: false + + ## This setting, if true, attempts to run `sphinx-apidoc` regardless of build + ## type. This is most useful if the user would like to generate Python API + ## documentation for a package that is not of the `ament_python` build type. + always_run_sphinx_apidoc: false + + # This setting, if provided, will override the build_type of this package + # for documentation purposes only. If not provided, documentation will be + # generated assuming the build_type in package.xml. + # override_build_type: 'ament_python' +builders: + ## Each stanza represents a separate build step, performed by a specific 'builder'. + ## The key of each stanza is the builder to use; this must be one of the + ## available builders. + ## The value of each stanza is a dictionary of settings for the builder that + ## outputs to that directory. + ## Required keys in the settings dictionary are: + ## * 'output_dir' - determines output subdirectory for builder instance + ## relative to --output-directory + ## * 'name' - used when referencing the built docs from the index. + + - doxygen: { + name: 'message_filters Public C/C++ API', + output_dir: 'generated/doxygen' + } + - sphinx: { + name: 'message_filters', + doxygen_xml_directory: 'generated/doxygen/xml', + output_dir: '' + } diff --git a/src/message_filters/__init__.py b/src/message_filters/__init__.py index 2ed01093..fe469b02 100644 --- a/src/message_filters/__init__.py +++ b/src/message_filters/__init__.py @@ -30,17 +30,24 @@ ====================== """ +"""Message Filter Objects.""" + from functools import reduce import itertools import threading import rclpy -import builtin_interfaces +from typing import Type, Union + from rclpy.clock import ROSClock from rclpy.duration import Duration from rclpy.logging import LoggingSeverity +from rclpy.node import Node +from rclpy.qos import QoSProfile from rclpy.time import Time +from .input_aligner import InputAligner, QueueStatus + class SimpleFilter(object): @@ -64,7 +71,7 @@ def signalMessage(self, *msg): cb(*(msg + args)) class Subscriber(SimpleFilter): - + """ ROS2 subscription filter,Identical arguments as :class:`rclpy.Subscriber`. @@ -72,12 +79,37 @@ class Subscriber(SimpleFilter): from a ROS2 subscription through to the filters which have connected to it. """ - def __init__(self, *args, **kwargs): + + def __init__( + self, + node: Node, + msg_type: Type, + topic: str, + qos_profile: Union[QoSProfile, int] = QoSProfile(depth=10), + **kwargs, + ) -> None: + """ + Construct a Subscriber. + + :param node: The node to create a subscriber for. + :param msg_type: The type of ROS messages the subscription will subscribe to. + :param topic: The name of the topic the subscription will subscribe to. + :param qos_profile: A QoSProfile or a history depth to apply to the + subscription. In the case that a history depth is provided, the QoS history is + set to KEEP_LAST, the QoS history depth is set to the value of the parameter, + and all other QoS settings are set to their default values. + :param kwargs: Additional keyword arguments passed to node.create_subscription. + """ SimpleFilter.__init__(self) - self.node = args[0] - self.topic = args[2] - kwargs.setdefault('qos_profile', 10) - self.sub = self.node.create_subscription(*args[1:], self.callback, **kwargs) + self.node = node + self.topic = topic + self.sub = self.node.create_subscription( + msg_type=msg_type, + topic=self.topic, + callback=self.callback, + qos_profile=qos_profile, + **kwargs, + ) def callback(self, msg): self.signalMessage(msg) @@ -183,13 +215,80 @@ def getOldestTime(self): if not self.cache_times: return None return self.cache_times[0] - + def getLast(self): if self.getLastestTime() is None: return None return self.getElemAfterTime(self.getLastestTime()) +class Chain(SimpleFilter): + """ + Chains a dynamic number of simple filters together. + + Allows retrieval of filters by index after they are added. + + The Chain filter provides a container for simple filters. + It allows you to store an N-long set of filters inside a single + structure, making it much easier to manage them. + + Adding filters to the chain is done by adding shared_ptrs of them + to the filter. They are automatically connected to each other + and the output of the last filter in the chain is forwarded + to the callback you've registered with Chain::registerCallback. + """ + + class FilterInfo: + def __init__( + self, + message_filter: any, + connection_callback_index: int, + ): + self.message_filter = message_filter + self.connection_callback_index = connection_callback_index + + def __init__(self, message_filter=None): + SimpleFilter.__init__(self) + + self.incoming_connection = None + + if message_filter is not None: + self.connectInput(message_filter) + + self._message_filters: dict[int, Chain.FilterInfo] = {} + + def connectInput(self, message_filter): + if self.incoming_connection is not None: + raise RuntimeError('Already connected') + self.incoming_connection = message_filter.registerCallback(self.add) + + def add(self, message): + if self._message_filters: + self._message_filters[0].message_filter.add(message) + else: + self.signalMessage(message) + + def addFilter(self, message_filter): + new_filter_index = len(self._message_filters) + last_filter_index = new_filter_index - 1 + + self._message_filters[new_filter_index] = Chain.FilterInfo( + message_filter=message_filter, + connection_callback_index=message_filter.registerCallback(self.signalMessage), + ) + + if last_filter_index >= 0: + last_filter = self._message_filters[last_filter_index].message_filter + callback_index = self._message_filters[last_filter_index].connection_callback_index + last_filter.callbacks.pop(callback_index) + + self._message_filters[last_filter_index].connection_callback_index = \ + last_filter.registerCallback(message_filter.add) + + def getFilter(self, index: int): + return self._message_filters[index].message_filter + + class TimeSynchronizer(SimpleFilter): """ @@ -231,12 +330,25 @@ def add(self, msg, my_queue, my_queue_index=None): del my_queue[min(my_queue)] # common is the set of timestamps that occur in all queues common = reduce(set.intersection, [set(q) for q in self.queues]) + signaled_time = None for t in sorted(common): # msgs is list of msgs (one from each queue) with stamp t msgs = [q[t] for q in self.queues] self.signalMessage(*msgs) for q in self.queues: del q[t] + signaled_time = t + break + + # for consistency with the C++ implementation: + # Delete all stored messages with a timestamp + # older than the time of the latest signaled time + if signaled_time is not None: + for queue in self.queues: + for stamp in list(queue.keys()): + if stamp < signaled_time: + del queue[stamp] + self.lock.release() class ApproximateTimeSynchronizer(TimeSynchronizer): diff --git a/src/message_filters/delta_filter.py b/src/message_filters/delta_filter.py new file mode 100644 index 00000000..981b00f2 --- /dev/null +++ b/src/message_filters/delta_filter.py @@ -0,0 +1,245 @@ +# Copyright 2026, Open Source Robotics Foundation, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +from abc import ABC +from functools import partial +from typing import Any, Callable + +from message_filters import SimpleFilter +from rclpy.type_support import MsgT + + +class ComparisonHandler(ABC): + + def message_fits(self, message: MsgT) -> bool: + raise NotImplementedError() + + def do_fields_fit(self, field_a: Any, field_b: Any) -> bool: + raise NotImplementedError() + + +class CachedComparisonHandler(ComparisonHandler): + """ + Implements cached messages comparison field by field. + + :class: CachedComparisonHandler implements messages comparison field by field. + A successor to this class should implement ``do_fields_fit`` method. + If any of the fields provided to the ``do_fields_fit`` method do satisfy + a comparison conditions the message is accepted. That means that the message + is stored in the ``_message_cache`` and the ``message_fits`` method returns ``True``. + """ + + def __init__(self): + super().__init__() + + self._message_cache: MsgT | None = None + + def message_fits(self, message: MsgT) -> bool: + if self._message_cache is None: + self._message_cache = message + return True + + any_field_fits = any( + self.do_fields_fit( + get_field(self._message_cache), + get_field(message), + ) + for get_field in self._field_getters + ) + + if any_field_fits: + self._message_cache = message + return any_field_fits + + def do_fields_fit(self, field_a: Any, field_b: Any) -> bool: + raise NotImplementedError( + f'Field comparison not implemented for class: {self.__class__.__name__}. ' + f'Consider implementing do_fields_fit' + ) + + +class DeltaCompare(CachedComparisonHandler): + """ + Implements delta comparison. + + If any of the fields, acquired by any of the ``field_getters`` do differ + between previously cached and current message, the current message is accepted. + """ + + _basic_types = ( + bool, + int, + float, + str, + # list requires a specific comparison function + ) + + def __init__( + self, + field_getters: Callable[[MsgT], Any], + ): + """ + Construct DeltaCompare. + + :param field_getters: A list of callable objects that are expected + to retrieve a value of a basic type from a message field for comparison. + Any of the ``field_getters`` is applied to the cached and to the current + message. The returned values are compared. If any of these values differ + between cached and current message, the current message is accepted. + """ + super().__init__() + + self._field_getters = field_getters + + def do_fields_fit(self, val_a, val_b) -> bool: + if any(not isinstance(val, self._basic_types) for val in [val_a, val_b]): + raise TypeError(f'Unable to compare fields of types {type(val_a)} and {type(val_b)}') + return val_a != val_b + + +class ComparisonFilter(SimpleFilter): + """ + ROS 2 Comparison filter. + + Given a stream of messages, the message is passed down to the next filter + if ``comparison_handler`` ``message_fits`` method returns ``True`` for that message. + """ + + def __init__( + self, + comparison_handler: CachedComparisonHandler, + message_filter: SimpleFilter | None = None, + ): + """ + Construct ComparisonFilter. + + :param comparison_handler: An instance of the CachedComparisonHandler class. + Is expected implement a ``message_fits`` method. If ``message_fits`` returns ``True`` + for a provided message, that message is considered valid and is passed + to a next filter if any. + :param message_filter: An instance of the ``SimpleFilter``. The input filter to connect to + """ + super().__init__() + + self._comparison_handler = comparison_handler + + self.incoming_connection = None + self.connectInput(message_filter) + + def add(self, message: MsgT): + if self.message_fits(message): + self.signalMessage(message) + + def message_fits(self, message: MsgT): + return self._comparison_handler.message_fits(message) + + def connectInput(self, message_filter): + if self.incoming_connection is not None: + raise RuntimeError( + 'This instance of ComparisonFilter already has an incoming connection' + ) + if message_filter is not None: + self.incoming_connection = message_filter.registerCallback(self.add) + + +class DeltaFilter(ComparisonFilter): + """ + ROS 2 Delta filter. + + Given a stream of messages, the message is passed down to the next filter + if any of the message fields, that may be acquired by ``field_getters`` + have changed compared to the previously accepted message. + """ + + def __init__( + self, + field_getters: list[Callable[[MsgT], Any]], + message_filter: SimpleFilter | None = None, + ): + """ + Construct a DeltaFilter. + + :param field_getters: A list of callable objects each of which returns a field value. + When message is processed, every field getter is applied to it and to the previously + accepted message. If any of the fields have changed between currently processed message + and previous value, the message is accepted, stored to the ``message_cache`` and passed + to a next message filter if any. + :param message_filter: A message filter to connect this filter to as a follow up. + """ + super().__init__( + comparison_handler=DeltaCompare(field_getters=field_getters), + message_filter=message_filter, + ) + + +class PathDeltaFilter(DeltaFilter): + """ + ROS 2 Path Delta Filter. + + Given a stream of messages, the message is passed down to the next filter + if any of the message fields, that may be found at any ``field_path`` + of the ``field_path_list`` have changed compared to the previously accepted message. + """ + + def __init__( + self, + field_path_list: list[str], + message_filter: SimpleFilter | None = None + ): + """ + Construct a DeltaFilter. + + :param field_path_list: A list of field paths to access fields by. + A field path is expected to be a dot-separated string. Eg: + ``a.b.c.d`` where ``d`` is a field of ``c``, ``c`` is a field of ``b`` e.t.c. + :param message_filter: A message filter to connect this filter to as a follow up. + """ + super().__init__( + field_getters=[ + partial( + self.get_field_by_path, + field_path=field_path, + ) + for field_path in field_path_list + ], + message_filter=message_filter, + ) + + def get_field_by_path(self, message: MsgT, field_path: str): + res = message + + for field_name in field_path.split('.'): + prev = res + res = getattr(res, field_name, None) + if res is None: + raise RuntimeError( + f'Unable to access field {field_name!r} of ' + f'{prev.__class__.__name__} (full path: {field_path!r})' + ) + return res diff --git a/src/message_filters/input_aligner.py b/src/message_filters/input_aligner.py new file mode 100644 index 00000000..fa54b334 --- /dev/null +++ b/src/message_filters/input_aligner.py @@ -0,0 +1,140 @@ +from bisect import insort_right +from dataclasses import dataclass +import threading + +from rclpy.duration import Duration +from rclpy.time import Time + + +@dataclass +class QueueStatus: + active: bool + queue_size: int + msgs_processed: int + msgs_dropped: int + + +class _Signal: + def __init__(self): + self.callbacks = {} + + def registerCallback(self, cb, *args): + conn = len(self.callbacks) + self.callbacks[conn] = (cb, args) + return conn + + def signalMessage(self, *msg): + for (cb, args) in self.callbacks.values(): + cb(*(msg + args)) + + +class _EventQueue: + def __init__(self): + self.events = [] + self.next_ts = Time(seconds=9223372036) + self.period = Duration(seconds=0) + self.active = False + self.msgs_processed = 0 + self.msgs_dropped = 0 + + def first_timestamp(self): + if self.events: + first_ts = self.events[0][0] + self.next_ts = first_ts + self.period + self.active = True + return first_ts + if self.active: + return self.next_ts + return Time(seconds=9223372036) + + def pop_first(self): + self.events.pop(0) + self.msgs_processed += 1 + + def msg_dropped(self): + self.msgs_dropped += 1 + + def set_period(self, period): + self.period = period + + def set_active(self, active): + self.active = active + + def get_status(self): + return QueueStatus(self.active, len(self.events), self.msgs_processed, self.msgs_dropped) + + +class InputAligner: + def __init__(self, timeout, *filters): + self.timeout = timeout + self.last_in_ts = Time(seconds=0) + self.last_out_ts = Time(seconds=0) + self.name = '' + self.lock = threading.Lock() + self.event_queues = [] + self.input_connections = [] + self.signals = [] + self.dispatch_timer = None + if filters: + self.connectInput(*filters) + + def connectInput(self, *filters): + self.disconnectAll() + self.event_queues = [_EventQueue() for _ in filters] + self.signals = [_Signal() for _ in filters] + self.input_connections = [f.registerCallback(self.add, idx) for idx, f in enumerate(filters)] + + def disconnectAll(self): + self.input_connections = [] + + def registerCallback(self, index, cb, *args): + return self.signals[index].registerCallback(cb, *args) + + def setName(self, name): + self.name = name + + def getName(self): + return self.name + + def add(self, msg, queue_index): + msg_timestamp = Time.from_msg(msg.header.stamp) + with self.lock: + queue = self.event_queues[queue_index] + if msg_timestamp < self.last_out_ts: + queue.msg_dropped() + return + if msg_timestamp > self.last_in_ts: + self.last_in_ts = msg_timestamp + insort_right(queue.events, (msg_timestamp, msg), key=lambda x: x[0].nanoseconds) + + def setInputPeriod(self, index, period): + self.event_queues[index].set_period(period) + + def getQueueStatus(self, index): + return self.event_queues[index].get_status() + + def setupDispatchTimer(self, node, update_rate): + self.dispatch_timer = node.create_timer(update_rate.nanoseconds / 1e9, self.dispatchMessages) + + def dispatchMessages(self): + with self.lock: + if not any(queue.events for queue in self.event_queues): + return + input_available = True + while input_available: + input_available = self._dispatch_first_message() + + def _dispatch_first_message(self): + timestamps = [queue.first_timestamp() for queue in self.event_queues] + idx = min(range(len(timestamps)), key=lambda i: timestamps[i].nanoseconds) + queue = self.event_queues[idx] + if queue.events: + stamp, msg = queue.events[0] + self.last_out_ts = stamp + self.signals[idx].signalMessage(msg) + queue.pop_first() + return True + if (self.last_in_ts - queue.first_timestamp()) >= self.timeout: + queue.set_active(False) + return True + return False diff --git a/test/directed.py b/test/directed.py deleted file mode 100644 index f77b1ed1..00000000 --- a/test/directed.py +++ /dev/null @@ -1,80 +0,0 @@ -# image_transport::SubscriberFilter wide_left; // "/wide_stereo/left/image_raw" -# image_transport::SubscriberFilter wide_right; // "/wide_stereo/right/image_raw" -# message_filters::Subscriber wide_left_info; // "/wide_stereo/left/camera_info" -# message_filters::Subscriber wide_right_info; // "/wide_stereo/right/camera_info" -# message_filters::TimeSynchronizer wide; -# -# PersonDataRecorder() : -# wide_left(nh_, "/wide_stereo/left/image_raw", 10), -# wide_right(nh_, "/wide_stereo/right/image_raw", 10), -# wide_left_info(nh_, "/wide_stereo/left/camera_info", 10), -# wide_right_info(nh_, "/wide_stereo/right/camera_info", 10), -# wide(wide_left, wide_left_info, wide_right, wide_right_info, 4), -# -# wide.registerCallback(boost::bind(&PersonDataRecorder::wideCB, this, _1, _2, _3, _4)); - -import rclpy -import random -import unittest - -from builtin_interfaces.msg import Time as TimeMsg -from message_filters import SimpleFilter, Subscriber, Cache, TimeSynchronizer - - -class MockHeader: - pass - -class MockMessage: - def __init__(self, stamp, data): - self.header = MockHeader() - self.header.stamp = TimeMsg(sec=stamp) - self.data = data - -class MockFilter(SimpleFilter): - pass - -class TestDirected(unittest.TestCase): - - def cb_collector_2msg(self, msg1, msg2): - self.collector.append((msg1, msg2)) - - def test_synchronizer(self): - m0 = MockFilter() - m1 = MockFilter() - ts = TimeSynchronizer([m0, m1], 1) - ts.registerCallback(self.cb_collector_2msg) - - if 0: - # Simple case, pairs of messages, make sure that they get combined - for t in range(10): - self.collector = [] - msg0 = MockMessage(t, 33) - msg1 = MockMessage(t, 34) - m0.signalMessage(msg0) - self.assertEqual(self.collector, []) - m1.signalMessage(msg1) - self.assertEqual(self.collector, [(msg0, msg1)]) - - # Scramble sequences of length N. - # Make sure that TimeSequencer recombines them. - random.seed(0) - for N in range(1, 10): - m0 = MockFilter() - m1 = MockFilter() - seq0 = [MockMessage(t, random.random()) for t in range(N)] - seq1 = [MockMessage(t, random.random()) for t in range(N)] - # random.shuffle(seq0) - ts = TimeSynchronizer([m0, m1], N) - ts.registerCallback(self.cb_collector_2msg) - self.collector = [] - for msg in random.sample(seq0, N): - m0.signalMessage(msg) - self.assertEqual(self.collector, []) - for msg in random.sample(seq1, N): - m1.signalMessage(msg) - self.assertEqual(set(self.collector), set(zip(seq0, seq1))) - -if __name__ == '__main__': - suite = unittest.TestSuite() - suite.addTest(TestDirected('test_synchronizer')) - unittest.TextTestRunner(verbosity=2).run(suite) diff --git a/test/test_input_aligner.py b/test/test_input_aligner.py new file mode 100644 index 00000000..1bcf37fd --- /dev/null +++ b/test/test_input_aligner.py @@ -0,0 +1,95 @@ +import unittest + +from builtin_interfaces.msg import Time as TimeMsg +from message_filters import InputAligner, SimpleFilter +import rclpy +from rclpy.duration import Duration +from rclpy.time import Time + + +class Header: + def __init__(self, stamp=None): + self.stamp = stamp if stamp is not None else TimeMsg() + + +class Msg1: + def __init__(self, stamp=None, data=None): + self.header = Header(stamp) + self.data = data + + +class Msg2: + def __init__(self, stamp=None, data=None): + self.header = Header(stamp) + self.data = data + + +class TestInputAligner(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = rclpy.create_node('test_input_aligner_node') + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def setUp(self): + self.timeout = Duration(seconds=1.0) + self.update_rate = Duration(nanoseconds=10000000) + self.cb_content = [] + + def cb(self, msg): + self.cb_content.append(msg.data) + + def create_msg(self, cls, milliseconds, data): + return cls(stamp=Time(nanoseconds=int(milliseconds * 1e6)).to_msg(), data=data) + + def test_dispatch_inputs_in_order(self): + aligner = InputAligner(self.timeout) + aligner.connectInput(SimpleFilter(), SimpleFilter(), SimpleFilter(), SimpleFilter()) + for i in range(4): + aligner.registerCallback(i, self.cb) + aligner.setInputPeriod(i, Duration(nanoseconds=int(4e6))) + aligner.add(self.create_msg(Msg1, 3, 3), 2) + aligner.add(self.create_msg(Msg1, 1, 1), 0) + aligner.add(self.create_msg(Msg1, 7, 7), 2) + aligner.add(self.create_msg(Msg1, 5, 5), 0) + aligner.add(self.create_msg(Msg2, 2, 2), 3) + aligner.add(self.create_msg(Msg1, 9, 9), 0) + aligner.add(self.create_msg(Msg2, 4, 4), 1) + aligner.add(self.create_msg(Msg2, 8, 8), 1) + aligner.add(self.create_msg(Msg2, 6, 6), 3) + aligner.dispatchMessages() + self.assertEqual(self.cb_content, list(range(1, 10))) + + def test_get_queue_status(self): + self.timeout = Duration(nanoseconds=int(1e7)) + aligner = InputAligner(self.timeout) + aligner.connectInput(SimpleFilter(), SimpleFilter()) + for i in range(2): + aligner.registerCallback(i, self.cb) + aligner.setInputPeriod(i, Duration(nanoseconds=int(2e6))) + aligner.add(self.create_msg(Msg2, 2, 2), 1) + aligner.add(self.create_msg(Msg1, 3, 3), 0) + aligner.add(self.create_msg(Msg1, 5, 5), 0) + status_0 = aligner.getQueueStatus(0) + self.assertFalse(status_0.active) + self.assertEqual(status_0.queue_size, 2) + status_1 = aligner.getQueueStatus(1) + self.assertFalse(status_1.active) + self.assertEqual(status_1.queue_size, 1) + aligner.dispatchMessages() + status_0 = aligner.getQueueStatus(0) + self.assertTrue(status_0.active) + self.assertEqual(status_0.queue_size, 1) + self.assertEqual(status_0.msgs_processed, 1) + status_1 = aligner.getQueueStatus(1) + self.assertTrue(status_1.active) + self.assertEqual(status_1.queue_size, 0) + self.assertEqual(status_1.msgs_processed, 1) + + +if __name__ == '__main__': + unittest.main() diff --git a/test/test_latest_time_policy.cpp b/test/test_latest_time_policy.cpp new file mode 100644 index 00000000..0390212b --- /dev/null +++ b/test/test_latest_time_policy.cpp @@ -0,0 +1,388 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include "rclcpp/time_source.hpp" +#include "message_filters/synchronizer.h" +#include "message_filters/sync_policies/latest_time.h" + +#include "rosgraph_msgs/msg/clock.hpp" + + +using namespace message_filters; +using namespace message_filters::sync_policies; +using namespace std::chrono; + + +struct Header +{ + rclcpp::Time stamp; +}; + +struct Msg +{ + Header header; + size_t data; +}; +typedef std::shared_ptr MsgPtr; +typedef std::shared_ptr MsgConstPtr; + +class Helper +{ +public: + Helper() + { + } + + void cb(const MsgConstPtr& p, const MsgConstPtr& q, const MsgConstPtr& r) + { + EXPECT_TRUE(p); + EXPECT_TRUE(q); + EXPECT_TRUE(r); + + p_ = p; q_ = q; r_ = r; + ++count_; + } + + MsgConstPtr p_{nullptr}, q_{nullptr}, r_{nullptr}; + uint16_t count_{0U}; +}; + +typedef LatestTime Policy3; +typedef Synchronizer Sync3; + +class LatestTimePolicy : public ::testing::Test +{ +protected: + rclcpp::Node::SharedPtr node; + rclcpp::SyncParametersClient::SharedPtr param_client; + Sync3 sync; + Helper h; + std::vector p; + std::vector q; + std::vector r; + + virtual void SetUp() + { + // Shutdown in case there was a dangling global context from other test fixtures + rclcpp::shutdown(); + rclcpp::init(0, nullptr); + node = std::make_shared("clock_sleep_node"); + param_client = std::make_shared(node); + ASSERT_TRUE(param_client->wait_for_service(5s)); + + sync.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3)); + p.reserve(12U); + q.reserve(6U); + r.reserve(3U); + for(std::size_t idx = 0U; idx < 12U; ++idx) + { + MsgPtr p_idx(std::make_shared()); p_idx->data = idx; p.push_back(p_idx); + if(idx % 2U == 0U) + { + MsgPtr q_idx(std::make_shared()); q_idx->data = idx; q.push_back(q_idx); + } + if(idx % 4U == 0U) + { + MsgPtr r_idx(std::make_shared()); r_idx->data = idx; r.push_back(r_idx); + } + } + } + + void TearDown() + { + node.reset(); + rclcpp::shutdown(); + } +}; + + +TEST_F(LatestTimePolicy, Leading) +{ + rclcpp::Rate rate(50.0); + for(std::size_t idx = 0U; idx < 8U; ++idx) + { + if(idx % 2U == 0U) + { + sync.add<1>(q[idx / 2U]); + } + if(idx % 4U == 0U) + { + sync.add<2>(r[idx / 4U]); + } + sync.add<0>(p[idx]); + + EXPECT_EQ(h.count_, idx); + if(idx > 0) + { + EXPECT_EQ(h.p_->data, p[idx]->data); + EXPECT_EQ(h.q_->data, q[idx / 2U]->data); + EXPECT_EQ(h.r_->data, r[idx / 4U]->data); + } + else + { + EXPECT_FALSE(h.p_); + EXPECT_FALSE(h.q_); + EXPECT_FALSE(h.r_); + } + + rate.sleep(); + } +} + +TEST_F(LatestTimePolicy, Trailing) +{ + rclcpp::Rate rate(50.0); + for(std::size_t idx = 0U; idx < 8U; ++idx) + { + if(idx % 2U == 1U) + { + sync.add<1>(q[(idx - 1U) / 2U]); + } + if(idx % 4U == 3U) + { + sync.add<2>(r[(idx - 3U) / 4U]); + } + sync.add<0>(p[idx]); + + if (idx > 2U) + { + EXPECT_EQ(h.count_, idx - 2U); + EXPECT_EQ(h.p_->data, p[idx]->data); + EXPECT_EQ(h.q_->data, q[(idx - 1U) / 2U]->data); + EXPECT_EQ(h.r_->data, r[(idx - 3U) / 4U]->data); + } + else + { + EXPECT_FALSE(h.p_); + EXPECT_FALSE(h.q_); + EXPECT_FALSE(h.r_); + } + + rate.sleep(); + } +} + +TEST_F(LatestTimePolicy, ChangeRateLeading) +{ + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + auto clock_publisher = node->create_publisher( + "/clock", rclcpp::ClockQoS()); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + sync.setClock(clock); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + for(std::size_t idx = 0U; idx < 12U; ++idx) + { + if(idx % 2U == 0U) + { + sync.add<1>(q[idx/2U]); + } + + if(idx % 4U == 0U) + { + sync.add<2>(r[idx/4U]); + } + + if (idx < 4U) + { + sync.add<0>(p[idx]); + } + else // Change rate of p + { + if(idx % 3U == 0U) + { + static std::size_t p_idx = 3U; + sync.add<0>(p[++p_idx]); + } + } + + // operates like "Leading" test for idx <= 3 + if (idx >= 1U && idx < 4U) + { + EXPECT_EQ(h.count_, idx); + EXPECT_EQ(h.p_->data, p[idx]->data); + EXPECT_EQ(h.q_->data, q[idx / 2U]->data); + EXPECT_EQ(h.r_->data, r[idx / 4U]->data); + } + // p rate is changed but isn't detected as late until idx==6, + // since q is 500Hz and p isn't late yet when q checks at idx==4. + // Will not publish again until idx==6 when q is found as new pivot. + // Same behavior as initialization dropping faster messages until rates of all are known + // or found to be late. + else if (idx >= 4U && idx < 6U) + { + EXPECT_EQ(h.count_, (idx + 2U) / 2U); + EXPECT_EQ(h.p_->data, p[3]->data); + EXPECT_EQ(h.q_->data, q[1]->data); + EXPECT_EQ(h.r_->data, r[0]->data); + } + // New actual rate of p computed when is received after q when idx==6 + // for idx >= 6, follows normal "Leading" pattern again + // with pivot on q + else if (idx >= 6U) + { + EXPECT_EQ(h.count_, (idx + 2U) / 2U); + EXPECT_EQ(h.p_->data, p[idx / 2U]->data); + EXPECT_EQ(h.q_->data, q[idx / 2U]->data); + EXPECT_EQ(h.r_->data, r[(idx - 2U) / 4U]->data); + } + else + { + EXPECT_FALSE(h.p_); + EXPECT_FALSE(h.q_); + EXPECT_FALSE(h.r_); + } + + double period = 20e6; + auto new_time = clock->now() + rclcpp::Duration(0, static_cast(period)); + auto msg = rosgraph_msgs::msg::Clock(); + msg.clock = rclcpp::Time(new_time); + clock_publisher->publish(msg); + while (rclcpp::ok() && clock->now() < new_time) { + executor.spin_once(10ms); + } + } +} + +TEST_F(LatestTimePolicy, ChangeRateTrailing) +{ + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + auto clock_publisher = node->create_publisher( + "/clock", rclcpp::ClockQoS()); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + sync.setClock(clock); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + for(std::size_t idx = 0U; idx < 12U; ++idx) + { + if(idx % 2U == 1U) + { + sync.add<1>(q[(idx - 1U) / 2U]); + } + + if(idx % 4U == 3U) + { + sync.add<2>(r[(idx - 3U) / 4U]); + } + + if (idx < 4U) + { + sync.add<0>(p[idx]); + } + else // Change rate of p (still 1kHz @ idx == 4) + { + if(idx % 3U == 1U) + { + static std::size_t p_idx = 3U; + sync.add<0>(p[++p_idx]); + } + } + + // operates like "Trailing" test for idx <= 3 + if (idx > 2U && idx <= 4U) + { + EXPECT_EQ(h.count_, idx - 2U); + EXPECT_EQ(h.p_->data, p[idx]->data); + EXPECT_EQ(h.q_->data, q[(idx - 1U) / 2U]->data); + EXPECT_EQ(h.r_->data, r[(idx - 3U) / 4U]->data); + } + // Rate of p still 1kHz @ idx==4. + // Then, change rate of p lower than q when idx==5. + // At idx==5, policy still doesn't know that p is late when q is received. + // Same behavior as initialization dropping faster messages until rates of all are known + // or found to be late. + else if (idx > 4U && idx < 7U) + { + EXPECT_EQ(h.count_, 2U); + EXPECT_EQ(h.p_->data, p[4U]->data); + EXPECT_EQ(h.q_->data, q[1U]->data); + EXPECT_EQ(h.r_->data, r[0U]->data); + } + // Will not publish again until idx==7, since rate of q is 500Hz + // and p is calculated as late when q recieved when idx==7 -- this makes q new pivot. + // Since q is new pivot and publishes when idx==7, + // and r comes in after q, r is now trailing. + // New actual rate of p computed when is received after q when idx==7 + // for idx >= 7, follows normal "Trailing" pattern again + // with pivot on q + else if (idx >= 7U) + { + EXPECT_EQ(h.count_, (idx - 1U) / 2U); + EXPECT_EQ(h.p_->data, p[(idx + 1U) / 2U]->data); + EXPECT_EQ(h.q_->data, q[(idx - 1U) / 2U]->data); + EXPECT_EQ(h.r_->data, r[(idx - 5U) / 4U]->data); + } + else + { + EXPECT_FALSE(h.p_); + EXPECT_FALSE(h.q_); + EXPECT_FALSE(h.r_); + } + + double period = 20e6; + auto new_time = clock->now() + rclcpp::Duration(0, static_cast(period)); + auto msg = rosgraph_msgs::msg::Clock(); + msg.clock = rclcpp::Time(new_time); + clock_publisher->publish(msg); + while (rclcpp::ok() && clock->now() < new_time) { + executor.spin_once(10ms); + } + } +} + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_message_filters_cache.py b/test/test_message_filters_cache.py index 63147401..154c7487 100644 --- a/test/test_message_filters_cache.py +++ b/test/test_message_filters_cache.py @@ -45,13 +45,10 @@ class AnonymMsg: class AnonymHeader: - stamp = None def __init__(self): stamp = Time() - header = None - def __init__(self): self.header = AnonymMsg.AnonymHeader() diff --git a/test/test_message_filters_chain.py b/test/test_message_filters_chain.py new file mode 100644 index 00000000..f8fe92a6 --- /dev/null +++ b/test/test_message_filters_chain.py @@ -0,0 +1,189 @@ +# Copyright 2025, Open Source Robotics Foundation, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +import unittest + +from message_filters import Cache, Chain, Subscriber +import rclpy +from rclpy.time import Time +from std_msgs.msg import String + + +class AnonymMsg: + class AnonymHeader: + + def __init__(self): + self.stamp = Time() + + def __init__(self): + self.header = AnonymMsg.AnonymHeader() + + +class TestChain(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = rclpy.create_node('TestChainNode', namespace='/my_ns') + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_add_filter(self): + chain_filter = Chain() + + first_cache_filter = Cache( + f=Subscriber(self.node, String, '/empty'), + cache_size=3, + ) + second_cache_filter = Cache( + f=Subscriber(self.node, String, '/empty'), + cache_size=3, + ) + + chain_filter.addFilter(first_cache_filter) + + assert chain_filter.getFilter(0) == first_cache_filter + + first_msg = AnonymMsg() + first_msg.header.stamp = Time(seconds=1) + chain_filter.add(first_msg) + + first_cached_messages = first_cache_filter.getInterval( + Time(seconds=0), + Time(seconds=10) + ) + + second_cached_messages = second_cache_filter.getInterval( + Time(seconds=0), + Time(seconds=10) + ) + + assert first_cached_messages == [first_msg] + assert second_cached_messages == [] + + chain_filter.addFilter(second_cache_filter) + + second_msg = AnonymMsg() + second_msg.header.stamp = Time(seconds=2) + chain_filter.add(second_msg) + + first_cached_messages = first_cache_filter.getInterval( + Time(seconds=0), + Time(seconds=10) + ) + + second_cached_messages = second_cache_filter.getInterval( + Time(seconds=0), + Time(seconds=10) + ) + + assert first_cached_messages == [first_msg, second_msg] + assert second_cached_messages == [second_msg] + + def test_get_filter(self): + chain_filter = Chain() + + with self.assertRaises(KeyError): + chain_filter.getFilter(0) + + first_cache_filter = Cache(Subscriber(self.node, String, '/empty')) + second_cache_filter = Cache(Subscriber(self.node, String, '/empty')) + + chain_filter.addFilter(first_cache_filter) + chain_filter.addFilter(second_cache_filter) + + first_filter_in_chain = chain_filter.getFilter(0) + second_filter_in_chain = chain_filter.getFilter(1) + + assert first_filter_in_chain is first_cache_filter + assert second_filter_in_chain is second_cache_filter + + def test_chain_callback(self): + class Counter: + + def __init__(self): + self._counter: int = 0 + + def counter_callback(self, _): + self._counter += 1 + + @property + def value(self): + return self._counter + + counter = Counter() + + assert counter.value == 0 + + chain_filter = Chain() + cache_filter = Cache(Subscriber(self.node, String, '/empty')) + chain_filter.addFilter(cache_filter) + chain_filter.registerCallback(counter.counter_callback) + + msg = AnonymMsg() + msg.header.stamp = Time(seconds=1) + chain_filter.add(msg) + + assert counter.value == 1 + + def test_connect_input(self): + chain_filter = Chain() + first_cache_filter = Cache(Subscriber(self.node, String, '/empty')) + second_cache_filter = Cache(Subscriber(self.node, String, '/empty')) + + chain_filter.connectInput(first_cache_filter) + chain_filter.addFilter(second_cache_filter) + + msg = AnonymMsg() + msg.header.stamp = Time(seconds=1) + first_cache_filter.add(msg) + + first_cached_messages = first_cache_filter.getInterval( + Time(seconds=0), + Time(seconds=10) + ) + + second_cached_messages = second_cache_filter.getInterval( + Time(seconds=0), + Time(seconds=10) + ) + + assert first_cached_messages == second_cached_messages + + +if __name__ == '__main__': + suite = unittest.TestSuite() + suite.addTest(TestChain('test_add_filter')) + suite.addTest(TestChain('test_get_filter')) + suite.addTest(TestChain('test_chain_callback')) + suite.addTest(TestChain('test_connect_input')) + unittest.TextTestRunner(verbosity=2).run(suite) diff --git a/test/test_message_filters_delta_filter.cpp b/test/test_message_filters_delta_filter.cpp new file mode 100644 index 00000000..792e5d29 --- /dev/null +++ b/test/test_message_filters_delta_filter.cpp @@ -0,0 +1,315 @@ +// Copyright 2026, Open Source Robotics Foundation, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include + +#include +#include "message_filters/connection.hpp" +#include +#include + + +namespace test_messages +{ +struct IntMsg +{ + int data; +}; + +struct FloatMsg +{ + float data; +}; + +struct BoolMsg +{ + bool data; +}; + +} // namespace test_messages + +using TestMessageTypes = ::testing::Types< + test_messages::IntMsg, + test_messages::FloatMsg, + test_messages::BoolMsg +>; + +template +struct ParametrizedTest : public ::testing::Test +{ + using TypeParam = T; + static constexpr T value_0 {0}; + static constexpr T value_1 {1}; +}; + + +template +class ComparisonHandlerMock : public message_filters::CachedComparisonHandler +{ +public: + typedef std::shared_ptr MConstPtr; + typedef std::function FieldComparatorFunctionType; + + explicit ComparisonHandlerMock( + std::forward_list /* comparators */ = {} + ) + : message_filters::CachedComparisonHandler( + {[] (const MConstPtr & a, const MConstPtr & b) { return a->data != b->data; }} + ) {} + + void set_comparison_success(bool success) + { + comparison_success = success; + } + + bool do_fields_fit( + const FieldComparatorFunctionType & /* comparator */, + const MConstPtr & /* cached */, + const MConstPtr & /* current */ + ) const + { + return comparison_success; + } + +private: + bool comparison_success {true}; +}; + +template +class SimpleCachefilter : public message_filters::SimpleFilter +{ +public: + typedef message_filters::MessageEvent EventType; + + template + explicit SimpleCachefilter(F & f) + : message_filters::SimpleFilter() + { + connectInput(f); + } + + template + void connectInput(F & f) + { + incoming_connection_ = f.registerCallback( + typename message_filters::SimpleFilter::EventCallback( + std::bind(&SimpleCachefilter::callback, this, std::placeholders::_1) + ) + ); + } + + void callback(const EventType & evt) + { + event_cache.reset(new EventType(evt)); + } + +public: + std::unique_ptr event_cache; + +private: + message_filters::Connection incoming_connection_; +}; + +template +struct ParametrizedCachedCompareTest : public ParametrizedTest {}; +TYPED_TEST_SUITE(ParametrizedCachedCompareTest, TestMessageTypes); +TYPED_TEST(ParametrizedCachedCompareTest, DoFieldsFitCall) +{ + using MsgType = typename TestFixture::TypeParam; + typedef message_filters::MessageEvent EventType; + + MsgType test_msg_0 = TestFixture::value_0; + MsgType test_msg_1 = TestFixture::value_1; + + auto handler = ComparisonHandlerMock(); + + EXPECT_TRUE( + handler.message_fits( + EventType( + std::make_shared(test_msg_0) + ) + ) + ); + + EXPECT_TRUE( + handler.message_fits( + EventType( + std::make_shared(test_msg_1) + ) + ) + ); + + handler.set_comparison_success(false); + + EXPECT_FALSE( + handler.message_fits( + EventType( + std::make_shared(test_msg_0) + ) + ) + ); +} + +template +struct ParametrizedDeltaCompareTest : public ParametrizedCachedCompareTest {}; +TYPED_TEST_SUITE(ParametrizedDeltaCompareTest, TestMessageTypes); +TYPED_TEST(ParametrizedDeltaCompareTest, TestComparison) +{ + using MsgType = typename TestFixture::TypeParam; + typedef message_filters::MessageEvent EventType; + typedef std::shared_ptr MConstPtr; + + MsgType test_msg_0 = TestFixture::value_0; + MsgType test_msg_1 = TestFixture::value_1; + + auto handler = message_filters::DeltaCompare( + {[] (const MConstPtr & a, const MConstPtr & b) { return a->data != b->data; }} + ); + + // New message for empty comparison handler -> True + EXPECT_TRUE( + handler.message_fits( + EventType( + std::make_shared(test_msg_0) + ) + ) + ); + + // New message for non empty comparison handler -> True + EXPECT_TRUE( + handler.message_fits( + EventType( + std::make_shared(test_msg_1) + ) + ) + ); + + // Message duplicate for non empty comparison handler -> False + EXPECT_FALSE( + handler.message_fits( + EventType( + std::make_shared(test_msg_1) + ) + ) + ); +} + +template +struct ParametrizedComparisonFilterTest : public ParametrizedCachedCompareTest {}; +TYPED_TEST_SUITE(ParametrizedComparisonFilterTest, TestMessageTypes); +TYPED_TEST(ParametrizedComparisonFilterTest, TestComparison) +{ + using MsgType = typename TestFixture::TypeParam; + typedef std::shared_ptr MConstPtr; + typedef message_filters::MessageEvent EventType; + + MsgType test_msg_0 = TestFixture::value_0; + MsgType test_msg_1 = TestFixture::value_1; + + auto simple_filter = message_filters::SimpleFilter(); + auto comparison_filter = message_filters::ComparisonFilter( + simple_filter, + {[] (const MConstPtr & a, const MConstPtr & b) { return a->data != b->data; }} + ); + auto cache_filter = SimpleCachefilter(comparison_filter); + + comparison_filter.add( + EventType( + std::make_shared(test_msg_0) + ) + ); + EXPECT_TRUE(bool(cache_filter.event_cache)); + EXPECT_EQ(cache_filter.event_cache->getMessage()->data, test_msg_0.data); + + comparison_filter.add( + EventType( + std::make_shared(test_msg_1) + ) + ); + EXPECT_TRUE(bool(cache_filter.event_cache)); + EXPECT_EQ(cache_filter.event_cache->getMessage()->data, test_msg_1.data); +} + + +template +struct ParametrizedDeltaFilterTest : public ParametrizedTest {}; +TYPED_TEST_SUITE(ParametrizedDeltaFilterTest, TestMessageTypes); +TYPED_TEST(ParametrizedDeltaFilterTest, TestComparison) +{ + using MsgType = typename TestFixture::TypeParam; + typedef std::shared_ptr MConstPtr; + typedef message_filters::MessageEvent EventType; + + MsgType test_msg_0 = TestFixture::value_0; + MsgType test_msg_1 = TestFixture::value_1; + + auto simple_filter = message_filters::SimpleFilter(); + auto delta_filter = message_filters::DeltaFilter( + simple_filter, + {[] (const MConstPtr & a, const MConstPtr & b) { return a->data != b->data; }} + ); + auto cache_filter = SimpleCachefilter(delta_filter); + + // New message received and passed down the line + delta_filter.add( + EventType( + std::make_shared(test_msg_0) + ) + ); + EXPECT_TRUE(bool(cache_filter.event_cache)); + EXPECT_EQ(cache_filter.event_cache->getMessage()->data, test_msg_0.data); + + // Same message received and not passed down the line + cache_filter.event_cache.reset(); + delta_filter.add( + EventType( + std::make_shared(test_msg_0) + ) + ); + EXPECT_FALSE(bool(cache_filter.event_cache)); + + // New message received and passed down the line + delta_filter.add( + EventType( + std::make_shared(test_msg_1) + ) + ); + EXPECT_TRUE(bool(cache_filter.event_cache)); + EXPECT_EQ(cache_filter.event_cache->getMessage()->data, test_msg_1.data); +} + + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + auto ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/test/test_message_filters_delta_filter.py b/test/test_message_filters_delta_filter.py new file mode 100644 index 00000000..c29a6e9b --- /dev/null +++ b/test/test_message_filters_delta_filter.py @@ -0,0 +1,244 @@ +# Copyright 2026, Open Source Robotics Foundation, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import unittest + +from message_filters import Cache +from message_filters.delta_filter import ( + CachedComparisonHandler, + ComparisonFilter, + DeltaCompare, + DeltaFilter, + PathDeltaFilter, + SimpleFilter, +) + +from rclpy.type_support import MsgT + +from std_msgs.msg import Bool, ColorRGBA, Int32, String + + +class TestDeltaCompareClasses(unittest.TestCase): + + def test_string_delta_compare_success(self): + delta_compare = DeltaCompare( + field_getters=[lambda message: message.data] + ) + msg_1 = String(data='a') + msg_2 = String(data='b') + assert delta_compare.message_fits(msg_1) + assert delta_compare.message_fits(msg_2) + + def test_string_delta_compare_failure(self): + delta_compare = DeltaCompare( + field_getters=[lambda message: message.data] + ) + msg_1 = String(data='a') + msg_2 = String(data='a') + assert delta_compare.message_fits(msg_1) + assert not delta_compare.message_fits(msg_2) + + def test_bool_delta_compare_success(self): + delta_compare = DeltaCompare( + field_getters=[lambda message: message.data] + ) + msg_1 = Bool(data=True) + msg_2 = Bool(data=False) + assert delta_compare.message_fits(msg_1) + assert delta_compare.message_fits(msg_2) + + def test_bool_delta_compare_failure(self): + delta_compare = DeltaCompare( + field_getters=[lambda message: message.data] + ) + msg_1 = Bool(data=True) + msg_2 = Bool(data=True) + assert delta_compare.message_fits(msg_1) + assert not delta_compare.message_fits(msg_2) + + def test_int32_delta_compare_success(self): + delta_compare = DeltaCompare( + field_getters=[lambda message: message.data] + ) + msg_1 = Int32(data=10) + msg_2 = Int32(data=11) + assert delta_compare.message_fits(msg_1) + assert delta_compare.message_fits(msg_2) + + def test_int32_delta_compare_failure(self): + delta_compare = DeltaCompare( + field_getters=[lambda message: message.data] + ) + msg_1 = Int32(data=10) + msg_2 = Int32(data=10) + assert delta_compare.message_fits(msg_1) + assert not delta_compare.message_fits(msg_2) + + +class TestComparisonFilter(unittest.TestCase): + + class ComparisonHandlerMock(CachedComparisonHandler): + + def __init__(self): + super().__init__() + self._compare_result: bool = True + + def message_fits(self, message: MsgT) -> bool: + return self._compare_result + + def set_compare_to(self, res: bool): + self._compare_result = res + + def test_comparison_filter_compare_messages(self): + comparison_handler = self.ComparisonHandlerMock() + + comparison_filter = ComparisonFilter( + comparison_handler=comparison_handler, + message_filter=SimpleFilter() + ) + + cache_filter = Cache(f=comparison_filter, allow_headerless=True) + + self.assertEqual(cache_filter.cache_msgs, []) + + msg = Int32(data=10) + comparison_filter.add(msg) + + self.assertEqual(cache_filter.cache_msgs, [msg]) + comparison_handler.set_compare_to(False) + + comparison_filter.add(msg) + self.assertEqual(cache_filter.cache_msgs, [msg]) + + +class TestDeltaFilter(unittest.TestCase): + + def test_delta_filter_str(self): + delta_filter = DeltaFilter( + field_getters=[lambda msg: msg.data] + ) + cache_filter = Cache(f=delta_filter, allow_headerless=True) + self.assertEqual(cache_filter.cache_msgs, []) + + msg_1 = String(data='a') + msg_2 = String(data='b') + + delta_filter.add(msg_1) + self.assertEqual(cache_filter.cache_msgs, [msg_1]) + + delta_filter.add(msg_2) + self.assertEqual(cache_filter.cache_msgs, [msg_2]) + + def test_multiple_field_messages(self): + delta_filter = DeltaFilter( + field_getters=[ + lambda msg: msg.r, + lambda msg: msg.g, + lambda msg: msg.b, + lambda msg: msg.a, + ] + ) + cache_filter = Cache(f=delta_filter, allow_headerless=True) + self.assertEqual(cache_filter.cache_msgs, []) + + msg_1 = ColorRGBA( + r=0, + g=0, + b=0, + a=0 + ) + msg_2 = ColorRGBA( + r=1, + g=0, + b=0, + a=0 + ) + + delta_filter.add(msg_1) + self.assertEqual(cache_filter.cache_msgs, [msg_1]) + + delta_filter.add(msg_2) + self.assertEqual(cache_filter.cache_msgs, [msg_2]) + + +class TestPathDeltaFilter(unittest.TestCase): + + def test_path_delta_filter(self): + path_delta_filter = PathDeltaFilter( + field_path_list=[ + 'r', + 'g', + 'b', + 'a', + ] + ) + cache_filter = Cache(f=path_delta_filter, allow_headerless=True) + self.assertEqual(cache_filter.cache_msgs, []) + + msg_1 = ColorRGBA( + r=0, + g=0, + b=0, + a=0 + ) + msg_2 = ColorRGBA( + r=1, + g=0, + b=0, + a=0 + ) + + path_delta_filter.add(msg_1) + self.assertEqual(cache_filter.cache_msgs, [msg_1]) + + path_delta_filter.add(msg_2) + self.assertEqual(cache_filter.cache_msgs, [msg_2]) + + +if __name__ == '__main__': + suite = unittest.TestSuite() + + # Test DeltaCompare comparison handler with basic data types + suite.addTest(TestDeltaCompareClasses('test_string_delta_compare_success')) + suite.addTest(TestDeltaCompareClasses('test_string_delta_compare_failure')) + suite.addTest(TestDeltaCompareClasses('test_bool_delta_compare_success')) + suite.addTest(TestDeltaCompareClasses('test_bool_delta_compare_failure')) + suite.addTest(TestDeltaCompareClasses('test_int32_delta_compare_success')) + suite.addTest(TestDeltaCompareClasses('test_int32_delta_compare_failure')) + + # Test ComparisonFilter + suite.addTest(TestComparisonFilter('test_comparison_filter_compare_messages')) + + # Test DeltaFilter + suite.addTest(TestDeltaFilter('test_delta_filter_str')) + suite.addTest(TestDeltaFilter('test_multiple_field_messages')) + + # Test PathDeltaFilter + suite.addTest(TestPathDeltaFilter('test_path_delta_filter')) + + unittest.TextTestRunner(verbosity=2).run(suite) diff --git a/test/test_time_synchronizer.py b/test/test_time_synchronizer.py new file mode 100644 index 00000000..5c3658d6 --- /dev/null +++ b/test/test_time_synchronizer.py @@ -0,0 +1,143 @@ +# image_transport::SubscriberFilter wide_left; // "/wide_stereo/left/image_raw" +# image_transport::SubscriberFilter wide_right; // "/wide_stereo/right/image_raw" +# message_filters::Subscriber wide_left_info; // "/wide_stereo/left/camera_info" +# message_filters::Subscriber wide_right_info; // "/wide_stereo/right/camera_info" +# message_filters::TimeSynchronizer wide; +# +# PersonDataRecorder() : +# wide_left(nh_, "/wide_stereo/left/image_raw", 10), +# wide_right(nh_, "/wide_stereo/right/image_raw", 10), +# wide_left_info(nh_, "/wide_stereo/left/camera_info", 10), +# wide_right_info(nh_, "/wide_stereo/right/camera_info", 10), +# wide(wide_left, wide_left_info, wide_right, wide_right_info, 4), +# +# wide.registerCallback(boost::bind(&PersonDataRecorder::wideCB, this, _1, _2, _3, _4)); + +import functools +import unittest + +from builtin_interfaces.msg import Time as TimeMsg +from message_filters import SimpleFilter, Subscriber, Cache, TimeSynchronizer + + +class MockHeader: + pass + +class MockMessage: + def __init__(self, stamp, data): + self.header = MockHeader() + self.header.stamp = TimeMsg(sec=stamp) + self.data = data + +class MockFilter(SimpleFilter): + pass + +class TestDirected(unittest.TestCase): + + @staticmethod + def collector_callback(msg1, msg2, collector): + collector.append((msg1, msg2)) + + def test_time_synchronizer_queue_lentgth(self): + for N in range(1, 10): + seq0 = [MockMessage(t, 0) for t in range(N)] + seq1 = [MockMessage(t, 0) for t in range(N)] + + m0 = MockFilter() + m1 = MockFilter() + ts = TimeSynchronizer([m0, m1], N) + + collector = [] + ts.registerCallback( + functools.partial( + self.collector_callback, + collector=collector, + ) + ) + + for msg in seq0: + m0.signalMessage(msg) + self.assertEqual(collector, []) + for msg in seq1: + m1.signalMessage(msg) + self.assertEqual(set(collector), set(zip(seq0, seq1))) + + def test_time_synchronizer_drop_old_messages(self): + collector = [] + + filter_0 = MockFilter() + filter_1 = MockFilter() + ts = TimeSynchronizer([filter_0, filter_1], 10) + ts.registerCallback( + functools.partial( + self.collector_callback, + collector=collector, + ) + ) + + t1 = 0 + t2 = 1 + + x0 = MockMessage(t1, 1) + x1 = MockMessage(t1, 2) + + y0 = MockMessage(t2, 1) + y1 = MockMessage(t2, 2) + + filter_0.signalMessage(x0) + assert len(collector) == 0 + + filter_1.signalMessage(y1) + assert len(collector) == 0 + + filter_0.signalMessage(y0) + assert len(collector) == 1 + assert collector[0] == (y0, y1) + + filter_1.signalMessage(x1) + assert len(collector) == 1 + assert collector[0] == (y0, y1) + + def test_time_synchronizer_shifted_time_signalling(self): + collector = [] + + filter_0 = MockFilter() + filter_1 = MockFilter() + ts = TimeSynchronizer([filter_0, filter_1], 10) + ts.registerCallback( + functools.partial( + self.collector_callback, + collector=collector, + ) + ) + + t1 = 0 + t2 = 1 + + x0 = MockMessage(t1, 1) + x1 = MockMessage(t1, 2) + + y0 = MockMessage(t2, 1) + y1 = MockMessage(t2, 2) + + filter_0.signalMessage(x0) + assert len(collector) == 0 + + filter_0.signalMessage(y0) + assert len(collector) == 0 + + filter_1.signalMessage(x1) + assert len(collector) == 1 + assert collector[0] == (x0, x1) + + filter_1.signalMessage(y1) + assert len(collector) == 2 + assert collector[0] == (x0, x1) + assert collector[1] == (y0, y1) + +if __name__ == '__main__': + suite = unittest.TestSuite() + suite.addTest(TestDirected('test_time_synchronizer_queue_lentgth')) + suite.addTest(TestDirected('test_time_synchronizer_drop_old_messages')) + suite.addTest(TestDirected('test_time_synchronizer_shifted_time_signalling')) + unittest.TextTestRunner(verbosity=2).run(suite)