CCNYRoboticsLab / imu_tools

ROS tools for IMU devices
Other
908 stars 429 forks source link

"odom" passed to lookupTransform argument target_frame does not exist. #194

Open sahilsaqi opened 6 months ago

sahilsaqi commented 6 months ago

I am using Ros Noetic on Ubuntu 20.04 LTS

and I installed this library for integration of IMU (mpu6050_driver) with Hector SLAM. basically as you know mpu6050_driver provides /imu/raw/data so I need to filter this using madgwick filter,

so here is the steps I am taking

  1. roscore
  2. roslaunch rplidar_ros rplidar_a1.launch
  3. roslaunch hector_slam_launch tutorial.launch
  4. rosrun imu_filter_madgwick imu_filter_node _use_mag:=false

also my rostopics are

aiot@ubuntu:~/ros_slam$ rostopic list
/ImuFilter/parameter_descriptions
/ImuFilter/parameter_updates
/clicked_point
/imu/data
/imu/data_raw
/imu/orientation_filtered
/imu/rpy/filtered
/imu/rpy/raw
/initialpose
/map
/map_metadata
/map_updates
/move_base_simple/goal
/poseupdate
/rosout
/rosout_agg
/scan
/slam_cloud
/slam_out_pose
/syscommand
/tf
/tf_static
/trajectory

output of /imu/data

aiot@ubuntu:~/ros_slam$ rostopic echo /imu/data
header:
 seq: 1474
  stamp:
    secs: 1711980942
    nsecs: 312890118
  frame_id: "imu"
orientation:
 x: 0.006631039159660429
  y: -0.0016873730512080361
  z: 0.3952459307362719
  w: 0.9185498551130837
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
  x: -0.0015987749211490154
  y: 0.003197549842298031
  z: -0.0014655436389148235
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
  x: -0.16645386815071106
  y: 0.19040405750274658
  z: 9.604028701782227
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
header:
  seq: 1475
  stamp:
    secs: 1711980942
    nsecs: 393032546
  frame_id: "imu"
orientation:
  x: 0.00964514501704718
  y: -0.009240772044757012
  z: 0.3955919134124634
  w: 0.9183292532377934
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
  x: -0.00559571199119091
  y: -0.002531393663957715
  z: 0.009059724397957325

and i want to change the frame id of /imu/data as imu_link because the frame id's are same for /imu/data_raw also so that it cannot create any buzz in the future

also, you can check my tf graph

image

and Finally here is the output of the madgwick command

  1. rosrun imu_filter_madgwick imu_filter_node _use_mag:=false
aiot@ubuntu:~/ros_slam$ rosrun imu_filter_madgwick imu_filter_node _use_mag:=false imu/data:=imu_link
[ INFO] [1711980219.435861494]: Starting ImuFilter
[ INFO] [1711980220.362094425]: Using dt computed from message headers
[ INFO] [1711980220.362490459]: The gravity vector is kept in the IMU message.
[ INFO] [1711980221.504864241]: Imu filter gain set to 0.100000
[ INFO] [1711980221.505046887]: Gyro drift bias set to 0.000000
[ INFO] [1711980221.505117424]: Magnetometer bias values: 0.000000 0.000000 0.000000
[ INFO] [1711980222.197121883]: First IMU message received.
[ WARN] [1711980222.305613436]: "odom" passed to lookupTransform argument target_frame does not exist.
[ WARN] [1711980222.407043290]: Lookup would require extrapolation at time 1711980222.232689142, but only time 1711980222.192807436 is in the buffer, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980222.508990416]: Lookup would require extrapolation 0.044581330s into the future.  Requested time 1711980222.277270317 but the latest data is at time 1711980222.232689142, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980222.610554379]: Lookup would require extrapolation 0.035531702s into the future.  Requested time 1711980222.312802076 but the latest data is at time 1711980222.277270317, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980222.712061917]: Lookup would require extrapolation 0.119802977s into the future.  Requested time 1711980222.432605028 but the latest data is at time 1711980222.312802076, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980222.813529918]: Lookup would require extrapolation 0.080046348s into the future.  Requested time 1711980222.512651443 but the latest data is at time 1711980222.432605028, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980222.915514322]: Lookup would require extrapolation 0.120317491s into the future.  Requested time 1711980222.632968903 but the latest data is at time 1711980222.512651443, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980223.018045944]: Lookup would require extrapolation 0.120189399s into the future.  Requested time 1711980222.753158331 but the latest data is at time 1711980222.632968903, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980223.121265448]: Lookup would require extrapolation 0.079526648s into the future.  Requested time 1711980222.832684994 but the latest data is at time 1711980222.753158331, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980223.227303781]: Lookup would require extrapolation 0.120277344s into the future.  Requested time 1711980222.952962399 but the latest data is at time 1711980222.832684994, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980223.328893410]: Lookup would require extrapolation 0.079751368s into the future.  Requested time 1711980223.032713652 but the latest data is at time 1711980222.952962399, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980223.430638297]: Lookup would require extrapolation 0.120016474s into the future.  Requested time 1711980223.152730227 but the latest data is at time 1711980223.032713652, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980223.532295870]: Lookup would require extrapolation 0.080413011s into the future.  Requested time 1711980223.233143091 but the latest data is at time 1711980223.152730227, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980223.633503965]: Lookup would require extrapolation 0.125268338s into the future.  Requested time 1711980223.358411551 but the latest data is at time 1711980223.233143091, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980223.734671654]: Lookup would require extrapolation 0.114230245s into the future.  Requested time 1711980223.472641706 but the latest data is at time 1711980223.358411551, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980223.835897193]: Lookup would require extrapolation 0.084709160s into the future.  Requested time 1711980223.557350874 but the latest data is at time 1711980223.472641706, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980223.937684728]: Lookup would require extrapolation 0.115282181s into the future.  Requested time 1711980223.672633171 but the latest data is at time 1711980223.557350874, when looking up transform from frame [imu] to frame [odom]
[ WARN] [1711980224.039572262]: Lookup would require extrapolation 0.080715063s into the future.  Requested time 1711980223.753348112 but the latest data is at time 1711980223.672633171, when looking up transform from frame [imu] to frame [odom]

if you need any clarification please reply soon,

i need help here

mintar commented 6 months ago

The imu_filter_madgwick shouldn't publish the odom -> imu transform. This is just for visualization/debugging. You can disable it like this:

rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _publish_tf:=false

The odom -> imu transform published by the IMU only contains the rotation, not the translation. On a wheeled robot, you need to use robot_localization or something equivalent to fuse the IMU values and the wheel odometry to obtain a TF transform like this:

  graph TD;
      IMU-driver -->|/imu/data_raw| imu_filter_madgwick;
      imu_filter_madgwick -->|/imu/data| robot_localization;
      your-robot-driver -->|/odom| robot_localization;
      robot_localization --> |tf-transform| hector_mapping;

... where the /odom topic has the message type nav_msgs/Odometry, /imu/data_raw and /imu/data topics have type sensor_msgs/Imu, and the "tf-transform" is a TF transform like odom -> base_footprint that is required (along with other TF transforms from the URDF) by hector_mapping.

sahilsaqi commented 6 months ago

The imu_filter_madgwick shouldn't publish the odom -> imu transform. This is just for visualization/debugging. You can disable it like this:

rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _publish_tf:=false

The odom -> imu transform published by the IMU only contains the rotation, not the translation. On a wheeled robot, you need to use robot_localization or something equivalent to fuse the IMU values and the wheel odometry to obtain a TF transform like this:

  graph TD;
      IMU-driver -->|/imu/data_raw| imu_filter_madgwick;
      imu_filter_madgwick -->|/imu/data| robot_localization;
      your-robot-driver -->|/odom| robot_localization;
      robot_localization --> |tf-transform| hector_mapping;

... where the /odom topic has the message type nav_msgs/Odometry, /imu/data_raw and /imu/data topics have type sensor_msgs/Imu, and the "tf-transform" is a TF transform like odom -> base_footprint that is required (along with other TF transforms from the URDF) by hector_mapping.

okay I just tried this approach

you can see the graph below, but there is still some problem.

image

i have used the static transform publisher instead of robot localization because i am using hand held device so i don't have any wheeled robot

here i am providing the static publisher launch file.

<launch>

<node pkg="tf2_ros" type="static_transform_publisher" name="test_static_pub_imu" args="0 0 0 0 0 0 1 base_link imu" />
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="test_static_pub_lidar" args="0 0 0 0 0 0 0 base_link lidar" /> -->

  <!-- Start the static tf publisher with a valid tf.  There is a test
       (tf_from_param_server_valid) that ensures the TF is published. -->
  <rosparam param="test_tf2/tf_valid"
            file="$(find test_tf2)/test/test_tf_valid.yaml" />
  <node pkg="tf2_ros" type="static_transform_publisher"
        name="test_static_pub_param_valid"
        args="test_tf2/tf_valid" />

  <!-- Start the static tf publisher with an *invalid* tf.
       The main purpose of this test is to ensure the node dies gracefully. -->
  <rosparam param="test_tf2/tf_invalid"
            file="$(find test_tf2)/test/test_tf_invalid.yaml" />
  <node pkg="tf2_ros" type="static_transform_publisher"
        name="test_static_pub_param_invalid"
        args="test_tf2/tf_invalid" />

  <!-- Start the static tf publisher with a non-existent parameter.
       The main purpose of this test is to ensure the node dies gracefully. -->
  <node pkg="tf2_ros" type="static_transform_publisher"
        name="test_static_pub_param_null"
        args="test_tf2/tf_null" />

  <test test-name="test_static_publisher" pkg="test_tf2"
        type="test_static_publisher" args="--text" />

  <test test-name="test_static_publisher_py"
        pkg="test_tf2"
        type="test_static_publisher.py"
        launch-prefix="python$(env ROS_PYTHON_VERSION)"/>
</launch> 

i am pretty sure that there is something wrong.

that is why I am getting drift on map while moving a bit or rotate the handheld device

2024-02-14_15-36

these are my topics

aiot@ubuntu:~$ rostopic list /ImuFilter/parameter_descriptions /ImuFilter/parameter_updates /clicked_point /imu/data /imu/data_raw /initialpose /map /map_metadata /map_updates /move_base_simple/goal /poseupdate /rosout /rosout_agg /scan /slam_cloud /slam_out_pose /syscommand /tf /tf_static /trajectory ``

Also you can check my tree

frames

mintar commented 6 months ago

So if you look at the hector_mapping documentation, you'll see the following:

Required tf Transforms

<the frame attached to incoming scans> → base_frame - usually a fixed value, broadcast periodically by a robot_state_publisher, or a tf static_transform_publisher.

... and:

~pub_map_odom_transform (bool, default: true) -- Determine if the map->odom transform should be published by the system.

So it looks like in your case, you'll need the base_link -> laser transform, which you can publish via a static_transform_publisher. Also, if you set the ~pub_map_odom_transform parameter (or just leave it as default), hector_mapping will publish map to odom, so you mustn't do that yourself.

Also, I don't see any option in hector_slam to use IMU input, so you probably don't need this package here at all.

Anyway, this discussion has nothing to do with imu_tools, so please ask any follow-up questions on https://robotics.stackexchange.com/.

Good luck!