Package that implements different filters for ros messages.
Refer to index.rst for more information.
The implementation with the details can be found in src/message_filters/init.py
License: BSD 3-Clause "New" or "Revised" License
Package that implements different filters for ros messages.
Refer to index.rst for more information.
The implementation with the details can be found in src/message_filters/init.py
@jwang11 @gaoethan @testkit In order to integrate the various changes from the https://github.com/intel/ros2_message_filters repository here please create separate PRs for separate /unrelated changes:
MessageEvent
) - depending on the amount of code the history might be good to keep (?)Boost
to std
intel
repo)By making separate PRs with orthogonal changes it will be easier / quicker to review / iterate /merge them.
Hello im new with this stuff and i've been trying for months to have orbslam2 running correctly and one of the requirements for the rgbd in the wrapper by alsora requires the message filters, the only issue is that when trying tho build the thing on foxy , it brings me back with the
which im not going to lie i half understand, i tried fixing by changing the ClockQoS to QoS or Clock but so far nothing... not really sure if im doing something wrong or what, please help...
Hello,
My ROS2 node subscribes to two topics (left_image and right_image), and has a message filter (sync_exact policy) to get sychronized data.
Now I'm working on a custom memory allocator (following this tutorial), and in this example an rclcpp::message_memory_strategy has to be specified when creating the node subsription.
rclcpp::SubscriptionOptionsWithAllocator<Alloc> subscription_options;
auto msg_mem_strat = std::make_shared<rclcpp::message_memory_strategy::MessageMemoryStrategy<
std_msgs::msg::UInt32, Alloc>>(alloc);
auto subscriber = node->create_subscription<std_msgs::msg::UInt32>(
"allocator_tutorial", 10, callback, subscription_options, msg_mem_strat);
However I can't find a constructor in message_filters::Subscriber which takes message memory strategy as input.
This is what I have right now, and should be changed to using custom memory allocator.
message_filters::Subscriber<sensor_msgs::msg::Image> left_img;
message_filters::Subscriber<sensor_msgs::msg::Image> right_img;
std::shared_ptr<message_filters::Synchronizer<exact_policy>> syncExact;
left_img.subscribe(this, left_topic, custom_qos_profile);
right_img.subscribe(this, right_topic, custom_qos_profile);
syncExact = std::make_shared<message_filters::Synchronizer<exact_policy>>(exact_policy(10), left_img, right_img);
syncExact->registerCallback(exact_sync_callback);
Am I following the right approach? How to specify the message memory strategy when using message filter?
Thank you very much!
Best Regards.
Derek
terminate called after throwing an instance of 'std::runtime_error'
what(): can't compare times with different time sources
Offending line is:
142 if (current.first > candidate) {
gef➤ p current.first
$1 = {
_vptr.Time = 0x7ffff7efa928 <vtable for rclcpp::Time+16>,
rcl_time_ = {
nanoseconds = 0x7fffffffffffffff,
clock_type = RCL_ROS_TIME
}
}
gef➤ p candidate
$2 = {
_vptr.Time = 0x7ffff7efa928 <vtable for rclcpp::Time+16>,
rcl_time_ = {
nanoseconds = 0x0,
clock_type = RCL_SYSTEM_TIME
}
}
gef➤
Required Info:
To test the issue, you can use composable_nodes_test_pkg from:
https://github.com/Deduard/ros2_demo/
The package contains a simple publishing and subscribing node implementation. The subscribing node uses ApproximateTime policy to synchronize messages from three publishers.
The file test_composable_nodes.launch.py contains the following block:
/***
ComposableNode(
package='composable_nodes_test_pkg',
plugin='PublisherNode',
name='publisher_node1',
parameters=[
{'publish_frequency': 30.0,
'topic': 'topic1'}
],
),
ComposableNode(
package='composable_nodes_test_pkg',
plugin='PublisherNode',
name='publisher_node2',
parameters=[
{'publish_frequency': 20.0,
'topic': 'topic2'}
],
),
ComposableNode(
package='composable_nodes_test_pkg',
plugin='PublisherLifecycleNode',
name='publisher_node3',
parameters=[
{'publish_frequency': 10.0,
'topic': 'topic3'}
],
),
***/
(Here the third publishing node is a publishing lifecycle node for the sake of test, though the behavior can be observed also when replacing it with another standard publishing node).
Run with the command:
ros2 launch composable_nodes_test_pkg test_composable_nodes.launch.py
Then swap the publish frequency parameter between publisher_node2 and publisher_node3:
/***
ComposableNode(
package='composable_nodes_test_pkg',
plugin='PublisherNode',
name='publisher_node1',
parameters=[
{'publish_frequency': 30.0,
'topic': 'topic1'}
],
),
ComposableNode(
package='composable_nodes_test_pkg',
plugin='PublisherNode',
name='publisher_node2',
parameters=[
{'publish_frequency': 10.0,
'topic': 'topic2'}
],
),
ComposableNode(
package='composable_nodes_test_pkg',
plugin='PublisherLifecycleNode',
name='publisher_node3',
parameters=[
{'publish_frequency': 20.0,
'topic': 'topic3'}
],
),
***/
and run again.
The synchronized callback in the subscribing nodes should be called consistently in both situations, regardless of the publishing frequencies.
In the first case (30, 20, 10 Hz), the Synchronizer callback works as expected. However, in the second case (30, 10, 20 Hz), the callback stops being called after a certain amount of time.
This behavior suggests that the ApproximateTime Synchronizer in ROS2 may have inconsistent performance when one of the topics has a higher message rate than the others. The issue seems to be specifically related to the relative frequencies of the publishers, particularly when the third topic has a higher frequency than at least one of the other two.
I try to get a time_synchronizer running with an Odometry and a LaserScan topic. The example provided in the README uses subscribers as normal function variables whereas in my case the subscriber are a class member variable.
My header
/* Polygon Explorer ROS class
*/
#pragma once
#include "geometry_msgs/msg/pose_with_covariance.hpp"
#include "kindr/Core"
#include "message_filters/subscriber.h"
#include "message_filters/time_synchronizer.h"
#include "nav_msgs/msg/odometry.hpp"
#include "poly_exploration/PolygonExplorer.h"
#include "poly_exploration/PolygonExplorerInterface.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
class PolygonExplorerNode : public rclcpp::Node,
public PolygonExplorerInterface {
public:
PolygonExplorerNode();
virtual void updateVisualizationCallback(const PoseGraph pose_graph) override;
private:
void subscriberCallback(
const std::shared_ptr<nav_msgs::msg::Odometry>& odometry,
const std::shared_ptr<sensor_msgs::msg::LaserScan>& laser_scan);
void convertFromRosGeometryMsg(
const geometry_msgs::msg::PoseWithCovariance& geometryPoseMsg,
Pose& pose);
void convertFromRosGeometryMsg(
const geometry_msgs::msg::Quaternion& geometryQuaternionMsg,
Rotation& rotationQuaternion);
void convertFromRosGeometryMsg(
const geometry_msgs::msg::Point& geometryPointMsg, Position& position);
PolygonExplorer polygonExplorer_;
Pose previousPose_;
message_filters::Subscriber<nav_msgs::msg::Odometry> odometrySubscription_;
message_filters::Subscriber<sensor_msgs::msg::LaserScan> laserSubscription_;
message_filters::TimeSynchronizer<nav_msgs::msg::Odometry,
sensor_msgs::msg::LaserScan>
timeSynchronizer_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr
poseGraphVisualizationPublisher_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
polygonVisualizationPublisher_;
};
My implementation
/* Polygon Explorer ROS class
*/
#include "PolygonExplorerNode.h"
#include <glog/logging.h>
#include <functional>
PolygonExplorerNode::PolygonExplorerNode()
: Node("polygon_explorer_node"),
odometrySubscription_(
message_filters::Subscriber<nav_msgs::msg::Odometry>(this,
"odom_11_odom")),
laserSubscription_(
message_filters::Subscriber<sensor_msgs::msg::LaserScan>(
this, "scan_11_laser_front")),
timeSynchronizer_(odometrySubscription_, laserSubscription_, 1) {
polygonExplorer_.setCallBack(this);
timeSynchronizer_.registerCallback(
std::bind(&PolygonExplorerNode::subscriberCallback, this,
std::placeholders::_1, std::placeholders::_2));
} /* ----- end of method PolygonExplorerNode::PolygonExplorerNode
(constructor) ----- */
void PolygonExplorerNode::subscriberCallback(
const std::shared_ptr<nav_msgs::msg::Odometry>& odometry,
const std::shared_ptr<sensor_msgs::msg::LaserScan>& laser_scan) {
std::vector<PolygonPoint> polygon_points;
for (unsigned int i = 0; i < laser_scan->ranges.size(); ++i) {
double theta = laser_scan->angle_min + i * laser_scan->angle_increment;
double x = laser_scan->ranges.at(i) * cos(theta);
double y = laser_scan->ranges.at(i) * sin(theta);
PointType point_type;
if (laser_scan->ranges.at(i) >= laser_scan->range_max) {
point_type = PointType::MAX_RANGE;
} else {
point_type = PointType::OBSTACLE;
}
polygon_points.emplace_back(x, y, point_type);
}
Polygon polygon(polygon_points);
Pose current_pose;
convertFromRosGeometryMsg(odometry->pose, current_pose);
auto position_diff = current_pose.getPosition() - previousPose_.getPosition();
// Orientation difference of quaternions
// (http://www.boris-belousov.net/2016/12/01/quat-dist/)
auto orientation_diff =
current_pose.getRotation() * previousPose_.getRotation().conjugated();
// Transform transformation into frame of previous pose
auto transformation_previous_pose_current_pose =
Pose(previousPose_.transform(position_diff),
previousPose_.getRotation() * orientation_diff);
polygonExplorer_.addPose(transformation_previous_pose_current_pose, polygon);
previousPose_ = current_pose;
}
void PolygonExplorerNode::convertFromRosGeometryMsg(
const geometry_msgs::msg::PoseWithCovariance& geometryPoseMsg, Pose& pose) {
convertFromRosGeometryMsg(geometryPoseMsg.pose.position, pose.getPosition());
// This is the definition of ROS Geometry pose.
typedef kindr::RotationQuaternion<double> RotationQuaternionGeometryPoseLike;
RotationQuaternionGeometryPoseLike rotation;
convertFromRosGeometryMsg(geometryPoseMsg.pose.orientation, rotation);
pose.getRotation() = rotation;
}
void PolygonExplorerNode::convertFromRosGeometryMsg(
const geometry_msgs::msg::Quaternion& geometryQuaternionMsg,
Rotation& rotationQuaternion) {
rotationQuaternion.setValues(geometryQuaternionMsg.w, geometryQuaternionMsg.x,
geometryQuaternionMsg.y,
geometryQuaternionMsg.z);
}
void PolygonExplorerNode::convertFromRosGeometryMsg(
const geometry_msgs::msg::Point& geometryPointMsg, Position& position) {
position.x() = geometryPointMsg.x;
position.y() = geometryPointMsg.y;
position.z() = geometryPointMsg.z;
}
void PolygonExplorerNode::updateVisualizationCallback(
const PoseGraph pose_graph) {
}
poseGraphVisualizationPublisher_->publish(pose_graph_marker);
}
#include "class_loader/register_macro.hpp"
When I try to compile it, I get the following errors:
--- stderr: poly_exploration_ros
In file included from /opt/ros/crystal/include/message_filters/synchronizer.h:47:0,
from /opt/ros/crystal/include/message_filters/time_synchronizer.h:41,
from /home/andreasziegler/OpenSource/poly-exploration_ws/src/poly_exploration_ros/include/PolygonExplorerNode.h:9,
from /home/andreasziegler/OpenSource/poly-exploration_ws/src/poly_exploration_ros/src/PolygonExplorerNode.cpp:4:
/opt/ros/crystal/include/message_filters/signal9.h: In instantiation of ‘message_filters::Connection message_filters::Signal9<M0, M1, M2, M3, M4, M5, M6, M7, M8>::addCallback(C&) [with C = const std::_Bind<void (PolygonExplorerNode::*(PolygonExplorerNode*, std::_Placeholder<1>, std::_Placeholder<2>))(const std::shared_ptr<nav_msgs::msg::Odometry_<std::allocator<void> > >&, const std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > >&)>; M0 = nav_msgs::msg::Odometry_<std::allocator<void> >; M1 = sensor_msgs::msg::LaserScan_<std::allocator<void> >; M2 = message_filters::NullType; M3 = message_filters::NullType; M4 = message_filters::NullType; M5 = message_filters::NullType; M6 = message_filters::NullType; M7 = message_filters::NullType; M8 = message_filters::NullType]’:
/opt/ros/crystal/include/message_filters/synchronizer.h:298:40: required from ‘message_filters::Connection message_filters::Synchronizer<Policy>::registerCallback(const C&) [with C = std::_Bind<void (PolygonExplorerNode::*(PolygonExplorerNode*, std::_Placeholder<1>, std::_Placeholder<2>))(const std::shared_ptr<nav_msgs::msg::Odometry_<std::allocator<void> > >&, const std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > >&)>; Policy = message_filters::sync_policies::ExactTime<nav_msgs::msg::Odometry_<std::allocator<void> >, sensor_msgs::msg::LaserScan_<std::allocator<void> >, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType>]’
/home/andreasziegler/OpenSource/poly-exploration_ws/src/poly_exploration_ros/src/PolygonExplorerNode.cpp:20:62: required from here
/opt/ros/crystal/include/message_filters/signal9.h:296:40: error: no matching function for call to ‘message_filters::Signal9<nav_msgs::msg::Odometry_<std::allocator<void> >, sensor_msgs::msg::LaserScan_<std::allocator<void> >, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType>::addCallback<const M0ConstPtr&, const M1ConstPtr&, const M2ConstPtr&, const M3ConstPtr&, const M4ConstPtr&, const M5ConstPtr&, const M6ConstPtr&, const M7ConstPtr&, const M8ConstPtr&>(std::_Bind_helper<false, const std::_Bind<void (PolygonExplorerNode::*(PolygonExplorerNode*, std::_Placeholder<1>, std::_Placeholder<2>))(const std::shared_ptr<nav_msgs::msg::Odometry_<std::allocator<void> > >&, const std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > >&)>&, const std::_Placeholder<1>&, const std::_Placeholder<2>&, const std::_Placeholder<3>&, const std::_Placeholder<4>&, const std::_Placeholder<5>&, const std::_Placeholder<6>&, const std::_Placeholder<7>&, const std::_Placeholder<8>&, const std::_Placeholder<9>&>::type)’
return addCallback<const M0ConstPtr&,
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
const M1ConstPtr&,
~~~~~~~~~~~~~~~~~~
const M2ConstPtr&,
~~~~~~~~~~~~~~~~~~
const M3ConstPtr&,
~~~~~~~~~~~~~~~~~~
const M4ConstPtr&,
~~~~~~~~~~~~~~~~~~
const M5ConstPtr&,
~~~~~~~~~~~~~~~~~~
const M6ConstPtr&,
~~~~~~~~~~~~~~~~~~
const M7ConstPtr&,
~~~~~~~~~~~~~~~~~~
const M8ConstPtr&>(std::bind(callback, _1, _2, _3, _4, _5, _6, _7, _8, _9));
I know that you can set allow_headerless = True
in python, but is there an equivalent option for C++? Are there plans to add it in the future?
HI I'm tryng to get the name of the publisher but I noticed that there is no an equivalent method as in ROS1 called getPublisherName(). Is it missing or there is another way to get the name of the publisher? Thanks for your time
Currently, the ApproximateTimeSynchronizer only allows the execution of one callback simultaneously, as the call to self.signalMessage(*msgs)
is inside of the lock.
For parallel execution oif callbacks (e.g. using a MultithreadedExecutor) it would be desirable to have the callback call outside of this lock. Parallel threads could then acquire the lock again and start comparing timestamps. This would allow a higher throughput for callbacks that take some time to process, in our case a deep learning model.
Additionally, the usage of a context manager for the lock and more inline comments would probably make the code more readable.
To sum it up, handle inside the lock:
Afterwards, outside of the lock
What do you think about this? We will implement this change internally and report if we can parallel execution to work.
Since c++11 already support variadic template, should synchronizer have no topic number limit or is it another way to achieve this without modifying it?
I failed to have callback function called using message filters. So I created a minimal project to verify the interface and functionality of message filters. In
this project, I add two publishers to publish identical messages into two topics. Then in main function, I try to synchronize them with message filters. But the callback functions still has no response. What's wrong with my code? I ask for your kindly help. Thanks very much.
OS: Ubuntu 18.04
ROS2: Dashing release
publisher.cpp
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/time.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <std_msgs/msg/header.hpp>
#include <iostream>
#include <memory>
#include <unistd.h>
class Publisher1 : public rclcpp::Node
{
public:
Publisher1() : Node("publisher") {};
};
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("publisher");
rclcpp::Publisher<std_msgs::msg::Header>::SharedPtr pub1, pub2;
pub1 = node->create_publisher<std_msgs::msg::Header>("/publish1", 16);
pub2 = node->create_publisher<std_msgs::msg::Header>("/publish2", 16);
std_msgs::msg::Header msg1, msg2;
while (rclcpp::ok())
{
std::cout << "publishing" << std::endl;
rclcpp::Time time= node->now();
msg1.stamp = time;
msg2.stamp = time;
pub1->publish(msg1);
pub2->publish(msg2);
rclcpp::spin_some(node);
sleep(1);
}
}
main.cpp
#include <rclcpp/rclcpp.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <std_msgs/msg/header.hpp>
#include <iostream>
#include <memory>
class MessageFilterTest : public rclcpp::Node
{
public:
MessageFilterTest() : Node("test")
{
rclcpp::Node::SharedPtr node = std::shared_ptr<rclcpp::Node>(this);
cam_sub_ = std::make_unique<camSub>(node, "/publisher1");
obj_sub_ = std::make_unique<objSub>(node, "/publisher2");
sync_sub_ = std::make_unique<sync>(*cam_sub_, *obj_sub_, 10);
sync_sub_->registerCallback(std::bind(&MessageFilterTest::callback, this, std::placeholders::_1, std::placeholders::_2));
};
private:
using camSub = message_filters::Subscriber<std_msgs::msg::Header>;
using objSub = message_filters::Subscriber<std_msgs::msg::Header>;
using sync =
message_filters::TimeSynchronizer<std_msgs::msg::Header, std_msgs::msg::Header>;
std::unique_ptr<camSub> cam_sub_;
std::unique_ptr<objSub> obj_sub_;
std::unique_ptr<sync> sync_sub_;
void callback(const std_msgs::msg::Header::ConstSharedPtr msg1, const std_msgs::msg::Header::ConstSharedPtr msg2) { std::cout << "callback" << std::endl; };
};
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
//rclcpp::executors::SingleThreadedExecutor exec;
auto message_filter_test_node = std::make_shared<MessageFilterTest>();
rclcpp::spin(message_filter_test_node);
rclcpp::shutdown();
return 0;
}
In order to get zero-copying inter-communication, std::unique_ptr
are required. Unfortunately message_filters does not yet support std::unique_ptr
.
/opt/ros/foxy/include/message_filters/message_traits.h:89:30: error: ‘class rclcpp::Time’ has no member named ‘sec’
89 | return rclcpp::Time(stam.sec, stam.nanosec);
| ~~~~~^~~
/opt/ros/foxy/include/message_filters/message_traits.h:89:40: error: ‘class rclcpp::Time’ has no member named ‘nanosec’; did you mean ‘nanoseconds’?
89 | return rclcpp::Time(stam.sec, stam.nanosec);
You can find ”message_traits.h“ in your local folder ,for example /opt/ros/foxy/include/message_filters/message_traits.h
and
85line
replace
struct TimeStamp<M, typename std::enable_if<HasHeader::value>::type >
{
static rclcpp::Time value(const M& m) {
auto stamp = m.header.stamp;
auto sec=stamp.seconds();
auto nanosec=stamp.nanoseconds();
return rclcpp::Time(sec, nanosec);
}
};
I have the following piece of code (essentially a minimal node that synchronizes two topics, logs when the callback is triggered and publishes the result on a topic). I have noticed that after a certain amount of messages published to the input topics the callback stops triggering for some reason, while the node remains alive. The amount of callbacks triggered is directly related with the queue size of the SyncPolicy as well as the rate at which the input messages are being published. Am I missing something here or is this a bug?
I am using ros2 humble on Ubuntu 22.04. On the screenshot above you can see the time difference between the last message and the manual stopping of the node.
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/string.hpp>
// message filter related headers
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <memory>
class SyncNode : public rclcpp::Node {
public:
using SyncPolicy =
message_filters::sync_policies::ApproximateTime<std_msgs::msg::Bool,
std_msgs::msg::String>;
using Synchronizer = message_filters::Synchronizer<SyncPolicy>;
SyncNode() : Node("sync_publisher") {
// Setup publisher
publisher_ =
this->create_publisher<std_msgs::msg::String>("resulting_string", 5);
bool_sub_.subscribe(this, "/bool_topic");
string_sub_.subscribe(this, "/string_topic");
sync_ =
std::make_shared<Synchronizer>(SyncPolicy(200), bool_sub_, string_sub_);
sync_->registerCallback(std::bind(&SyncNode::sync_cb, this,
std::placeholders::_1,
std::placeholders::_2));
sync_->setMaxIntervalDuration(rclcpp::Duration(0, 5e+8));
}
void sync_cb(const std_msgs::msg::Bool::ConstSharedPtr& bool_msg,
const std_msgs::msg::String::ConstSharedPtr& string_msg) {
RCLCPP_INFO(this->get_logger(),
"Received synchronized string and bool data");
// Create a message and populate its data
auto message = std::make_unique<std_msgs::msg::String>();
message->data = string_msg->data;
if (bool_msg->data) {
message->data += " True";
}
else {
message->data += " False";
}
// Publish the message
publisher_->publish(std::move(message));
}
private:
message_filters::Subscriber<std_msgs::msg::Bool> bool_sub_;
message_filters::Subscriber<std_msgs::msg::String> string_sub_;
std::shared_ptr<Synchronizer> sync_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<SyncNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
I am runnning nav2 stack and the controller node is writing
[controller_server-1] 1580394739.997408374: [osg_1.local_costmap.local_costmap_rclcpp_node] [INFO] Message Filter dropping message: frame 'scan_link' at time 1580394739.922 for reason 'Unknown'
[controller_server-1] 1580394740.153582206: [osg_1.local_costmap.local_costmap_rclcpp_node] [INFO] Message Filter dropping message: frame 'scan_link' at time 1580394740.077 for reason 'Unknown'
[controller_server-1] 1580394740.314822634: [osg_1.local_costmap.local_costmap_rclcpp_node] [INFO] Message Filter dropping message: frame 'scan_link' at time 1580394740.232 for reason 'Unknown'
[controller_server-1] 1580394740.550064022: [osg_1.local_costmap.local_costmap_rclcpp_node] [INFO] Message Filter dropping message: frame 'scan_link' at time 1580394740.475 for reason 'Unknown'
[controller_server-1] 1580394740.708030249: [osg_1.local_costmap.local_costmap_rclcpp_node] [INFO] Message Filter dropping message: frame 'scan_link' at time 1580394740.625 for reason 'Unknown'
[controller_server-1] 1580394740.862123086: [osg_1.local_costmap.local_costmap_rclcpp_node] [INFO] Message Filter dropping message: frame 'scan_link' at time 1580394740.783 for reason 'Unknown'
[controller_server-1] 1580394741.255866962: [osg_1.local_costmap.local_costmap_rclcpp_node] [INFO] Message Filter dropping message: frame 'scan_link' at time 1580394741.175 for reason 'Unknown'
[controller_server-1] 1580394741.414919378: [osg_1.local_costmap.local_costmap_rclcpp_node] [INFO] Message Filter dropping message: frame 'scan_link' at time 1580394741.333 for reason 'Unknown'
And I don't know if there is a connection to dropping messages, but I get a seg fault after some seconds.
The frame id of the scan msg is definitely set.
#0 std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >::basic_string (__str=..., this=0x7f009f7fb750) at /usr/include/c++/7/bits/basic_string.h:440
440 { _M_construct(__str._M_data(), __str._M_data() + __str.length()); }
[Current thread is 1 (Thread 0x7f009f7fe700 (LWP 20512))]
(gdb) bt
#0 std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >::basic_string (__str=..., this=0x7f009f7fb750) at /usr/include/c++/7/bits/basic_string.h:440
#1 message_filters::message_traits::FrameId<sensor_msgs::msg::LaserScan_<std::allocator<void> >, void>::value[abi:cxx11](sensor_msgs::msg::LaserScan_<std::allocator<void> > const&) (m=...)
at /opt/ros/eloquent/include/message_filters/message_traits.h:67
#2 tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::signalFailure (this=this@entry=0x561c853883c0, evt=..., reason=reason@entry=tf2_ros::filter_failure_reasons::Unknown)
at /opt/ros/eloquent/include/tf2_ros/message_filter.h:683
#3 0x00007f009d5977a3 in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::messageDropped (reason=tf2_ros::filter_failure_reasons::Unknown, evt=..., this=0x561c853883c0)
at /opt/ros/eloquent/include/tf2_ros/message_filter.h:660
#4 tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::add (this=0x561c853883c0, evt=...) at /opt/ros/eloquent/include/tf2_ros/message_filter.h:426
#5 0x00007f009d598a8c in std::function<void (message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)>::operator()(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) const (__args#0=..., this=0x561c85467a48) at /usr/include/c++/7/bits/std_function.h:706
#6 message_filters::CallbackHelper1T<message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call (this=0x561c85467a40,
event=..., nonconst_force_copy=<optimized out>) at /opt/ros/eloquent/include/message_filters/signal1.h:74
#7 0x00007f009d598840 in message_filters::Signal1<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call (event=..., this=0x561c85513548) at /opt/ros/eloquent/include/message_filters/signal1.h:117
#8 message_filters::SimpleFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::signalMessage (event=..., this=0x561c85513548) at /opt/ros/eloquent/include/message_filters/simple_filter.h:133
#9 message_filters::Subscriber<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::cb (e=..., this=0x561c85513540) at /opt/ros/eloquent/include/message_filters/subscriber.h:235
#10 message_filters::Subscriber<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::subscribe(rclcpp::Node*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rmw_qos_profile_t)::{lambda(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)#1}::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) const (msg=..., __closure=<optimized out>)
at /opt/ros/eloquent/include/message_filters/subscriber.h:174
#11 std::_Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), message_filters::Subscriber<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::subscribe(rclcpp::Node*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rmw_qos_profile_t)::{lambda(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)#1}>::_M_invoke(std::_Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>&&) (__functor=..., __args#0=...) at /usr/include/c++/7/bits/std_function.h:316
#12 0x00007f009d5902e2 in std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) const (
__args#0=std::shared_ptr<const sensor_msgs::msg::LaserScan_<std::allocator<void> >> (use count 1797718528, weak count 1253155528) = {...}, this=0x561c85510b78) at /usr/include/c++/7/bits/std_function.h:706
#13 rclcpp::AnySubscriptionCallback<sensor_msgs::msg::LaserScan_<std::allocator<void> >, std::allocator<void> >::dispatch (this=0x561c85510b38,
message=std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> >> (use count 7, weak count 0) = {...}, message_info=...) at /opt/ros/eloquent/include/rclcpp/any_subscription_callback.hpp:166
#14 0x00007f009d590500 in rclcpp::Subscription<sensor_msgs::msg::LaserScan_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<sensor_msgs::msg::LaserScan_<std::allocator<void> >, std::allocator<void> > >::handle_message (this=0x561c85510a90, message=..., message_info=...) at /opt/ros/eloquent/include/rclcpp/subscription.hpp:228
#15 0x00007f00bbd5dd1f in rclcpp::executor::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) () from /opt/ros/eloquent/lib/librclcpp.so
#16 0x00007f00bbd5f045 in rclcpp::executor::Executor::execute_any_executable(rclcpp::executor::AnyExecutable&) () from /opt/ros/eloquent/lib/librclcpp.so
#17 0x00007f00bbd6571f in rclcpp::executors::SingleThreadedExecutor::spin() () from /opt/ros/eloquent/lib/librclcpp.so
#18 0x00007f00bb39032a in nav2_util::NodeThread::<lambda()>::operator() (__closure=0x561c8543c708) at /home/kin/alpen/ros2_ws/src/navigation/nav2/nav2_util/src/node_thread.cpp:29
#19 std::__invoke_impl<void, nav2_util::NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda()> > (__f=...) at /usr/include/c++/7/bits/invoke.h:60
#20 std::__invoke<nav2_util::NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda()> > (__fn=...) at /usr/include/c++/7/bits/invoke.h:95
#21 std::thread::_Invoker<std::tuple<nav2_util::NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda()> > >::_M_invoke<0> (this=0x561c8543c708) at /usr/include/c++/7/thread:234
#22 std::thread::_Invoker<std::tuple<nav2_util::NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda()> > >::operator() (this=0x561c8543c708) at /usr/include/c++/7/thread:243
#23 std::thread::_State_impl<std::thread::_Invoker<std::tuple<nav2_util::NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr)::<lambda()> > > >::_M_run(void) (this=0x561c8543c700)
at /usr/include/c++/7/thread:186
#24 0x00007f00b97a166f in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#25 0x00007f00ba6b66db in start_thread (arg=0x7f009f7fe700) at pthread_create.c:463
#26 0x00007f00b8e5e88f in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
Hi,
I am working with ROS 2- foxy distribution. I have two USB cameras simultaneously running and publishing images in topics - /usb_cam_0/image_raw and /usb_cam_1/image_raw . I want to subscribe the topics and concatenate the images and save them in a folder.
I went through this https://answers.ros.org/question/8112... and tried to adapt the same logic as,
subscription_1 = message_filters.Subscriber('/usb_cam_0/image_raw', Image)
subscription_2 = message_filters.Subscriber('/usb_cam_1/image_raw', Image)
message_filters.ApproximateTimeSynchronizer([subscription_1, subscription_2],10,1)
But this script throws me an error in the subscriber class of message filter. The error was
File "/home/device/messageFilter/src/install/message_filters/lib/python3.6/site-packages/message_filters/init.py", line 78, in init self.topic = args[2] IndexError: tuple index out of range
Unable to clear this error. Other approach which worked was
subscription_1 = self.create_subscription(Image, '/usb_cam_0/image_raw', imageClass.camera_0_callback, 1)
But here i would have to use two separate callback functions which I don't want to. I cloned the message filters package from this repository but still the same error persists.
The first logic works was from the dicsussion with ROS. But does it have a big difference with ROS2 - foxy? I am new to this platform. Any guidance would be helpful.
Regards
Niranjan
Hello!
Thank you for developing such a useful package. It has become an irreplaceable piece of our system.
In our system, the timestamp is expressed as a single uint64 number with nanosecond precision. Even if converting from nanosec
to sec
+ nanosec
is perfectly feasible, it would be cleaner if we could just use the timestamp as is.
Hence my question: would it be possible to use a custom definition of std_msgs/msg/Header Message for time synchronization with TimeSynchronizer
or ApproximateTimeSynchronizer
?
Concretely, I would like to use a header whose timestamp is a single uint64
stamp with nanosecond precision, instead of two int32
and uint32
stamps with second and nanosecond precision respectively, as in the default ROS2 definition.
If this feature is not implemented yet, I would like to contribute with a pull request. Just let me know if there is any request or suggestion on your side.
Regards
Giorgio
only find 'TimeSynchronizer' here.
We've been having an issue in Nav2 as of moving to 22.04 that our obstacle layers / costmap collision checkers haven't been working properly ros-navigation/navigation2#3014 -- e.g. no data is getting to our callbacks, which is quite the problem for a vision-based navigation system 😆
It seems to boil down to the fact that when we moved to using a single set of nodes for each process, we starting making a bunch of use of callback groups that were tested and working fine on rolling in 20.04. I checked and Galactic is on 3.X.X of this package so rolling was probably around there when it was on 20.04, but now rolling is on 4.X.X along with Humble, which includes the changes in the PR below.
What appears to be the issue is that #56 incorrectly added an error in one of the constructors for processing subscriber options which includes the callback group request.
virtual void subscribe(
NodePtr node,
const std::string& topic,
const rmw_qos_profile_t qos,
rclcpp::SubscriptionOptions options)
{
this->subscribe(node.get(), topic, qos, options);
};
virtual void subscribe(
NodeType * node,
const std::string& topic,
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);
}
So if the constructor is called with a node type pointer rather than a shared pointer then the options
aren't passed and has a warning which seems bogus (?) since the constructor just above it does this fine with another pointer type. This entirely explains the behavior we're seeing if its using the second constructor and throwing out our options because of the type of the node provided.
I believe the fix is to remove the warning and use the option
in the this->subscribe()
call in the second implementation.
The subscription created in the Subscriber __init__
has a hard-coded queue depth of 10. If qos_profile
is set as a named param like I am doing:
self.pc_filter = message_filters.Subscriber(
self,
PointCloud2,
constants.PC2_TOPIC,
qos_profile = qos_profile_sensor_data
)
Then an exception is thrown, because qos_profile is being set twice.
I am throwing around a lot of pointcloud data, and I really want to be able to specify a best effort quality of service For some of my filters.
Thank you for the clear type annotations, making this easy to track down.
for reference, the problem __init__
function is shown below:
class Subscriber(SimpleFilter):
def __init__(self, *args, **kwargs):
SimpleFilter.__init__(self)
self.node = args[0]
self.topic = args[2]
self.sub = self.node.create_subscription(*args[1:], self.callback, 10, **kwargs)
Current implementation of time synchronizer only supports callback with std::shared_ptr to a certain message.
void callback(const std::shared_ptr&);
Is it feasible to implement a callback API with std::unique_ptr as its parameter to support zero-copy mentioned in this tutorial?
Hi
While installing ros2_foxy, I am facing this error:
--- stderr: message_filters
/home/utkubuntu/ros2_foxy/src/ros2/message_filters/test/test_latest_time_policy.cpp: In member function ‘virtual void LatestTimePolicy_ChangeRateLeading_Test::TestBody()’:
/home/utkubuntu/ros2_foxy/src/ros2/message_filters/test/test_latest_time_policy.cpp:212:23: error: ‘ClockQoS’ is not a member of ‘rclcpp’; did you mean ‘Clock’?
212 | "/clock", rclcpp::ClockQoS());
| ^~~~~~~~
| Clock
/home/utkubuntu/ros2_foxy/src/ros2/message_filters/test/test_latest_time_policy.cpp: In member function ‘virtual void LatestTimePolicy_ChangeRateTrailing_Test::TestBody()’:
/home/utkubuntu/ros2_foxy/src/ros2/message_filters/test/test_latest_time_policy.cpp:298:23: error: ‘ClockQoS’ is not a member of ‘rclcpp’; did you mean ‘Clock’?
298 | "/clock", rclcpp::ClockQoS());
| ^~~~~~~~
| Clock
make[2]: *** [CMakeFiles/message_filters-test_latest_time_policy.dir/build.make:63: CMakeFiles/message_filters-test_latest_time_policy.dir/test/test_latest_time_policy.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:237: CMakeFiles/message_filters-test_latest_time_policy.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
How to resolve this?
At work, we have been using message filters for a while now.
While transitioning to LifecycleNodes
, we noticed that the interface of the message subscriber still relies on a Node type, which supports create_subscription
with a specific signature.
The intended way of the rclcpp::NodeInterfaces
seems to circumvent the use of any Node type and use specific interfaces instead.
We are currently working on implementing similar changes in other projects (see image_transport and image_transport_plugins).
This is why we depend on similar changes here.
Are there any reasons against such a change?
Since we (especially @authaldo) are working on it anyway, we would be happy to contribute a PR.
One of our CI jobs just failed because the TimeSequencer.simple unit test hung:
Start 4: message_filters-test_time_sequencer
4: Test command: /usr/bin/python3 "-u" "opt_path/share/ament_cmake_test/cmake/run_test.py" "ws_path/build/message_filters/test_results/message_filters/message_filters-test_time_sequencer.gtest.xml" "--package-name" "message_filters" "--output-file" "ws_path/build/message_filters/ament_cmake_gtest/message_filters-test_time_sequencer.txt" "--command" "ws_path/build/message_filters/message_filters-test_time_sequencer" "--gtest_output=xml:ws_path/build/message_filters/test_results/message_filters/message_filters-test_time_sequencer.gtest.xml"
4: Test timeout computed to be: 60
4: -- run_test.py: invoking following command in 'ws_path/src/path/message_filters':
4: - ws_path/build/message_filters/message_filters-test_time_sequencer --gtest_output=xml:ws_path/build/message_filters/test_results/message_filters/message_filters-test_time_sequencer.gtest.xml
4: [==========] Running 3 tests from 1 test case.
4: [----------] Global test environment set-up.
4: [----------] 3 tests from TimeSequencer
4: [ RUN ] TimeSequencer.simple
4/13 Test #4: message_filters-test_time_sequencer ............***Timeout 60.01 sec
A passing run in our CI takes about half a second to run, so this isn't a case of the test just taking a little longer than normal.
It's almost certainly getting stuck in rclcpp::spin_some(node)
on line 97 or a few lines later. The queue of things to handle is either building up faster than spin_some can process, or some work item in the queue is adding a work item to the queue so spin_some never finishes.
I need to dive into this a little more to see what's being tested, but maybe we could spin until something specific happens, or spin until future complete? If someone reads this and has a suggestion, please chime in
Hi,
Per ros-navigation/navigation2#1889, we're seeing memory leaks in Navigation2 in servers that make use of message filters that we believe message filters is the root cause (potentially could be TF2 ROS within message filters).
Despite having a <test_depend>
on ament_lint_auto
, this project is missing a dependency on ament_lint_common
(or a subset of linters). The missing dependencies means that, for example, the package is not checking copyrights.
Hi,
I'm working on ROS2 with Foxy version and in my project I find memory leak in message_filters::Synchronizer
when testing with Sanitizer tool. So I write a small test case, then find that when shutting down the node, there is 8 bytes heap-use-after-free issue in message_filters::Signal1<sensor_msgs::msg::Image_<std::allocator<void> > >::removeCallback
. Here is the code.
#include <signal.h>
#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/msg/image.hpp>
class SLAMNode: public rclcpp::Node
{
public:
SLAMNode(): rclcpp::Node("test"), sync_policy_(10),
image_sync_(sync_policy_) {
auto subscribe_image_topic = [this](ImageSubscriberFilter &sub, std::string topic, const std::string = "raw") {
sub.subscribe(this, topic, rclcpp::SensorDataQoS().get_rmw_qos_profile());
};
subscribe_image_topic(sub_image_, "image");
subscribe_image_topic(sub_depth_, "depth");
image_sync_.connectInput(sub_image_, sub_depth_);
image_sync_.registerCallback(
std::bind(&SLAMNode::rgbd_callback, this, std::placeholders::_1, std::placeholders::_2));
};
~SLAMNode() {};
void rgbd_callback(const sensor_msgs::msg::Image::ConstSharedPtr msgRGB,
const sensor_msgs::msg::Image::ConstSharedPtr msgD) {
std::cout << "Callback " << std::endl;
}
private:
typedef message_filters::Subscriber<sensor_msgs::msg::Image> ImageSubscriberFilter;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image> sync_policy;
sync_policy sync_policy_;
message_filters::Synchronizer<sync_policy> image_sync_;
ImageSubscriberFilter sub_image_, sub_depth_, sub_right_image_, sub_mono_;
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
std::shared_ptr<SLAMNode> node = std::make_shared<SLAMNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return EXIT_SUCCESS;
}
Regards,
Han
Hi, using Humble, I would like to create a dynamic amount of subscribers. With standard subscribers, I can do:
std::vector<rclcpp::Subscription<msgType>::SharedPtr> subscriptions_;
for (auto& topic : topics_){ // topics_ is an std::vector<std::string>
subscriptions_.push_back(node->create_subscription<msgType>(
topic.c_str(), 10, std::bind(&MyClass::dataCb, this, std::placeholders::_1)));
}
And it works fine.
But when using Subscriber, the compiler returns an error since it inherits from noncopyable. I suppose there were some design choices leading to this. Is there any workaround or fix I could apply to handle this case?
Thanks!
Hello everyone,
This is not an actual bug or an explicit feature request.
I am currently using a Foxy-based node to synchronize two messages the node is subscribed to:
PointCloud2
messageImage
message.The two timestamps are not exactly the same, hence, I use a Synchronizer
with ApproximateTime
policy. Everything works fine whenever the two messages are considered synchronized by the Synchronizer
: my callback is executed, and I process both messages.
However, I would like to process the PointCloud2
message alone, when the two messages are not considered synchronized.
In Synchronizer
based on ExactTime
policy, I see that there is a method registerDropCallback
that apparently should do the trick. Unfortunately, I cannot use this policy in my system, since message timestamps are not exactly the same between point cloud and image.
Therefore, my question is: how is it possible to achieve the same behavior of registerDropCallback
, but for a Synchronizer
with ApproximateTime
policy?
Thank you
There is one commit deleted connection method
(2947b20), see comment by @tfoote that "TODO(tfoote) find why", this method will be added later? or is there any replace method could be call instead?
the delated code:
Connection::Connection(const WithConnectionDisconnectFunction& func, boost::signals2::connection c)
: connection_disconnect_(func)
, connection_(c)
{
}
I am porting tf2_ros/message_filter.h on ros2 which used this method, appreciate if anyone could help.
message_filters::Connection registerFailureCallback(const FailureCallback& callback)
{
std::unique_lock<std::mutex> lock(failure_signal_mutex_);
return message_filters::Connection(std::bind(&MessageFilter::disconnectFailure, this, _1), failure_signal_.connect(callback));
}
Hi, I use the python version of the message filters now. However, when I subscribe to multiple topics, if one or more of them don't work, the other normal topics can't be synchronized. Therefore, how to ensure the synchronization of the normal topics and give up these wrong topics ?
Thanks!
Identified in #7 specifically #7 (comment)
This is not currently in our default build so we don't get nightly reports on this.
I am trying to implement a system of three nodes like this:
foo
: publishes to /topic_foo
(latched)bar
: publishes to /topic_bar
continuouslysyncer
: approximately synchronizes messages from /topic_foo
and /topic_bar
I am able to achieve this using message_filters in Python, but not so with C++. I am using ROS 2 Humble.
Check out this Gist message_filter_minimum.py for a Python implementation that does the above. To run this program, just do python message_filter_minimum.py
inside a ROS 2 Humble environment.
Check out this Gist message_filter_minimum.cpp for my attempt at an equivalent C++ implementation. To run this program, add the following to your CMakeLists.txt (of some arbitrary package):
add_executable(message_filter_minimum
path/to/message_filter_minimum.cpp)
ament_target_dependencies(message_filter_minimum
rclcpp std_msgs geometry_msgs message_filters)
install(TARGETS
message_filter_minimum
DESTINATION lib/${PROJECT_NAME}
)
Then, after compilation, you can run it with ros2 run <pkg_name> message_filter_minimum
.
When running the Python program, you will get the expected behavior, which looks like:
$ python message_filter_minimum.py
[INFO] [1681059940.027053883] [foo]: foo published!
[INFO] [1681059940.537453616] [bar]: bar published!
[INFO] [1681059940.538126797] [syncer]: Received messages!!
[INFO] [1681059940.538889366] [syncer]: from Foo: geometry_msgs.msg.PointStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg
.Time(sec=1681059940, nanosec=17754646), frame_id=''), point=geometry_msgs.msg.Point(x=1.0, y=0.0, z=0.0))
[INFO] [1681059940.539450511] [syncer]: from Bar: geometry_msgs.msg.PointStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg
.Time(sec=1681059940, nanosec=530389836), frame_id=''), point=geometry_msgs.msg.Point(x=0.0, y=0.0, z=1.0))
[INFO] [1681059941.039520412] [bar]: bar published!
[INFO] [1681059941.537791646] [bar]: bar published!
[INFO] [1681059942.040768503] [bar]: bar published!
[INFO] [1681059942.538500992] [bar]: bar published!
[INFO] [1681059943.039352663] [bar]: bar published!
[INFO] [1681059943.537148395] [bar]: bar published!
[INFO] [1681059944.035679004] [bar]: bar published!
To explain: Since /topic_foo
is latched, the synchronizer Syncer
's callback will only ever be called once. Afterwards, you should only see bar published!
printed on the screen.
When running the C++ program, however, no synchronization happens. I only get
$ ros2 run <pkg_name> message_filter_minimum
[INFO] [1681060193.486347829] [foo]: foo published!
[INFO] [1681060193.987998543] [bar]: bar published!
[INFO] [1681060194.488068847] [bar]: bar published!
[INFO] [1681060194.988011731] [bar]: bar published!
[INFO] [1681060195.488036074] [bar]: bar published!
Questions
Thank you!
Could someone explain me, How to work with this package and when do I compile which file. Thanks)
I use Boost in the parent project, including the message filters and get the "Reference "_1" is ambigious" everywhere where the placeholders is defined. Even though the using namespace std::placeholders;
is defined in each function scope.
template<typename P0, typename P1>
Connection addCallback(void(*callback)(P0, P1))
{
using namespace std::placeholders;
return addCallback(std::function<void(P0, P1, NullP, NullP, NullP, NullP, NullP, NullP, NullP)>(std::bind(callback, _1, _2)));
}
template<typename P0, typename P1, typename P2>
Connection addCallback(void(*callback)(P0, P1, P2))
{
using namespace std::placeholders;
return addCallback(std::function<void(P0, P1, P2, NullP, NullP, NullP, NullP, NullP, NullP)>(std::bind(callback, _1, _2, _3)));
}
It should lookup the variable in the local namespace first I presume? as stated here https://stackoverflow.com/a/9381283/1829511 but it seems that the global namespace somehow takes precedence? Could this happen if somewhere any of my libs has a using namespace boost
for instance?
Any other ideas why this happens ?
My solution was to define the fully qualified name
std::placeholders::_1
but that is a bit of a pain.
This is the follow-up to the future optimization of the logger using in #7 for the following comment from @tfoote , now it's using exclusive named logger instead of the root logger within the initial porting.
However we've been looking at this elsewhere and for useful logging we will likely need to pass in the logger objects so that they can be remapped and otherwise grouped appropriately. So adding the logger to the init signature wouldn't be unreasonable. We can ticket that as a follow up.
I'm not totally sure but I think LifecycleNodes are not supported.
When using message_filters in ros2, it'd be nice to be able to set SubscriptionOptions when creating Subscribers as one would when creating subscriptions via the underlying rclcpp::Node API. However passing such SubscriptionOptions is not yet possible using the current Subscriber constructors from the message_filters API. I'd like to suggest updating message_filters to follow that of rclcpp::Node.
Required Info:
Extend message_filters::Subscriber so that the underlying rclcpp::SubscriptionOptions can be set.
Downstream packages such as image_transport
should also be updated to support this.
In principle, there should be no issues* in filtering native structures associated with a ROS2 message (rclcpp API, type adaptation feature).
*An "header" attribute shall be present in the native structure to retrieve timestamps(?).
However, this is currently not supported by this package. Is there any planned activity already / would you like to review a PR? I'm currently working on it
So I have a callback that I need to await a transform from tf2 in. Here is a MWE using a regular ros2 subscription that works
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image as ImgMsg
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
class TestAsync(Node):
def __init__(self) -> None:
super().__init__("test_async")
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.create_subscription(
ImgMsg, "zed2/zed_node/rgb/image_rect_color", self.async_callback, 10
)
async def async_callback(self, msg: ImgMsg) -> None:
self.get_logger().info("Entered callback")
now = rclpy.time.Time.from_msg(msg.header.stamp)
try:
tf = await self.tf_buffer.lookup_transform_async(
msg.header.frame_id, "odom", now
)
self.get_logger().info(f"Got {repr(tf)}")
except TransformException as ex:
self.get_logger().info(f"Exception: {ex}")
def main():
rclpy.init()
node = TestAsync()
rclpy.spin(node)
rclpy.shutdown()
When I switch this MWE to using message_filters like this
img_sub = message_filters.Subscriber(
self, ImgMsg, "zed2/zed_node/rgb/image_rect_color"
)
img_sync = message_filters.TimeSynchronizer([img_sub], 1)
img_sync.registerCallback(self.async_callback)
instead of
self.create_subscription(
ImgMsg, "zed2/zed_node/rgb/image_rect_color", self.async_callback, 10
)
I get the error
timeWarning: coroutine 'TestAsync.async_callback' was never awaited
cb(*(msg + args))
RuntimeWarning: Enable tracemalloc to get the object allocation traceback
and the callback does not seem to be called correctly (I see no logs).
This is a toy example for demonstration but in my use case I am trying to use message_filters.TimeSynchronizer
to synchronize messages from callbacks for multiple topics. Is there something I am doing wrong or is this a bug?
I was experimenting with using message filter caches to lookup message data and hit an odd behavior where the interpolation time always needs to be RCL_SYSTEM_TIME. Otherwise you will get a:
terminate called after throwing an instance of 'std::runtime_error' what(): can't compare times with different time sources
It is my understanding that ROS2 design documents intend the preferred timestamp type to be RCL_ROS_TIME. Is this correct?
If so, I can put a pull request in for a one line change of:
to:
return rclcpp::Time(stamp);
This should use the message contructor instead of the sec/nanosec contructor, which defaults to RCL_ROS_TIME.
Alternately:
return rclcpp::Time(stamp.sec, stamp.nanosec, RCL_ROS_TIME);
In the TimeSynchronizer class the following error is thrown because msg.header.stamp (rcl_interfaces/builtin_interfaces/msg/Time.msg) is not hashable (perhaps it was in ROS 1), see below:
File "/home/jamie/my_ros2_ws/install/message_filters/lib/python3.6/site-packages/message_filters/__init__.py", line 227, in add
my_queue[msg.header.stamp] = msg
TypeError: unhashable type: 'Time'
This occurs at message_filters/__init__.py#L227.
In include/message_filters/sync_policies/latest_time.h
rates_ is array of struct Rate. I thought that its index corresponds to channel index. But its initialization in function initialize_rate<i>()
doesn't put the struct (including alphas and prev) to index "i". Instead "push_back" is used. As a result, the index of rates_ seems to represent the order of arrival of the first message of each channel. This appears confusing to me. How can I properly set the alphas for each channel?
Here is the source code of initialize_rate():
template<int i>
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()));
}
}
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.