<> == Package Summary == The [[cob_3d_mapping_geometry_map]] package provides a node for processing and visualization of 3D environment data. <> == Overview == The node subscribes to messages of type [[http://ros.informatik.uni-freiburg.de/roswiki/doc/api/cob_3d_mapping_msgs/html/msg/ShapeArray.html|ShapeArray]], handling shapes of types polygon and cylinder. If necessary the shapes are transformed to a common target frame. Reoccuring shapes are associated and merged accordingly. For every input message the resulting aggregated geometric map is published and ready for visualization in [[rviz]] or further processing in [[cob_3d_visualization]]. {{{ #!clearsilver CS/NodeAPI name = cob_3d_mapping_geometry_map desc = goal{ } result{ } feedback{ } sub{ 0.name = /shape_array 0.type = cob_3d_mapping_msgs/ShapeArray 0.desc = Input topic with geometric shapes. 1.name = /tf 1.type = /tf 1.desc = Transformations, connecting target frames } pub{ 0.name = /geometry_map/map_array 0.type = cob_3d_mapping_msgs/ShapeArray 0.desc = Array with geometric shapes 1.name = /geometry_map/geometry_marker 1.type = visualization_msgs/Marker 1.desc = Line strip of shape contours. 2.name = /geometry_map/primitives 2.type = visualization_msgs/Marker 2.desc = Cylinder marker visualization of cylinder pieces. } srv{ 1.name = clear_map 1.type = cob_srvs/Trigger 1.desc = Clears the geometry map. 2.name = get_map 2.type = cob_3d_mapping_msgs/GetGeometryMap 2.desc = Returns the current geometry map. 3.name = set_map 3.type = cob_3d_mapping_msgs/SetGeometryMap 3.desc = Sets a map, overrides existing map 4.name = modify_map 4.type = cob_3d_mapping_msgs/ModifyMap 4.desc = Returns the modified map } param{ 0.name = ~map_frame_id 0.type = string 0.default = "/map" 0.desc = Target frame the map is supposed to be transformed to. Make sure, that the corresponding transformation is available as /tf topic in the input data. 1.name = ~enable_tf 1.type = bool 1.default = true 1.desc = If set to false, transformation of incoming shapes to the map frame is deactivated. Use this option if no tf is available. 2.name = ~d 2.type = double 2.default = 0.05 2.desc = Distance threshold. If distance between two shapes falls below this value, they are considered merge candidates. 3.name = ~cos_angle 3.type = double 3.default = 0.97 3.desc = Cosine of angle threshold. If angular merge criteria falls below this value, shapes are considered merge candidates. } }}} == Usage == The geometry map node can be started using the corresponding launch file: {{{ roslaunch cob_3d_mapping_geometry_map geometry_map.launch }}} Save map to a bag file {{{ rosrun cob_3d_mapping_geometry_map get_geometry_map map.bag }}} Clear the map {{{ rosservice call geometry_map/clear_map }}} Load an initial map from a bag file *Edit set_geometry_map.launch: set the path to map.bag as file_path *Load the map {{{ roslaunch cob_3d_mapping_geometry_map set_geometry_map.launch }}} == Example == A sample point_map of a scene, recorded in a kitchen. {{attachment:gm_pointmap.png||width=100%}} The result of using the [[cob_3d_mapping_geometry_map]] is shown below. It consists of published shapes, geometry markers and cylinder primitives. {{attachment:gm_geometrymap.png||width=100%}} If one intends to use the enhanced capabilities provided by interactive markers, the node is started according to [[cob_3d_visualization]]. {{attachment:interactive.png||width=100%}}