Use MoveIt .scene file instead of custom yaml file to persist a planning scene

[jarkenau]

MoveIt provides a .scene format to persist and load planning scenes. Replacing our custom yaml format to this format would allow easier integration of external planning scenes. The example below shows the structure of the file format for storing a box:

* table_1
13.22 2.15 0.36
0 0 0.707107 0.707107
1
box
1.4 0.7 0.72
0 0 0
0 0 0 1
0 0 0 0
0

instead of

- scene_name: 'table_1'
  frame_id: 'map'
  box_x_dimension: 1.40
  box_y_dimension: 0.70
  box_z_dimension: 0.72
  box_position_x: 13.22
  box_position_y: 2.15
  box_position_z: 0.36
  box_orientation_x: 0.0
  box_orientation_y: 0.0
  box_orientation_z: 0.7071
  box_orientation_w: 0.7071

Further there is also a node that can publish the scene from the .scene file https://github.com/ros-planning/moveit/blob/master/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp, so that we don't have to parse and publish it (as it is currently done with the yaml files).

[mintar]

Very nice find, and great idea to use this! I totally forgot about the MoveIt scenes.

By the way, the "Motion Planning" RViz plugin supports importing/exporting/editing these scenes.

While Gazebo (or the real robot) is running, launch the following:

roslaunch mobipick_moveit_config moveit_rviz.launch

Then, in the "Motion Planning" Rviz plugin, in the "Scene Objects" tab, you can click "Scene Geometry" -> Export in be bottom right corner.

This gives you the following file:

(noname)+
* back_wall_1
10.65 1.15 1
0 0 0 1
1
box
3.1 0.2 2
0 0 0
0 0 0 1
0 0 0 0
0
* back_wall_2
12.85 1.38 1
0 0 0 1
1
box
1.4 0.25 2
0 0 0
0 0 0 1
0 0 0 0
0
* relay_1
10.5395 3.00007 0.771991
-0.000242066 0.000254415 -0.706768 0.707446
1
box
0.0575 0.0451 0.104364
0 0 0
0 0 0 1
0 0 0 0
0
* side_wall_1
9.2 2.15 1
0 0 0 1
1
box
0.2 1.8 2
0 0 0
0 0 0 1
0 0 0 0
0
* side_wall_2
9.45 3.28 1
0 0 0 1
1
box
0.28 0.63 2
0 0 0
0 0 0 1
0 0 0 0
0
* table_1
13.22 2.15 0.36
0 0 0.707107 0.707107
1
box
1.4 0.7 0.72
0 0 0
0 0 0 1
0 0 0 0
0
* table_2
12.07 3.25 0.36
0 0 1 0
1
box
1.4 0.7 0.72
0 0 0
0 0 0 1
0 0 0 0
0
* table_3
10.42 3.25 0.36
0 0 1 0
1
box
1.4 0.7 0.72
0 0 0
0 0 0 1
0 0 0 0
0
.

Note the scene name ((noname)+) in the beginning and the . in the end to terminate the scene description.

Then you can kill everything and run this:

roslaunch mobipick_moveit_config demo.launch

... and import the scene and play around with it.

[mintar]

I really like using existing standards and hate reinventing the wheel, so I would be all in favor of using this. The only problem I can see is that other nodes perhaps need to parse this as well. Does grasplan use the MoveIt planning scene everywhere else? (For example, to figure out the position + dimension of the tables, for the GUIs etc.). If yes, then we could switch to the MoveIt scene format without problems.

[oscar-lima]

I agree and support the use of the standard MoveIt scene, I just didn't knew they existed. Even if we have to migrate other modules I would be up for switching.

[oscar-lima]

I really like using existing standards and hate reinventing the wheel, so I would be all in favor of using this. The only problem I can see is that other nodes perhaps need to parse this as well. Does grasplan use the MoveIt planning scene everywhere else? (For example, to figure out the position + dimension of the tables, for the GUIs etc.). If yes, then we could switch to the MoveIt scene format without problems.

Instead of other nodes parsing the planning scene file again, I suggest that they subscribe to the planning scene that is published in a topic. Side NOTE: the planning scene file does not contain information about the reference frame that the data is expressed on. Instead I think what MoveIt does is to assume it is on the "fixed frame" that is selected in rviz, a bit odd but since we always work on map frame that should be fine.

[mintar]

Instead of other nodes parsing the planning scene file again, I suggest that they subscribe to the planning scene that is published in a topic.

You are right, that would be the proper way to do it, good point! My only reservation is that it means adding a dependency to MoveIt to packages like the pose_selector, but pose_selector is supposed to be lightweight, and MoveIt is not exactly a lightweight dependency. We'll have to decide later. Why can things never be easy. :(

Side NOTE: the planning scene file does not contain information about the reference frame that the data is expressed on. Instead I think what MoveIt does is to assume it is on the "fixed frame" that is selected in rviz, a bit odd but since we always work on map frame that should be fine.

Yes, that isn't nice. The frame should be specified in the scene file.

Usually in MoveIt, when the frame is not specified, poses are expressed in the PlanningScene's planning frame:

Model Frame: RobotModel's root frame == PlanningScene's planning frame
If the SRDF defines a virtual, non-fixed (e.g. floating) joint, this is the parent of this virtual joint.

https://github.com/moveit/moveit/blob/c43c555704a9a3292256f049135619b273df97c8/moveit_core/robot_state/include/moveit/robot_state/robot_state.h#L52-L53

We define that frame as map:

https://github.com/DFKI-NI/mobipick/blob/c2a8f170035223636983ba054936ed664810e4e3/mobipick_moveit_config/config/mobipick_os.srdf.xacro#L157-L158

However, things get more complicated, since a PlanningScene can have a parent PlanningScene to which all frames are relative, CollisionObject messages have a header.frame_id which can override things, and so on. But since none of that information is in the scene file, I assume MoveIt uses the global planning frame (i.e., map).