Electric get_fk forward kinematics fails simple test

edit

Hello ROS Gurus,

I am trying to get the new Electric arm navigation stack working on a pair of custom 6-dof Dynamixel arms (see pi_robot description). The planning wizard worked like a charm and I have two kinematics chains, one for each arm, both with root link "lower_torso_link" and tip link "left_hand_link" and "right_hand_link" respectively. The only tweak I made to the resulting launch files was to substitute my joint trajectory action controller (thanks to Anton Rebgun's dynamixel_controller package).

The resulting pi_robot_arm_navigation.launch file fires up without errors. Then I tried the simple get_fk.cpp sample from here but with the appropriate substitutions for my robot's left arm. (See my code listing below). When I run the resulting get_fk binary, I get the error:

[ERROR] [1311941555.891867066]: Forward kinematics service call failed

and in the terminal window where I launch the arm navigation nodes, I find that the pi_robot_left_arm_kinematics node has died:

[pi_robot_left_arm_kinematics-3] process has died [pid 10452, exit code -11].
log files: /home/patrick/.ros/log/b194841a-b998-11e0-bf54-8c736e77238f/pi_robot_left_arm_kinematics-3*.log

I can run the same get_fk.cpp file against David Lu's generic arm_kinematics package and it gives me back the FK solution without error.

Here now is my test get_fk.cpp file:

#include <ros/ros.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetPositionFK.h>

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

  ros::service::waitForService("/pi_robot_left_arm_kinematics/get_fk_solver_info");
  ros::service::waitForService("/pi_robot_left_arm_kinematics/get_fk");

  ros::ServiceClient query_client = 
    rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>
    ("/pi_robot_left_arm_kinematics/get_fk_solver_info");
  ros::ServiceClient fk_client = rh.serviceClient
    <kinematics_msgs::GetPositionFK>("/pi_robot_left_arm_kinematics/get_fk");

  // define the service messages
  kinematics_msgs::GetKinematicSolverInfo::Request request;
  kinematics_msgs::GetKinematicSolverInfo::Response response;
  if(query_client.call(request,response))
    {
      for(unsigned int i=0; 
      i< response.kinematic_solver_info.joint_names.size(); i++)
    {
      ROS_INFO("Joint: %d %s", i,
            response.kinematic_solver_info.joint_names[i].c_str());
    }
    }
  else
    {
      ROS_ERROR("Could not call query service");
      ros::shutdown();
      exit(1);
    }
  // define the service messages
  kinematics_msgs::GetPositionFK::Request  fk_request;
  kinematics_msgs::GetPositionFK::Response fk_response;
  fk_request.header.frame_id = "lower_torso_link";
  fk_request.fk_link_names.resize(1);
  //fk_request.fk_link_names[0] = "left_shoulder_pan_link";
  //fk_request.fk_link_names[1] = "left_shoulder_lift_link";
  //fk_request.fk_link_names[2] = "left_upper_arm_link";
  //fk_request.fk_link_names[2] = "left_elbow_link";
  //fk_request.fk_link_names[3] = "left_wrist_link";
  //fk_request.fk_link_names[4] = "left_hand_link";
  fk_request.fk_link_names[0] = "left_hand_link";
  fk_request.robot_state.joint_state.position.resize
    (response.kinematic_solver_info.joint_names.size());
  fk_request.robot_state.joint_state.name = 
    response.kinematic_solver_info.joint_names;
  for(unsigned int i=0; 
      i< response.kinematic_solver_info.joint_names.size(); i++)
    {
      fk_request.robot_state.joint_state.position[i] = 0;
    }
  fk_request.robot_state.joint_state.position[0] = 1.00;
  if(fk_client.call(fk_request, fk_response))
    {
      if(fk_response.error_code.val == fk_response.error_code.SUCCESS)
    {

      for(unsigned int i=0; i < fk_response.pose_stamped.size(); i ++)
        {
          ROS_INFO_STREAM("Link    : " << fk_response.fk_link_names[i].c_str());
          ROS_INFO_STREAM("Position: " << 
                  fk_response.pose_stamped[i].pose.position.x << "," <<  
                  fk_response.pose_stamped[i].pose.position.y << "," << 
                  fk_response.pose_stamped[i].pose.position.z);
          ROS_INFO("Orientation: %f %f %f %f",
               fk_response.pose_stamped[i].pose.orientation.x,
               fk_response.pose_stamped[i].pose.orientation.y,
               fk_response.pose_stamped[i].pose.orientation.z,
               fk_response.pose_stamped[i].pose.orientation.w);
        } 
    }
      else
    {
      ROS_ERROR("Forward kinematics failed");
    }
    }
  else
    {
      ROS_ERROR("Forward kinematics service call failed");
    }
  ros::shutdown();
}