c++boostrosboost-bindmoveit

Error using boost::bind for subscribe callback


We're getting this compile error followed by many more errors showing attempts to match the subscribe parameters to all possible candidate functions when using boost::bind as a callback for subscribe.

error: no matching function for call to ‘ros::NodeHandle::subscribe(const char [18], int, boost::_bi::bind_t<void, void (*)(const geometry_msgs::WrenchStamped_<std::allocator<void> >&, moveit::planning_interface::MoveGroup&), boost::_bi::list2<boost::arg<1>, boost::_bi::value<moveit::planning_interface::MoveGroup*> > >)’

Our code is as follows. The commented lines show the code which works when the MoveGroup context (object pointer) is not passed.

#include <stdio.h>
#include <boost/bind.hpp>
#include <geometry_msgs/WrenchStamped.h>
#include <moveit/move_group_interface/move_group.h>

using namespace Eigen;
using namespace std;

//void contact_callback(const geometry_msgs::WrenchStamped& msg) {
void contact_callback(const geometry_msgs::WrenchStamped& msg, moveit::planning_interface::MoveGroup &group){
    //if(msg.wrench.force.z > 5) group.stop();
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "get_stiffness");
    ros::NodeHandle node_handle; 
    ros::AsyncSpinner spinner(1);
    spinner.start();
    moveit::planning_interface::MoveGroup group("manipulator");
    ros::Subscriber contact_sub;
    //contact_sub = node_handle.subscribe("/finger1/nano17ft",1,contact_callback);
    contact_sub = node_handle.subscribe("/finger1/nano17ft",100,boost::bind(contact_callback,_1,&group));
    ros::waitForShutdown();
    return 0;
}

Solution

  • The handler takes a MoveGroup& but you pass it the address of group.

    Instead use ref(group):

    boost::bind(contact_callback,_1,boost::ref(group))
    

    Or, indeed

    std::bind(contact_callback,std::placeholders::_1,std::ref(group))
    

    UPDATE:

    Your callback does not adhere to the required signature:

    void contact_callback(const geometry_msgs::WrenchStamped&, moveit::planning_interface::MoveGroup & group) {
    

    must be

    void contact_callback(const boost::shared_ptr<geometry_msgs::WrenchStamped const>, moveit::planning_interface::MoveGroup & group) {
    

    At the call site you must either make the message type explicit (it's in non-deducible context):

    contact_sub = node_handle.subscribe<geometry_msgs::WrenchStamped>("/finger1/nano17ft", 100,
            boost::bind(contact_callback, _1, boost::ref(group)));
    

    OR you could simply explicitly wrap in a function<> first:

    boost::function<void(const boost::shared_ptr<geometry_msgs::WrenchStamped const>&)> callback =
        boost::bind(contact_callback, _1, boost::ref(group));
    contact_sub = node_handle.subscribe("/finger1/nano17ft", 100, callback);
    

    Live Demo

    With all roscpp/Eigen stuff stubbed out:

    Live On Coliru

    #include <boost/bind.hpp>
    #include <boost/shared_ptr.hpp>
    #include <boost/function.hpp>
    #include <iostream>
    
    ////////////////// STUBS STUBS STUBS //////
    //#include <geometry_msgs/WrenchStamped.h>
    namespace Eigen {}
    namespace geometry_msgs {
    struct WrenchStamped {}; }
    namespace moveit { namespace planning_interface { struct MoveGroup { std::string name; MoveGroup(std::string s):name(s){} }; } }
    
    namespace ros {
        void init(...) {}
        void waitForShutdown(...) {}
        struct Subscriber {};
        struct NodeHandle {
            using VoidConstPtr = void const *;
            enum class TransportHints {};
            template <typename M>
            Subscriber subscribe(const std::string &topic, uint32_t queue_size,
                    const boost::function<void(const boost::shared_ptr<M const> &)> &callback,
                    const VoidConstPtr &tracked_object = VoidConstPtr(),
                    const TransportHints &transport_hints = TransportHints())
            { 
                (void)topic, (void)queue_size, void(tracked_object), void(transport_hints);
                callback({});
                return {};
            }
        };
        struct AsyncSpinner {
            AsyncSpinner(int) {}
            void start() {}
        };
    };
    //#include <moveit/move_group_interface/move_group.h>
    ////////////////// END STUBS END STUBS END STUBS //////
    
    using namespace Eigen;
    
    void contact_callback(const boost::shared_ptr<geometry_msgs::WrenchStamped const>, moveit::planning_interface::MoveGroup & group) {
        // if(msg.wrench.force.z > 5) group.stop();
        std::cout << "Invoked! " << group.name << "\n";
    }
    
    int main(int argc, char **argv) {
        ros::init(argc, argv, "get_stiffness");
        ros::NodeHandle node_handle;
        ros::AsyncSpinner spinner(1);
        spinner.start();
        moveit::planning_interface::MoveGroup group("manipulator");
        ros::Subscriber contact_sub;
    
        contact_sub = node_handle.subscribe<geometry_msgs::WrenchStamped>("/finger1/nano17ft", 100,
                boost::bind(contact_callback, _1, boost::ref(group)));
        {
            boost::function<void(const boost::shared_ptr<geometry_msgs::WrenchStamped const>&)> callback =
                boost::bind(contact_callback, _1, boost::ref(group));
            contact_sub = node_handle.subscribe("/finger1/nano17ft", 100, callback);
        }
        ros::waitForShutdown();
    }
    

    Prints

    Invoked! manipulator
    Invoked! manipulator