"Did not get reply from planning scene client" when using CollisionModelsInterface

edit

Hi,

I have found that if a node creates a planning_environment::CollisionModelsInterface object and also calls the /environment_server/set_planning_scene_diff service, the service always reports that it is unable to get a reply from planning scene client with the name of the node. Specifically, if I run the pr2_tabletop_manipulation_launch pr2_tabletop_manipulation.launch file and this piece of test code:

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

  ros::ServiceClient scene_client =
    n.serviceClient<arm_navigation_msgs::setplanningscenediff>
    ("/environment_server/set_planning_scene_diff");
  ROS_INFO("Waiting for planning scene service");
  scene_client.waitForExistence();
  ROS_INFO("Planning scene service is now available");

  planning_environment::CollisionModelsInterface cmi("robot_description");
  arm_navigation_msgs::SetPlanningSceneDiff ssd;
  if (!scene_client.call(ssd)) {
    ROS_ERROR("Unable to set planning scene");
    return 1;
  }
  ROS_INFO("Successfully set planning scene");
  return 0;
}

[ INFO] [1322764074.606153612, 4535.143000000]: Successfully connected to planning scene action server for /planning_scene_test_node
[ INFO] [1322764080.585060399, 4540.165000000]: Did not get reply from planning scene client /planning_scene_test_node.  Incrementing counter to 1

The collision models interface declared within this node also does not update to the new planning scene and has to be set manually.

I am using 64-bit Ubuntu 10.04 and ROS electric. The version of the arm_navigation stack is 1.0.7-s1321929829~lucid.

Thanks,

Jenny