TF broken and not correct in Hector Pose Estimation when fusing IMU and Pressure Sensor

asked 2021-10-03 10:43:25 -0500

Astronaut gravatar image

updated 2021-10-03 11:33:05 -0500

Im doing some underwater UUV Gazebo simulation and would like to use the

hector_pose_estimation

package to fuse IMU and Fluid pressure sensors input for pose estimation. In the gazebo launch file I add this tf transformation <node pkg="tf" type="static_transform_publisher" name="world_to_nav" args="0 0 0 0 0 0 /world /nav 100" /> . Im not sure is need it and is the correct one, so the tf looks like this gazebo tf after adding the tf transformation line in the launch file

when using the following hector_pose_estimation launch file

<?xml version="1.0"?>
<launch>
  <node pkg="hector_pose_estimation" type="pose_estimation" name="hector_pose_estimation">
    <remap from="raw_imu" to="/rexrov2/imu" />
    <remap from="pressure_height" to="/pressure_height" />
  </node>

   <node pkg="tf" type="static_transform_publisher" name="nav_to_base_footprint" args="0 0 0 0 0 0 /nav /rexrov2/base_footprint 100" />
</launch>

The TF is following hector_pose launch tf

The both TF from gazebo and hector_pose_estimation are not correct. Any help how to get the correct frames to make it work? And when using the above launch the hector_pose_estimation /pose is like these

header: 
  seq: 329
  stamp: 
    secs: 781
    nsecs: 118000000
  frame_id: "nav"
pose: 
  position: 
    x: 0.0
    y: 0.0
    z: -1.64102132948e-09
  orientation: 
    x: -8.80621330591e-06
    y: 0.000294167493465
    z: 3.71623915803e-14
    w: 0.999999956694

Which is not correct as I have the true pose which if following

header: 
  seq: 15460
  stamp: 
    secs: 781
    nsecs: 608000000
  frame_id: "world"
child_frame_id: "rexrov2/base_link"
pose: 
  pose: 
    position: 
      x: 0.083413690347
      y: 0.133535986662
      z: -0.77756440376
    orientation: 
      x: -9.34768900238e-06
      y: 0.000294151069482
      z: 0.00184072884918
      w: 0.999998262551

In the hector /pose the frame_id: "nav" and no child_frame. In the real pose is frame_id: "world" and child_frame_id: "rexrov2/base_link" (rexrov2 is the name of the robot) . So any help how to fix it?

Thanks

edit retag flag offensive close merge delete