errors when subscribing a topic and using its callback function in a class

asked 2017-04-28 02:54:01 -0500

bobpop gravatar image

updated 2017-04-28 03:22:44 -0500

Hello, thanks for your attention. Just like the question, when I subscribe topics in a class, I meet the error :

error: no matching function for call to ‘ros::NodeHandle::subscribe(std::string&, int, void (ur_control_demo::)(), ur_control_demo const)’ sub2= n.subscribe(falconPos, 10, &ur_control_demo::pos_callback,this);

It is the first time I try to describe in a class. What should I do to handle it? Here is a part of code:

class ur_control_demo
{
    protected:
        std::string ur_name;
        std::string ur_jointname[6];
        std::string ur_linkname[8];
        std::string topic_jointstate;
        std::string falconPos;

        std::string server_urio;
        std::string robot_description;
        std::string ur_action_client_name;

    KDL::Chain                              ur_chain;
    KDL::ChainFkSolverPos_recursive         *kdl_fk_solver;
    TRAC_IK::TRAC_IK                        *track_ik_solver;

    KDL::Frame                              task_pose[TASK_POS_NUM];
    KDL::Frame                              tagert_pose;
    KDL::Frame                              access_tagertpose;

    sensor_msgs::JointState                 cur_RobotJointStates;
    sensor_msgs::JointState                 target_RobotJointStates;

    trajectory_msgs::JointTrajectoryPoint   targetTra_robotJoint;
    trajectory_msgs::JointTrajectoryPoint   curTra_robotJoint;
    control_msgs::FollowJointTrajectoryGoal mygoal;

    ros::NodeHandle         *nh, n;
            ros::Subscriber                 sub1;
            ros::Subscriber                 sub2;

    ros::ServiceClient      callsrv;

    Client                  *client;
    boost::thread           ros_manager_thread;
    boost::mutex            cmd_mutex, joint_mutex;
    double test_pose_rpy[6];

public:
    ur_control_demo(ros::NodeHandle _n):
        n(_n),
        ur_jointname(default_joint_name),
        ur_linkname(default_link_name),
        ur_name("ur3"),
        topic_jointstate("/joint_states"),
                    falconPos("/falconPos"),
        server_urio("/ur_driver/set_io"),
        robot_description("/robot_description"),
        ur_action_client_name("/follow_joint_trajectory"),

        ros_manager_thread(&ur_control_demo::ros_manager, this)
    {
        for(int i=0;i<6;i++)
        {
            cur_RobotJointStates.name.push_back(ur_jointname[i]);
            target_RobotJointStates.name.push_back(ur_jointname[i]);
            mygoal.trajectory.joint_names.push_back(ur_jointname[i]);
        }

        cur_RobotJointStates.position.resize(6);
        curTra_robotJoint.positions.resize(6);
        target_RobotJointStates.position.resize(6);
        targetTra_robotJoint.positions.resize(6);
        curTra_robotJoint.velocities.resize(6);
        targetTra_robotJoint.velocities.resize(6);

        client = new Client(ur_action_client_name, true);
        ROS_INFO("waiting for server...");
        bool state = client->waitForServer(ros::Duration(10));
        if(state) ROS_INFO("connected the server.");
        else {ROS_INFO("connect the server failed.");}

        track_ik_solver = new TRAC_IK::TRAC_IK(ur_linkname[0], ur_linkname[7], robot_description);
        track_ik_solver->getKDLChain(ur_chain);
        track_ik_solver->SetSolveType(TRAC_IK::Distance);
        kdl_fk_solver = new KDL::ChainFkSolverPos_recursive(ur_chain);
        usleep(100);
        ROS_INFO("binpicking task is running");
    }


    void pos_callback(const rosfalcon::falconPosConstPtr& _msg)
            {
            }

    void jointstates_subCB(const sensor_msgs::JointState &_msg)
    {
        joint_mutex.lock();
        cur_RobotJointStates = _msg;

        joint_mutex.unlock();
    }
    void ros_manager(void)
    {
        ROS_INFO("ros manager is running.");
        sub1= n.subscribe(topic_jointstate, 1, &ur_control_demo::jointstates_subCB, this);

        callsrv = n.serviceClient<ur_msgs::SetIO>(server_urio);
        ros::Rate r(100);
        while(ros::ok())
        {
            r.sleep();
            ros::spinOnce();
        }
    }
            void setRPYPosition(void)
            {
            sub2= n.subscribe(falconPos, 10, &ur_control_demo::pos_callback,this);
            ros::Rate r(100);
            }
    void start_ur_demo(void)
     {
        task_pose[0].p = KDL::Vector(test_pose_rpy[0], test_pose_rpy[1], test_pose_rpy[2]);
         task_pose[0].M = KDL::Rotation::RPY(test_pose_rpy[3], test_pose_rpy[4], test_pose_rpy[5]);
         while(ros::ok())

         {
                            moveto(task_pose[0], 4);
            setURIO(0, true);
         }
                     }

};

             int main(int argc, char **argv)
                  {
               ros::init(argc, argv, "ur_control_demo");
               ros::NodeHandle n;   

               ur_control_demo ur_demo(n);
               ur_demo.setRPYPosition();
               ur_demo.start_ur_demo();

               ROS_INFO("task complete.");
               ros::shutdown();
                return 0;
                    }

Another describer named "sub1" and its callback function jointstates_* are right(It's not writed by me). But the sub2(I add) is wrong.

edit retag flag offensive close merge delete

Comments

Try this: sub2= n.subscribe < rosfalcon::falconPos> (falconPos, 10, &ur_control_demo::pos_callback,this);

sometimes it works with the template and sometimes without, I do not know why.

angeltop gravatar image angeltop  ( 2017-04-28 04:15:04 -0500 )edit

ok, I will try it later,thanks!

bobpop gravatar image bobpop  ( 2017-04-28 04:51:12 -0500 )edit