introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
999 stars 558 forks source link

Not able to use external odometry with d435i #636

Open aditdoshi333 opened 3 years ago

aditdoshi333 commented 3 years ago

Hi, I am using a d435i camera but want to use odometry generated by an external device (Topic name: "/Odometry").

I did following steps in launch file- Visual_odometry = "false" odom_topic = "/Odometry".

In the odometry, the tf is camera_init -> body. The following is the rqt graph for the same rqt_grph

I am not able to understand what should I set frame_id and odom_frame_id: `

`

I tried publishing static transform between body / camera_init to camera_link but nothing is working.

Any help is appreciated. Thank you

matlabbe commented 3 years ago

I guess /body is the base frame of the robot (in REP105, it is called /base_link usually):

roslaunch rtabmap_ros rtabmap.launch \
   frame_id:=body \
   ...
aditdoshi333 commented 3 years ago

Hello,

Thanks @matlabbe for the response. I tried seeting frame_id:=body but getting the following error.

[ WARN] [1630895096.920028898]: /rtabmap_camera_1/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=200). /rtabmap_camera_1/rtabmap subscribed to (approx sync): /Odometry \ /camera1/color/image_raw \ /camera1/aligned_depth_to_color/image_raw \ /camera1/color/camera_info

I even did rostopic hz [list of topics] and the output is as follows

Screenshot from 2021-09-06 07-58-47

For the reference I am attaching output of rostopic echo /tf

` transforms:

header: 
  seq: 0
  stamp: 
    secs: 1630895058
    nsecs: 641886768
  frame_id: "/body"
child_frame_id: "/camera1_link"
transform: 
  translation: 
    x: 0.0
    y: 0.0
    z: 0.0
  rotation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

transforms:

header: 
  seq: 0
  stamp: 
    secs: 100
    nsecs: 799157945
  frame_id: "camera_init"
child_frame_id: "body"
transform: 
  translation: 
    x: 0.0274710170565
    y: 0.129712920275
    z: -0.00861240203739
  rotation: 
    x: -0.000350691396034
    y: 8.12551388396e-05
    z: -0.00287745650531
    w: 0.99999579532

`

camera_init -> body is published by the odometry node and body -> camera1_link is published by static publisher. The following is the code for static publisher.

<node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf" args="0 0 0 0 0 0 /body /camera1_link 100" />

I think I missing a bit somewhere. @matlabbe Please throw some light.

Thank you

matlabbe commented 3 years ago

Compare header of the topics in 4 terminals to see if the stamps are correctly set and not too far from the other topics:

rostopic echo /Odometry/header
rostopic echo /camera1/color/image_raw/header
rostopic echo /camera1/aligned_depth_to_color/image_raw/header
rostopic echo /camera1/color/camera_info/header

For more info about approximate synchronization, see http://wiki.ros.org/message_filters#ApproximateTime_Policy