Ask Your Question
0

subscriber class method callback problem

asked 2014-07-23 14:57:45 -0500

Wedontplay gravatar image

Hi, i'm trying to make a node that subscribe to a topic ik_from_pose_request that as the name suggest receive pose messages and compute arm joint values with inverse kinematic.

I saw that is possible to use a class method as a subscriber callback so i made my class that basically hold the object needed to calculate the inverse kinematic using MoveIt!

Here is the class implementation file with the constructor that initialize the required objects and the callback:

#include "kinematic_solver.h"

KinematicSolver::KinematicSolver()

{   
    //building robot model                           
    robot_model_loader_ptr = new robot_model_loader::RobotModelLoader("robot_description");
    robot_model::RobotModelPtr kinematic_model = robot_model_loader_ptr->getModel();
    ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());

    // Using the :moveit_core:`RobotModel`
    kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
    kinematic_state->setToDefaultValues();
    joint_model_group_ptr = kinematic_model->getJointModelGroup("arm");
    joint_names = joint_model_group_ptr->getJointModelNames();

    // We can retreive the current set of joint values stored in the state for the right arm.
    kinematic_state->copyJointGroupPositions(joint_model_group_ptr, joint_values);
    for(int i = 0; i < joint_names.size(); ++i)
    {
    ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
    }
}

KinematicSolver::~KinematicSolver()
{

}

void KinematicSolver::kinematic_solver_callback(const geometry_msgs::Pose::ConstPtr& pose)
{

    ROS_INFO("Received pose message:");
    ROS_INFO("x: %f, y: %f, z: %f", pose->position.x, pose->position.y, pose->position.z);
    ROS_INFO("q1: %f, q2: %f, q3: %f, q4: %f", pose->orientation.x, pose->orientation.y, pose->orientation.z, pose->orientation.w);

    bool found_ik = kinematic_state->setFromIK(joint_model_group_ptr, *pose, 10, 0.1);

    // Now, we can print out the IK solution (if found):
    if (found_ik)
    {
        kinematic_state->copyJointGroupPositions(joint_model_group_ptr, joint_values);
        for(std::size_t i=0; i < joint_names.size(); ++i)
        {
          ROS_INFO("Joint %s: %f", joint_names[i].c_str(), joint_values[i]);
        }
        }
        else
        {
        ROS_INFO("Did not find IK solution");
    }   

}

In my main the only things I do is to instantiate my class and subscribe to the topic:

#include <ros/ros.h>
#include "kinematic_solver.h"

int main(int argc, char **argv)
{
  ros::init (argc, argv, "ik_solver");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  KinematicSolver kinematic_solver;
  ros::NodeHandle node_handle();  

  ros::Subscriber subscriber = node_handle.subscribe("ik_from_pose_request", 10, &KinematicSolver::kinematic_solver_callback, &kinematic_solver);

  return 0;
}

The problem is that with AsyncSpinner the node does not remain alive. Here the console output

[INFO] [WallTime: 1406134201.220080] Rosbridge WebSocket server started on port 8080

[ INFO] [1406134201.241428169]: Loading robot model 'Ax12-Prototype'...

[ WARN] [1406134201.241945320]: Could not identify parent group for end-effector 'EF'

[ INFO] [1406134201.245101268]: Using position only ik

[ INFO] [1406134201.245528729]: Model frame: /base_link

[ INFO] [1406134201.245820733]: Joint joint1: 0.000000

[ INFO] [1406134201.250660517]: Joint joint2: 2.617994

[ INFO] [1406134201.250967732]: Joint joint3: 2.617994

[ INFO] [1406134201.251196630]: Joint joint4: 2.617994

[ INFO] [1406134201.251405720]: Joint joint5: 0.000000

[ik_solver-3] process has finished cleanly

Where I am doing wrong? if i use ros::spin instead the node stay alive but all MoveIt object do not load.

Tx. Gabriele

edit retag flag offensive close merge delete

Comments

Thank you!

Wedontplay gravatar imageWedontplay ( 2014-07-25 02:50:43 -0500 )edit

@Wedontplay: What is te content of your header file?

ksirks gravatar imageksirks ( 2017-09-15 07:07:08 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2014-07-23 16:55:14 -0500

bvbdort gravatar image

use ros::waitForShutdown();

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-07-23 14:57:45 -0500

Seen: 420 times

Last updated: Jul 23 '14