How do you override a message handler callback in ROS2?

asked 2021-10-11 22:32:22 -0500

KenYN gravatar image

updated 2021-10-11 22:33:05 -0500

I am trying to write code that traces message callbacks. I have a class that I insert between rclcpp::Node and MyHandler as follows:

class LoggerNode: public rclcpp::Node
{
// whatever...
    // Copy-paste from rclcpp::Node
    template<
        typename MessageT,
        typename CallbackT,
        typename AllocatorT = std::allocator<void>,
        typename CallbackMessageT =
            typename rclcpp::subscription_traits::has_message_type<CallbackT>::type,
        typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>,
        typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
            CallbackMessageT,
            AllocatorT
        >
    >
    std::shared_ptr<SubscriptionT> create_subscription(
        const std::string & topic_name,
        const rclcpp::QoS & qos,
        CallbackT && callback,
        const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
            rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>(),
        typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
            MessageMemoryStrategyT::create_default()
        )
    )
    {
        auto lambdaCallback = [topic_name, this, callback](const typename SubscriptionT::ConstSharedPtr msg) {
            beforeCallback<SubscriptionT>(topic_name, msg);
            // Call original callback
            callback(msg); // **CANNOT GET THIS RIGHT**
            afterCallback<SubscriptionT>(topic_name, msg);
        };

        // Call parent function
        return Node::create_subscription<SubscriptionT>(topic_name, qos, lambdaCallback, options, msg_mem_strat);
    }
}

class MyHandler:: public LoggerNode
{
//...
    // Unchanged from original code
    m_sub
        = create_subscription<msgs::msg::Msg>
                ("TopicName", 4u, std::bind(&MyHandler::HandlerCallback, this, std::placeholders::_1));
}

The line callback(msg); is the bit I am having trouble with - I get this initial error:

error: no match for call to ‘(const std::_Bind<void (MyHandler::*(MyHandler*, std::_Placeholder<1>))(std::shared_ptr<const msgs::msg::Msg_<std::allocator<void> > >)>) (const ConstSharedPtr&)’

Then the typical template argument deduction/substitution failed sequence of errors

edit retag flag offensive close merge delete