Using RTAB with Manipulator

Posted by KasaIvas on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Using-RTAB-with-Manipulator-tp11147.html

Hello, I want to estimate the real pose of the robot's end-effector via SLAM.

So far I have managed to connect a depth camera to the wrist_link of my robot (and not to the tool_link=end-effector) and create an octomap directly in MoveIt. However, reading around I understood that it is not possible to apply SLAM directly in MoveIt and I came to know this ROS package, RTAB-Map.

I then created a custom launch file that first launches the launch file that starts Gazebo+Rviz+MoveIt and then launches the rtabmap_slam and rtabmap_viz nodes with delay of 20 seconds (to give Gazebo and Moveit time to launch). Starting everything I have several problems, many concerning the TF tree:

[rtabmap-11] [ WARN] MsgConversion.cpp:2005::getTransform() (getting transform odom -> base_link) Lookup would require extrapolation into the past. Requested time 373.503000 but the earliest data is at time 373.530000, when looking up transform from frame [base_link] to frame [odom] (wait_for_transform=0.200000)

[rtabmap-11] [ WARN] MsgConversion.cpp:2005::getTransform() (getting transform odom -> base_link) Could not find a connection between 'odom' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees. (wait_for_transform=0.200000)

I have several doubts  that I can't solve:
1) Which frames  should I use for SLAM? Does it make sense to create a new odometry frame or can I use the base_link? Can I estimate the pose of the end-effector with this? (or the camera, then I would have to do a transformation)
2) To view in RTAB-Viz which frame should I use? Do I have to create a new one (for example "map") or use an existing one (for example "world"). Also, do I need to remap the topics and set "'subscribe_depth': True," or not?

_______________________________________________________________
This is my node launch:

rtabmap_node = Node(
        package='rtabmap_slam',
        executable='rtabmap',
        name='rtabmap',
        output='screen',
        parameters=[{
            'use_sim_time': True,
            'frame_id': 'base_link',
            'odom_frame_id': 'odom',
            'subscribe_depth': True,
            'subscribe_rgbd': False,
            'subscribe_scan': False,
            'approx_sync': True,
            'sync_queue_size': 10,  
        arguments=['-d'],
        remappings=[
            ('rgb/image', '/gazebo_depth_camera/image_raw'),
            ('depth/image', '/gazebo_depth_camera/depth/image_raw'),
            ('rgb/camera_info', '/gazebo_depth_camera/camera_info'),
        ]
    )

rtabmap_viz_node = Node(
        package='rtabmap_viz',
        executable='rtabmap_viz',
        name='rtabmap_viz',
        output='screen',
        parameters=[{
            'use_sim_time': True,
            'frame_id': 'map',
            'odom_frame_id': 'odom',

            #'subscribe_depth': True,  #I don't know if these need to be put on or not
            #'subscribe_rgbd': False,
        }]
        # remappings=[
        #     ('rgb/image', '/gazebo_depth_camera/image_raw'),
        #     ('depth/image', '/gazebo_depth_camera/depth/image_raw'),
        #     ('rgb/camera_info', '/gazebo_depth_camera/camera_info'),
        # ]
    )
_______________________________________________________________

This is my tf tree (robot + camera), where the "world" is a virtual link and the planning frame in MoveIt: