Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 32 additions & 9 deletions doc/Tutorials/Approximate-Synchronizer-Cpp.rst
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,8 @@ If you have not done so already `create a workspace <https://docs.ros.org/en/rol
#include "message_filters/synchronizer.hpp"
#include "message_filters/sync_policies/approximate_time.hpp"

#include "sensor_msgs/msg/temperature.hpp"
#include "sensor_msgs/msg/fluid_pressure.hpp"
#include <sensor_msgs/msg/temperature.hpp>
#include <sensor_msgs/msg/fluid_pressure.hpp>

using namespace std::chrono_literals;

Expand All @@ -48,7 +48,8 @@ If you have not done so already `create a workspace <https://docs.ros.org/en/rol

For this example we will be using the ``temperature`` and ``fluid_pressure`` messages found in
`sensor_msgs <https://github.com/ros2/common_interfaces/tree/rolling/sensor_msgs/msg>`_.
To simulate a working ``Synchronizer`` using the ``ApproximateTime`` Policy. We will be publishing and subscribing to topics of those respective types, to showcase how real sensors would be working.
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++

Expand All @@ -57,7 +58,8 @@ To simulate a working ``Synchronizer`` using the ``ApproximateTime`` Policy. We
message_filters::Subscriber<sensor_msgs::msg::Temperature> temp_sub;
message_filters::Subscriber<sensor_msgs::msg::FluidPressure> fluid_sub;

Notice that the ``Subscribers`` are in the ``message_filters`` namespace, while we can utilize ``rclcpp::Publishers``. To simulate them we will also need two ``TimerBases``. Then, we will be utilizing a ``Synchronizer`` to get these messages from the sensor topics aligned.
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

Expand Down Expand Up @@ -87,7 +89,11 @@ Next, we can initialize these private elements within a basic ``Node`` construct

}

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

Expand Down Expand Up @@ -131,7 +137,8 @@ So, we must create three (or more) private callbacks, one for the ``Synchronizer
}


``SyncCallback`` takes ``const shared_ptr references`` relating to both topics becasue they will be taken at the exact time, from here you can compare these topics, set values, etc. This callback is the final goal of synching multiple topics and the reason why the qos and header stamps must be the same. This will be seen with the logging statement as both of the times will be the same. Though, the headers have to have the same ``stamp`` value, they don't have to be triggered at the same time with ``ApproximateTime`` which will be seen in a delay between logging calls. For the ``TimerCallback`` just initialize both the ``Temperature`` and ``FluidPressure`` in whatever way necessary. .
``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

Expand Down Expand Up @@ -169,14 +176,30 @@ Finally, add the ``install(TARGETS…)`` section so ``ros2 run`` can find your e
approximate_time_sync
DESTINATION lib/${PROJECT_NAME})


3. Build
~~~~~~~~
From the root of your package, build and source.

.. code-block:: bash
.. 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 && . install/setup.zsh
$ colcon build
$ call C:\dev\ros2\local_setup.bat

4. Run
~~~~~~
Expand Down
16 changes: 12 additions & 4 deletions doc/Tutorials/Approximate-Synchronizer-Python.rst
Original file line number Diff line number Diff line change
Expand Up @@ -43,8 +43,13 @@ If you have not done so already `create a workspace <https://docs.ros.org/en/rol

For this example we will be using the ``temperature`` and ``fluid_pressure`` messages found in
`sensor_msgs <https://github.com/ros2/common_interfaces/tree/rolling/sensor_msgs/msg>`_.
To simulate a working ``ApproximateTimeSynchronizer``. We will be publishing and subscribing to topics of those respective types, to showcase how real sensors would be working. To simulate them we will also need two ``Timers`` on different intervals. Then, we will be utilizing an ``ApproximateTimeSynchronizer`` to get these messages from the sensor topics aligned with a slight delay between messages..
It is essential that the QoS is the same for all of the publishers and subscribers, otherwise the Message Filter cannot align the topics together. So, create one ``QoSProfile`` and stick with it, or find out what ``qos`` is being used in the native sensor code, and replicate it. Do basic construction of each object relating to the ``Node`` and callback methods that may be used in the future. Both of the two timers we utilize will have different timer values of ``1`` and ``1.05`` which causes the timers to off at different points, which is an advantage of using ``ApproximateTime``. Notice that we must call ``sync->registerCallback`` to sync up the two (or more) chosen topics.
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``.

Expand Down Expand Up @@ -80,7 +85,10 @@ So, we must create three (or more) private callbacks, one for the ``ApproximateT
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. .
``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

Expand All @@ -107,7 +115,7 @@ Finally, create a main function and spin the node
^^^^^^^^^^^^^^^^^^^^^^
Navigate to the root of your package's directory, where ``package.xml`` is located, open, and add the following dependencies:

.. code-block:: Python
.. code-block:: xml

<exec_depend>rclpy</exec_depend>
<exec_depend>message_filters</exec_depend>
Expand Down
65 changes: 32 additions & 33 deletions doc/Tutorials/Cache-Cpp.rst
Original file line number Diff line number Diff line change
Expand Up @@ -29,17 +29,17 @@ The next step is to create a new C++ file inside your package, e.g., ``cache_tut
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include <rclcpp/rclcpp.hpp>

#include "message_filters/subscriber.hpp"
#include "message_filters/cache.hpp"

#include "std_msgs/msg/string.hpp"
#include <std_msgs/msg/string.hpp>

using namespace std::chrono_literals;

const std::string TUTORIAL_TOPIC_NAME = "tutorial_topic";

class CacheNode : public rclcpp::Node {
public:
CacheNode()
Expand All @@ -48,65 +48,64 @@ The next step is to create a new C++ file inside your package, e.g., ``cache_tut
auto qos = rclcpp::QoS(10);
publisher_ = this->create_publisher<std_msgs::msg::String>(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<std_msgs::msg::String>::SharedPtr publisher_;

rclcpp::TimerBase::SharedPtr publisher_timer_;
rclcpp::TimerBase::SharedPtr query_timer_;

message_filters::Subscriber<std_msgs::msg::String> subscriber_filter_;
message_filters::Cache<std_msgs::msg::String> cache_filter_{subscriber_filter_, 10, true};
};


int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto cache_node = std::make_shared<CacheNode>();
rclcpp::spin(cache_node);
rclcpp::shutdown();

return 0;
}


1.1 Examine the code
~~~~~~~~~~~~~~~~~~~~
Expand All @@ -119,15 +118,15 @@ Now, let's break down this code and examine the details.
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include <rclcpp/rclcpp.hpp>

#include "message_filters/subscriber.hpp"
#include "message_filters/cache.hpp"

#include "std_msgs/msg/string.hpp"
#include <std_msgs/msg/string.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.
Expand All @@ -143,10 +142,10 @@ For starters, let's take a look at the ``private`` section of this class:
.. code-block:: C++

rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

rclcpp::TimerBase::SharedPtr publisher_timer_;
rclcpp::TimerBase::SharedPtr query_timer_;

message_filters::Subscriber<std_msgs::msg::String> subscriber_filter_;
message_filters::Cache<std_msgs::msg::String> cache_filter_{subscriber_filter_, 10, true};

Expand Down Expand Up @@ -205,7 +204,7 @@ 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``.

Expand All @@ -219,18 +218,18 @@ Finally, let's take a look at the class constructor.
auto qos = rclcpp::QoS(10);
publisher_ = this->create_publisher<std_msgs::msg::String>(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++
Expand All @@ -251,7 +250,7 @@ After that all what's left to be done is to create timers and we are good to go.
1s,
std::bind(&CacheNode::publisher_timer_callback, this)
);

query_timer_ = this->create_wall_timer(
1s,
std::bind(&CacheNode::query_timer_callback, this)
Expand All @@ -267,7 +266,7 @@ The ``main`` function in this case is pretty straightforward.
auto cache_node = std::make_shared<CacheNode>();
rclcpp::spin(cache_node);
rclcpp::shutdown();

return 0;
}

Expand Down Expand Up @@ -300,7 +299,7 @@ Finally, add the install(TARGETS…) section so ros2 run can find your executabl

install(TARGETS cache_tutorial
DESTINATION lib/${PROJECT_NAME})

4. Build Your Package
~~~~~~~~~~~~~~~~~~~~~

Expand Down Expand Up @@ -341,7 +340,7 @@ 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:

Expand All @@ -365,7 +364,7 @@ Note as the oldest time is starting to update after the 5'th message is added to
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
6. Other methods of the Cache filter interface
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

The ``Cache`` filter stores the last N messages (in this case, 5), and allows querying:
Expand Down
8 changes: 4 additions & 4 deletions doc/Tutorials/Cache-Python.rst
Original file line number Diff line number Diff line change
Expand Up @@ -162,7 +162,7 @@ It is the way to chain these two filters together. Message is going to pass thro
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``.
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.

Expand Down Expand Up @@ -254,7 +254,7 @@ From the root of your workspace:

.. code-block:: console

$ colcon build && . install/setup.bash
$ colcon build && . install/setup.bash

.. group-tab:: macOS

Expand Down Expand Up @@ -283,7 +283,7 @@ 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:

Expand All @@ -302,7 +302,7 @@ Note as the oldest time is starting to update after the 5'th message is added to
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
6. Other methods of the Cache filter interface
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

The ``Cache`` filter stores the last N messages (in this case, 5), and allows querying:
Expand Down
Loading