Revision history [back]

The program shouldn't segfault - that's a bug, and it will be fixed for the next release, which should be coming out in the next week or so.

What's going on is that pretty much all of the generated nodes depend on getting the planning scene from the environment server - this happens under the hood from the points of the tools that you've been using, and when you are programmatically accessing the ik node the planning scene has not been set. I won't get too far into the details, but the planning scene helps divorce the nodes themselves from the current state of the world - they are instead provided state using this planning scene mechanism. This means that you can use the nodes even without simulation, and even plan or solve ik for states that aren't the current state of the robot. Your pi_robot_arm_navigation.launch brings up the environment_server in a mode where it will supplement whatever planning scene you set with the current planning scene, but nothing is ever actually setting the planning scene at all and the get_fk call isn't robust to that, which is a bug.

To get things working before the release you can add the following lines to your test file (in the appropriate places):

//add before main
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";

    // add before you call any kinematics services     
    ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
    ros::ServiceClient set_planning_scene_diff_client = rh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);


    arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
    arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;

    if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
      ROS_WARN("Can't set planning scene");
      return;
    }

If you set the planning scene before you call get_fk then things should work. This will set the planning scene to the current robot state.

The arm_kinematic_constraint_aware node should have more functionality in the get_fk and get_ik (non-constraint aware) service calls even if you haven't set a planning scene - I'll make sure that gets into the next release.

And all of this system design stuff will get posted to the wiki in short order - sorry for the early adoption pain.