Ask Your Question
1

Impossible to set a PlanningScene

asked 2012-08-16 01:18:18 -0500

mbj gravatar image

Hi all.

I'm trying to call the "environment_server/set_planning_scene_diff" service to get a PlanningScene to perform some collision checking. However, after a couple of days trying and searching in ROS Answers and more I didn't get any advance. My situation is the same that jbarry describes in this thread but this solution doesn't work in my case, I continue getting this:

[ WARN] [1345114122.403491890, 1345114122.395468950]: Incomplete robot state in setPlanningScene
[ERROR] [1345114122.403650694, 1345114122.395468950]: Setting planning scene state to NULL

I tried to debug manually searching through the different classes involved as Adolfo Rodriguez suggest in this thread (collision_models.cpp, collision_models_interfaces.cpp, model_utils.cpp) but I didn't find anything special except that if I print the joint_names.size() I get 13 joints (I expected 7). I also observed that when I launch the service I get 0 joints and the interactive markers in the RViz stops working.

I have also checked as bit-pirate suggest that /joint_state are correctly published (/world_joint and the 7 joints of my arm).

Can you say me what I'm doing wrong? Thanks in advance!

edit retag flag offensive close merge delete

Comments

The problem seems different from the question you linked. The request looks like it's malformed. Can you say, where you are getting the different joint name/position sizes? You do set them correctly?

dornhege gravatar image dornhege  ( 2012-08-16 01:30:45 -0500 )edit

Hi dornhege. As I saw in all the other cases, the request is only a empty arm_navigation_msgs::SetPlanningSceneDiff:Request. I don't understand why I have to set it. I was obtaining the different joint name/position sizes when I put a ROS_ERROR to debug after the 44 line in the model_utils.cpp. Thx!

mbj gravatar image mbj  ( 2012-08-16 02:05:48 -0500 )edit

OK, that should definitely work. An empty diff is a common use case.

dornhege gravatar image dornhege  ( 2012-08-16 03:19:16 -0500 )edit

Indeed, in all examples is sent an empty message and it works. So, do you have any idea where can be the error? I tried to generate the arm_navigation package with the wizard for different robots and I obtained the same result. Thanks.

mbj gravatar image mbj  ( 2012-08-16 10:23:12 -0500 )edit

i'm stuck with the same problem, "incomplete robot state"...have you solved this problem yet?

yangyangcv gravatar image yangyangcv  ( 2012-08-31 01:10:19 -0500 )edit

No, I reviewed and tested again the code after 2 weeks and the result is the same: "incomplete robot state". Can help us @dornhege?

mbj gravatar image mbj  ( 2012-09-02 19:46:43 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2012-09-05 22:52:57 -0500

updated 2012-10-28 21:38:48 -0500

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.

edit flag offensive delete link more

Comments

Your way doesn't set an empty planning scene, but provides a robot state.

dornhege gravatar image dornhege  ( 2012-09-05 23:59:26 -0500 )edit

what's the difference between an empty planning scene and my method? in my trial, it doesn't affect the following operations, like FK, path planning, etc, with my method.

yangyangcv gravatar image yangyangcv  ( 2012-09-06 01:00:08 -0500 )edit

Hi @yangyangcv, thanks for share the code, effectively, it works! However, when I try to get a complete state with "collision_models_->setPlanningScene(planning_scene_res.planning_scene)" don't works (it get blocked at this point). Do you tried it? I think that it can be related with @dornhege says.

mbj gravatar image mbj  ( 2012-09-06 02:16:01 -0500 )edit

@mbj: i do get blocked in that call, don't know why. but i just edited my answer and added two lines at the buttom. now it works. but still i don't quite understand what's happening with the planning scene.

yangyangcv gravatar image yangyangcv  ( 2012-09-06 02:33:06 -0500 )edit

in fact most of my above code are from planning_components_visualizer.cpp which is under the folder stacks/arm_navigation/src. the codes there are very clean and clear. so you can just refer that file.

yangyangcv gravatar image yangyangcv  ( 2012-09-06 02:35:15 -0500 )edit

The difference is that it should work regardless, so there might still be something wrong somewhere else.

dornhege gravatar image dornhege  ( 2012-09-06 02:35:34 -0500 )edit

@mbj: in the function setPlanningScene() which is in planning_environment/src/models/collision_models.cpp i find this: if(planning_scene_set_) { ROS_WARN("Must revert before setting planning scene again"); return NULL; }

yangyangcv gravatar image yangyangcv  ( 2012-09-06 21:24:44 -0500 )edit

so you need to call collisionModels->revertPlanningScene(kinematicState); before to call setPlanningScene()

yangyangcv gravatar image yangyangcv  ( 2012-09-06 21:25:22 -0500 )edit
1

answered 2012-09-03 07:47:06 -0500

egiljones gravatar image

Publishing something called /world_joint is not what you need. The Wizard by default produces a joint called /world_joint that reads in data from TF, transforming between the frame listed under the multi_dof_joints entry for world_joint as the parent_frame_id in your planning_description.yaml and the one listed as child_frame_id (which must be the root of your urdf). This allows you to incorporate information from an odometric system. Thus in addition to publishing /joint_states for your joints, you need to have a TF system running that publishes a transform from parent_frame_id to child_frame_id - this can just be an identity transform, but it needs to exist or the planning system will never fully initialize.

edit flag offensive delete link more

Comments

thanks for the answer, but i'm still confused. in myRobot_planning_description.yaml, i have: multi_dof_joints: - name: world_joint type: Floating parent_frame_id: base_link child_frame_id: base_link do i need to change the parent_frame_id?

yangyangcv gravatar image yangyangcv  ( 2012-09-03 20:56:14 -0500 )edit

and in another node publishing the joint state, i write: geometry_msgs::TransformStamped odom_trans; odom_trans.header.frame_id = "world_joint"; odom_trans.child_frame_id = "base_link"; and then i set this transform to identity. is it correct? by doing so, i still get the errors as above.

yangyangcv gravatar image yangyangcv  ( 2012-09-03 21:00:43 -0500 )edit

Change the odom_trans.header.frame_id to "base_link", and use a tf publisher to put it on the wire and things should work.

egiljones gravatar image egiljones  ( 2012-09-04 05:41:44 -0500 )edit

Thanks for your answers @egiljones, although these, I can't get running the PlanningScene. I didn't change anything in the config files, I only executed a tf_publisher "rosrun tf static_transform_publisher 0 0 0 0 0 0 1 world_joint base_link 1" but the result is the same: "Incomplete robot state..."

mbj gravatar image mbj  ( 2012-09-05 20:43:27 -0500 )edit

If I change the frame_id to "base_link" as you suggested to @yangyangcv I get a TF_SELF_TRANSFORM error. I suppose that any change is necessary in the config file. Any idea? Thanks.

mbj gravatar image mbj  ( 2012-09-05 20:53:50 -0500 )edit

thank god, i just got the whole thing running correctly! just wait a moment, i will post my solution.

yangyangcv gravatar image yangyangcv  ( 2012-09-05 21:28:41 -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: 2012-08-16 01:18:18 -0500

Seen: 381 times

Last updated: Oct 28 '12