Impossible to set a PlanningScene

edit

After days of work, now i can answer this question.

1) put the line above your main:

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

2) put the code below in appropriate position within your main.

ros::NodeHandle nh;
planning_environment::CollisionModels* collisionModels;
planning_models::KinematicState* kinematicState;
collisionModels = new planning_environment::CollisionModels("robot_description");
kinematicState = new planning_models::KinematicState(collisionModels->getKinematicModel());
ros::ServiceClient set_planning_scene_diff_client;
while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0)))
{
ROS_INFO_STREAM("Waiting for planning scene service " << SET_PLANNING_SCENE_DIFF_NAME);
}
set_planning_scene_diff_client = nh.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;
//THE LINE BELOW IS VERY IMPORTANT!
planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res))
{
  ROS_WARN("Can't get planning scene");
  exit(-1);
}
else
{
  ROS_INFO("Successfully get planning scene!");
}
collisionModels->revertPlanningScene(kinematicState);
//must call revertPlanningScene() first before calling setPlanningScene(). otherwise you'll be blocked in setPlanningScene()
collisionModels->setPlanningScene(planning_scene_res.planning_scene);

btw, you don't need to change other config files. just use the ones generated by the planning_description_configuration_wizard.