0
$\begingroup$

Rosanswers logo

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!


Originally posted by mbj on ROS Answers with karma: 197 on 2012-08-16

Post score: 1


Original comments

Comment by dornhege on 2012-08-16:
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?

Comment by mbj on 2012-08-16:
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!

Comment by dornhege on 2012-08-16:
OK, that should definitely work. An empty diff is a common use case.

Comment by mbj on 2012-08-16:
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.

Comment by yangyangcv on 2012-08-31:
i'm stuck with the same problem, "incomplete robot state"...have you solved this problem yet?

Comment by mbj on 2012-09-02:
No, I reviewed and tested again the code after 2 weeks and the result is the same: "incomplete robot state". Can help us @dornhege?

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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.


Originally posted by yangyangcv with karma: 741 on 2012-09-05

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by dornhege on 2012-09-05:
Your way doesn't set an empty planning scene, but provides a robot state.

Comment by yangyangcv on 2012-09-06:
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.

Comment by mbj on 2012-09-06:
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.

Comment by yangyangcv on 2012-09-06:
@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.

Comment by yangyangcv on 2012-09-06:
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.

Comment by dornhege on 2012-09-06:
The difference is that it should work regardless, so there might still be something wrong somewhere else.

Comment by yangyangcv on 2012-09-06:
@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; }

Comment by yangyangcv on 2012-09-06:
so you need to call collisionModels->revertPlanningScene(kinematicState); before to call setPlanningScene()

$\endgroup$
0
$\begingroup$

Rosanswers logo

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.


Originally posted by egiljones with karma: 2031 on 2012-09-03

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by yangyangcv on 2012-09-03:
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?

Comment by yangyangcv on 2012-09-03:
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.

Comment by egiljones on 2012-09-04:
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.

Comment by mbj on 2012-09-05:
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..."

Comment by mbj on 2012-09-05:
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.

Comment by yangyangcv on 2012-09-05:
thank god, i just got the whole thing running correctly! just wait a moment, i will post my solution.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.