subscriber class method callback problem

edit

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