Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

package build fails when message_filters::Subscriber is assigned a callback for receving a new message #163

Open
theksg opened this issue Jan 15, 2025 · 1 comment
Labels
more-information-needed Further information is required

Comments

@theksg
Copy link

theksg commented Jan 15, 2025

ROS 2 Jazzy, Ubuntu 24.04.1(WSL)

I am syncing 2 subscribers and running a callbck with synced messages. It is working fine.

For one of the subscribers, I want to execute a callback whenever new message is received.

TimeSyncNode() : Node("sync_node")
{
  rclcpp::QoS qos = rclcpp::QoS(10);
  temp_pub = this->create_publisher<sensor_msgs::msg::Temperature>("temp", qos);
  fluid_pub = this->create_publisher<sensor_msgs::msg::FluidPressure>("fluid", qos);

  temp_sub.subscribe(this, "temp", qos.get_rmw_qos_profile());
  fluid_sub.subscribe(this, "fluid", qos.get_rmw_qos_profile());

  // Register a callback for temp_sub
  temp_sub.registerCallback(std::bind(&TimeSyncNode::TempSubCallback, this, _1));

  timer = this->create_wall_timer(500ms, std::bind(&TimeSyncNode::TimerCallback, this));
  second_timer = this->create_wall_timer(550ms, std::bind(&TimeSyncNode::SecondTimerCallback, this));

  uint32_t queue_size = 10;
  sync = std::make_shared<message_filters::Synchronizer<message_filters::sync_policies::
    ApproximateTime<sensor_msgs::msg::Temperature, sensor_msgs::msg::FluidPressure>>>(
    message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Temperature,
    sensor_msgs::msg::FluidPressure>(queue_size), temp_sub, fluid_sub);

  sync->setAgePenalty(0.50);
  sync->registerCallback(std::bind(&TimeSyncNode::SyncCallback, this, _1, _2));

}

void TempSubCallback(const sensor_msgs::msg::Temperature::SharedPtr msg) {
  // Log the receipt of a new temperature message
  RCLCPP_INFO(this->get_logger(), "Received temperature message.");
  // Add any additional processing logic here
}

I am getting build failure, I am trying this because in doc page, it is shown that callback can be attached to the subscriber,
https://github.com/ros2/message_filters/blob/jazzy/doc/index.rst#:~:text=Python%3A%20callback(msg)-,2.2%20Example%20(C%2B%2B),-message_filters%3A%3ASubscriber%3Cexample_interfaces

but when i try to do the same, i get build failure as following:

Starting >>> msg_filter_demo
--- stderr: msg_filter_demo
/home/sakshamg/msg_filter_ws2/src/msg_filter_demo/src/approximate_time_synchronizer.cpp: In member function ‘void TimeSyncNode::TempSubCallback(sensor_msgs::msg::Temperature_<std::allocator<void> >::SharedPtr’:
/home/sakshamg/msg_filter_ws2/src/msg_filter_demo/src/approximate_time_synchronizer.cpp:93:69: warning: unused parameter ‘msg’ [-Wunused-parameter]
   93 | void TempSubCallback(const sensor_msgs::msg::Temperature::SharedPtr msg) {
      |                      ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~
In file included from /opt/ros/jazzy/include/message_filters/message_filters/subscriber.h:44,
                 from /home/sakshamg/msg_filter_ws2/src/msg_filter_demo/src/approximate_time_synchronizer.cpp:7:
/opt/ros/jazzy/include/message_filters/message_filters/simple_filter.h: In instantiation of ‘message_filters::Connection message_filters::SimpleFilter<M>::registerCallback(const C&) [with C = std::_Bind<void (TimeSyncNode::*(TimeSyncNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::Temperature_<std::allocator<void> > >)>; M = sensor_msgs::msg::Temperature_<std::allocator<void> >]’:
/home/sakshamg/msg_filter_ws2/src/msg_filter_demo/src/approximate_time_synchronizer.cpp:33:30:   required from here
/opt/ros/jazzy/include/message_filters/message_filters/simple_filter.h:72:67: error: no matching function for call to ‘std::function<void(const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&)>::function(const std::_Bind<void (TimeSyncNode::*(TimeSyncNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::Temperature_<std::allocator<void> > >)>&)’
   72 |     typename CallbackHelper1<M>::Ptr helper = signal_.addCallback(Callback(callback));
      |                                                                   ^~~~~~~~~~~~~~~~~~
In file included from /usr/include/c++/13/functional:59,
                 from /opt/ros/jazzy/include/rclcpp/rclcpp/utilities.hpp:19,
                 from /opt/ros/jazzy/include/rclcpp/rclcpp/logging.hpp:25,
                 from /opt/ros/jazzy/include/rclcpp/rclcpp/copy_all_parameter_values.hpp:27,
                 from /opt/ros/jazzy/include/rclcpp/rclcpp/rclcpp.hpp:171,
                 from /home/sakshamg/msg_filter_ws2/src/msg_filter_demo/src/approximate_time_synchronizer.cpp:1:
/usr/include/c++/13/bits/std_function.h:435:9: note: candidate: ‘template<class _Functor, class _Constraints> std::function<_Res(_ArgTypes ...)>::function(_Functor&&) [with _Constraints = _Functor; _Res = void; _ArgTypes = {const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&}]’
  435 |         function(_Functor&& __f)
      |         ^~~~~~~~
/usr/include/c++/13/bits/std_function.h:435:9: note:   template argument deduction/substitution failed:
In file included from /usr/include/c++/13/bits/move.h:37,
                 from /usr/include/c++/13/bits/new_allocator.h:36,
                 from /usr/include/x86_64-linux-gnu/c++/13/bits/c++allocator.h:33,
                 from /usr/include/c++/13/bits/allocator.h:46,
                 from /usr/include/c++/13/memory:65,
                 from /opt/ros/jazzy/include/rclcpp/rclcpp/rclcpp.hpp:169:
/usr/include/c++/13/type_traits: In substitution of ‘template<bool _Cond, class _Tp> using std::__enable_if_t = typename std::enable_if::type [with bool _Cond = false; _Tp = void]’:
/usr/include/c++/13/bits/std_function.h:353:8:   required by substitution of ‘template<class _Res, class ... _ArgTypes> template<class _Cond, class _Tp> using std::function<_Res(_ArgTypes ...)>::_Requires = std::__enable_if_t<_Cond::value, _Tp> [with _Cond = std::function<void(const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&)>::_Callable<const std::_Bind<void (TimeSyncNode::*(TimeSyncNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::Temperature_<std::allocator<void> > >)>&, std::_Bind<void (TimeSyncNode::*(TimeSyncNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::Temperature_<std::allocator<void> > >)>, std::__invoke_result<std::_Bind<void (TimeSyncNode::*(TimeSyncNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::Temperature_<std::allocator<void> > >)>&, const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&> >; _Tp = void; _Res = void; _ArgTypes = {const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&}]’
/usr/include/c++/13/bits/std_function.h:434:9:   required from ‘message_filters::Connection message_filters::SimpleFilter<M>::registerCallback(const C&) [with C = std::_Bind<void (TimeSyncNode::*(TimeSyncNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::Temperature_<std::allocator<void> > >)>; M = sensor_msgs::msg::Temperature_<std::allocator<void> >]’
/home/sakshamg/msg_filter_ws2/src/msg_filter_demo/src/approximate_time_synchronizer.cpp:33:30:   required from here
/usr/include/c++/13/type_traits:116:11: error: no type named ‘type’ in ‘struct std::enable_if<false, void>’
  116 |     using __enable_if_t = typename enable_if<_Cond, _Tp>::type;
      |           ^~~~~~~~~~~~~
/opt/ros/jazzy/include/message_filters/message_filters/simple_filter.h: In instantiation of ‘message_filters::Connection message_filters::SimpleFilter<M>::registerCallback(const C&) [with C = std::_Bind<void (TimeSyncNode::*(TimeSyncNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::Temperature_<std::allocator<void> > >)>; M = sensor_msgs::msg::Temperature_<std::allocator<void> >]’:
/home/sakshamg/msg_filter_ws2/src/msg_filter_demo/src/approximate_time_synchronizer.cpp:33:30:   required from here
/usr/include/c++/13/bits/std_function.h:404:7: note: candidate: ‘std::function<_Res(_ArgTypes ...)>::function(std::function<_Res(_ArgTypes ...)>&&) [with _Res = void; _ArgTypes = {const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&}]’
  404 |       function(function&& __x) noexcept
      |       ^~~~~~~~
/usr/include/c++/13/bits/std_function.h:404:27: note:   no known conversion for argument 1 from ‘const std::_Bind<void (TimeSyncNode::*(TimeSyncNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::Temperature_<std::allocator<void> > >)>’ to ‘std::function<void(const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&)>&&’
  404 |       function(function&& __x) noexcept
      |                ~~~~~~~~~~~^~~
/usr/include/c++/13/bits/std_function.h:386:7: note: candidate: ‘std::function<_Res(_ArgTypes ...)>::function(const std::function<_Res(_ArgTypes ...)>&) [with _Res = void; _ArgTypes = {const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&}]’
  386 |       function(const function& __x)
      |       ^~~~~~~~
/usr/include/c++/13/bits/std_function.h:386:32: note:   no known conversion for argument 1 from ‘const std::_Bind<void (TimeSyncNode::*(TimeSyncNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::Temperature_<std::allocator<void> > >)>’ to ‘const std::function<void(const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&)>&’
  386 |       function(const function& __x)
      |                ~~~~~~~~~~~~~~~~^~~
/usr/include/c++/13/bits/std_function.h:375:7: note: candidate: ‘std::function<_Res(_ArgTypes ...)>::function(std::nullptr_t) [with _Res = void; _ArgTypes = {const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&}; std::nullptr_t = std::nullptr_t]’
  375 |       function(nullptr_t) noexcept
      |       ^~~~~~~~
/usr/include/c++/13/bits/std_function.h:375:16: note:   no known conversion for argument 1 from ‘const std::_Bind<void (TimeSyncNode::*(TimeSyncNode*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::Temperature_<std::allocator<void> > >)>’ to ‘std::nullptr_t’
  375 |       function(nullptr_t) noexcept
      |                ^~~~~~~~~
/usr/include/c++/13/bits/std_function.h:368:7: note: candidate: ‘std::function<_Res(_ArgTypes ...)>::function() [with _Res = void; _ArgTypes = {const std::shared_ptr<const sensor_msgs::msg::Temperature_<std::allocator<void> > >&}]’
  368 |       function() noexcept
      |       ^~~~~~~~
/usr/include/c++/13/bits/std_function.h:368:7: note:   candidate expects 0 arguments, 1 provided
gmake[2]: *** [CMakeFiles/approximate_time_sync.dir/build.make:76: CMakeFiles/approximate_time_sync.dir/src/approximate_time_synchronizer.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:137: CMakeFiles/approximate_time_sync.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< msg_filter_demo [8.78s, exited with code 2]

Summary: 0 packages finished [9.26s]
  1 package failed: msg_filter_demo
  1 package had stderr output: msg_filter_demo

from the code at :https://github.com/ros2/message_filters/blob/jazzy/include/message_filters/subscriber.h, i feel that we cannot assign it a callback, let me know if I am doing something wrong, or we cannot assing callback for latest messages via messsage_filters::Subscriber

@khaledgabr77
Copy link

Hi,
Try to change from:
sync->registerCallback(std::bind(&TimeSyncNode::SyncCallback, this, _1, _2));

To:
sync->registerCallback(&TimeSyncNode::SyncCallback, this);

@clalancette clalancette added the more-information-needed Further information is required label Jan 30, 2025
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
more-information-needed Further information is required
Projects
None yet
Development

No branches or pull requests

3 participants