Impossible to set a PlanningScene

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

mbj
197 ●16 ●18 ●26

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 ( 2012-08-16 01:30:45 -0600 )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 ( 2012-08-16 02:05:48 -0600 )edit

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

dornhege ( 2012-08-16 03:19:16 -0600 )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 ( 2012-08-16 10:23:12 -0600 )edit

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

yangyangcv ( 2012-08-31 01:10:19 -0600 )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 ( 2012-09-02 19:46:43 -0600 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

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

yangyangcv

741 ●21 ●32 ●42 http://www.cnblogs.com...

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

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

Comments

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

dornhege ( 2012-09-05 23:59:26 -0600 )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 ( 2012-09-06 01:00:08 -0600 )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 ( 2012-09-06 02:16:01 -0600 )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 ( 2012-09-06 02:33:06 -0600 )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 ( 2012-09-06 02:35:15 -0600 )edit

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

dornhege ( 2012-09-06 02:35:34 -0600 )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 ( 2012-09-06 21:24:44 -0600 )edit

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

yangyangcv ( 2012-09-06 21:25:22 -0600 )edit

add a comment

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

egiljones
2026 ●5 ●18 ●36

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

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 ( 2012-09-03 20:56:14 -0600 )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 ( 2012-09-03 21:00:43 -0600 )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 ( 2012-09-04 05:41:44 -0600 )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 ( 2012-09-05 20:43:27 -0600 )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 ( 2012-09-05 20:53:50 -0600 )edit

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

yangyangcv ( 2012-09-05 21:28:41 -0600 )edit

add a comment

Impossible to set a PlanningScene

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Impossible to set a PlanningScene edit

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Impossible to set a PlanningScene