Creating tables in the disney usecase

[ikrets]

This pull request implements creating:

• a table using edge grasp and surface grasp frames and a predefined height, or
• a table from a predefined length, width and height

in a /check_kinematics_tabletop service call.

The parameter table_pose was renamed to table_surface_pose, as it is not the centroid of the table, but the centroid of the table surface. Additionally, parameters table_from_edges and edge_frames were introduced. From the service call definition:

# The position part is the centroid of the table surface. The Z axis
# of the frame points downward from the table surface.
# If the table is fixed, than X axis points along the first dimension
# of the fixed table. If the table is created from edges, X and Y do
# not play any role.
geometry_msgs/Pose table_surface_pose

# Whether to use predefined table model or construct table from edge_frames
bool table_from_edges

# EdgeGrasp frames that are used to construct a table. Only used when
# table_from_edges is set to true.
geometry_msgs/Pose[] edge_frames

The tub_feasibility_check node now reads the parameter /feasibility_check/table_dimensions, which is a list of three dimension of the box: width, length and height (X, Y and Z dimensions).

Note that the service call now expects the table_surface_pose and edge_frames frames such that the Z axis points downwards. Examples with the correct frames are examples/disney/surface_grasp_reference.sh and examples/tabletop_surface_grasp_sampling_orientation.sh.

I have verified that the examples using WAM and IFCO, WAM and tabletop, Prob2 and tabletop work.

[ikrets]

Now there is no need to supply table_surface_pose when constructing the table from edges. There is also a new parameter /feasibility_check/loose_ends_extension_length that determines how much the loose ends of the table will be extended before creating an edge between them (loose ends appear when not all edges of a table are visible).

[JannisW]

Could be merged to master, works in the disney use case together with branch disney_edge_grasp_integration of ec_grasp_planner.
Run together with edge_grasp_check_support (soma_utils) vision (master_disney) and temp_edges (tub_feasibilty_check)
ocado use case should not be affected, in case it is, we can roll back to the version tagged with ms5

However, I would not merge it before the review-meeting in May