Posted by
matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Using-RTAB-with-Manipulator-tp11147p11156.html
A couple of questions:
* What means world -> base_link? It is always Identity?
* Is your robot arm's base moving? Note that if the arm's base is not moving, using directly octomap package may just work already.
Here are two scenarios:
A) The arm's base is fixed on a table. In this case, we will run rtabmap exactly like you would run the octomap_server. You can run rtabmap with odom_frame_id set to base_link and frame_id set to wrist_link. Note that if world->base_link is fixed and Identity, you may also use world frame for odom_frame_id. You can also remove your node publishing world->base_link and set map_frame_id to world instead (then rtabmap will publish world->base_link). Then add these parameters to rtabmap:
# Values sohuld be all "string" type
'Kp/MaxFeatures': '-1', # Disable visual loop closure detection
'RGBD/ProximityBySpace': 'false', # Disable proximity detection
'Grid/CellSize': '0.05', # Adjust octomap's minimum voxel size
'Grid/3D': 'true',
#'Grid/RayTracing': 'true', # Uncomment if you want octomap to clear dynamic obstacles
'RGBD/LinearUpdate': '0.1', # Adjust minimum motion to add new nodes to the map
'RGBD/AngularUpdate': '0.1', # Adjust minimum motion to add new nodes to the map
'Rtabmap/DetectionRate': '1', # Adjust at which rate new nodes are added to the map
The /rtabmap/octomap topic will be generated. rtabmap will publish map-> (base_link or world) TF, though it would stay Identity the whole time.
B) The arm is on a mobile platform. In this case, you will need to get rid of world frame, it will be replaced by map frame. You would also need odometry node, either it is wheel odometry from your mobile platform or computed from the camera on the wrist of the robot. To compute odometry from the camera, you could try rgbd_odometry node. For rtabmap node, the config could stay relatively the same than what you have right now. The odom frame will be published by the odometry node. Note that if world->base_link is moving (i.e., you have kinda already odometry computed), you can set odom_frame_id to world in your original example.
For rtabmap_viz, you don't need to set subscribe_depth, unless you want to see high frame rate feedback of the current camera data. The frame_id should be the same frame_id set for rtabmap node, same for odom_frame_id (though it is optional for rtabmap_viz).