Ask Your Question
0

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

asked 2011-12-01 04:56:35 -0500

jbarry gravatar image

updated 2011-12-01 04:57:23 -0500

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;
}
the execution will pause for five seconds during the scene_client call and print the following to rosout:
[ 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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2011-12-01 05:57:39 -0500

egiljones gravatar image

You aren't calling ros::spin anywhere and the main thread of the program is blocking so the necessary callbacks aren't ever occurring. Adding an AsynSpinner is the right way to go. This code works for me:

#include <ros/ros.h>

#include <arm_navigation_msgs/SetPlanningSceneDiff.h>
#include <planning_environment/models/collision_models_interface.h>

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

  ros::AsyncSpinner spinner(1); 
  spinner.start();

  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;
}
edit flag offensive delete link more

Comments

1
Oh, of course. Thank you. It's working for me too now.
jbarry gravatar imagejbarry ( 2011-12-01 07:15:12 -0500 )edit

Your Answer

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

Add Answer

Question Tools

Stats

Asked: 2011-12-01 04:56:35 -0500

Seen: 472 times

Last updated: Dec 01 '11