ros2 / rclcpp

rclcpp (ROS Client Library for C++)
Apache License 2.0
513 stars 412 forks source link

AnySubscriptionCallback doesn't accept `std::bind` callbacks with bound arguments #2429

Open HovorunB opened 4 months ago

HovorunB commented 4 months ago

Bug report

Required Info:

Steps to reproduce issue

1) Build rclcpp on a version after https://github.com/ros2/rclcpp/pull/1928 2) Try to build for example foxglove-bridge

Expected behavior

std::bind(&FoxgloveBridge::rosMessageHandler, this, channelId, clientHandle, _1), (source) is cast to std::function from any_subscription_callback.hpp

Actual behavior

Screenshot from 2024-02-19 10-56-06

Additional information

For example std::bind(&Class::method, this, std::placeholders::_1) (without bound arguments) will build fine

We also were able to fix the issue by casting the callback to std::function before passing it to the subscription

auto subscriber = this->create_generic_subscription(
      topic, datatype, qos,
      static_cast<std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)>>(std::bind(&FoxgloveBridge::rosMessageHandler, this, channelId, clientHandle, _1)),
      subscriptionOptions);

Is this how it is supposed to be done now, or is there a bug in casting std::bind from any_subscription_callback.hpp?

mjcarroll commented 4 months ago

@DensoADAS or @jmachowinski can you take a look at this? I read the error and nothing obvious jumped out at me. Maybe you have more context after landing #1928

jmachowinski commented 4 months ago

I'll have a look.

jmachowinski commented 4 months ago

@mjcarroll I looked into this, as far as I can say, the argument deduction for std::bind in https://github.com/ros2/rclcpp/blob/rolling/rclcpp/include/rclcpp/function_traits.hpp is broken. For some reason,

class SomeClass
{

public:

  struct SomeHandle
  {
    int i;
  };

  void rosMessageHandler(const int& /*channelId*/,
                          SomeHandle /*clientHandle*/,
                          std::shared_ptr<rclcpp::SerializedMessage> /*msg*/) {
  }
};

rclcpp::function_traits::function_traits<std::bind(&SomeClass::rosMessageHandler, &s, 1, SomeClass::SomeHandle(), std::placeholders::_1)>::arguments

resolves to

std::tuple<int const&, SomeClass::SomeHandle, std::shared_ptr<rclcpp::SerializedMessage> >

instead of the expected

std::shared_ptr<rclcpp::SerializedMessage>

But this is serious meta programming foo, which isn't my best discipline. Any Ideas ?

jmachowinski commented 4 months ago

@mjcarroll https://github.com/cellumation/rclcpp/tree/function_traits_bug This branch contains a test, that shows the bug, and currently fails.

jmachowinski commented 4 months ago

@mjcarroll Something I forgot to mention: As to why this bug now surfaced: Before the interface to create_generic_subscription did not forward the given callback object directly, but converted it to std::function<void(std::shared_ptr)>. Therefore the function_trait was never call with the original bind result.

ros-discourse commented 4 months ago

This issue has been mentioned on ROS Discourse. There might be relevant details there:

https://discourse.ros.org/t/rclcpp-template-metaprogramming-bug-help-wanted/36319/1

chhtz commented 4 months ago

I guess in this (and related) blocks

// std::bind for object methods
template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
#if defined DOXYGEN_ONLY
struct function_traits<std::bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
#elif defined _LIBCPP_VERSION  // libc++ (Clang)
struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
#elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
struct function_traits<std::_Bind<ReturnTypeT(ClassT::* (FArgs ...))(Args ...)>>
#elif defined __GLIBCXX__  // glibc++ (GNU C++)
struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...)>(FArgs ...)>>
#elif defined _MSC_VER  // MS Visual Studio
struct function_traits<
  std::_Binder<std::_Unforced, ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
#else
#error "Unsupported C++ compiler / standard library"
#endif
  : function_traits<ReturnTypeT(Args ...)>
{};

you need to write some meta-function which iterates through FArgs... and for every placeholder it needs to place the corresponding element of Args... at position std::is_placeholder<FArg>::value of the resulting arguments tuple. This may work for "sane" use cases, but e.g., std::bind(&CLS::Func, object, _1, _1) could break this (the bind construct is valid as long as the parameter passed to _1 can be converted to both arguments of CLS::Func).

chhtz commented 4 months ago

Very crude proof-of-concept: https://godbolt.org/z/7oWfYTEqh Feel free to improve that. Especially for MSVC I have no idea, why the std::is_placeholder does not work properly -- it seems std::bind replaces the placeholders by some other type. I'm also not sure if the order of Args... vs FArgs... is correct for all compilers.

chhtz commented 4 months ago

The MSVC issue can easily be solved by:

std::is_placeholder<std::remove_reference_t<FArg> >::value

Adding this does not hurt other compilers: https://godbolt.org/z/sP63PMdMM

I won't spend time working on a cleaned-up solution (at least not in the foreseeable future).