Robotics StackExchange | Archived questions

Help needed to get simple IMU localization working

I am having trouble getting a simple imu robot localization going. I am interested in observing the imu drift and am not using an odometer. A method to correct the drift will be added later. The imu is the BN0055 and it is serviced by the rtimulib_ros package.

Problem 1: The rosout topic puts out the following every 2 seconds:

name: "/ekf_se"
msg: "Could not obtain transform from map to base_link. Error was \"base_link\" passed\
  \ to lookupTransform argument target_frame does not exist. \n"
file: "/tmp/binarydeb/ros-kinetic-robot-localization-2.4.3/src/ros_filter_utilities.cpp"
function: "RosFilterUtilities::lookupTransformSafe"
line: 135

Problem 2: No position estimate comes out any of the topics. I'm assuming the package will double integrate the imu accelerations to calculate a position estimate. Which topic should I get the position?

rostopic list:

/cmd_vel                # no activity
/diagnostics            # "The robot_localization state estimation node appears to be functioning properly."
/example/another_odom   # no activity
/example/odom           # no activity
/example/pose           # no activity
/example/twist          # no activity
/imu                    # correct imu data is streaming
/odometry/filtered      # no activity
/rosout                 # The map transform error message
/rosout_agg             # The map transform error message
/set_pose               # no activity
/tf                     # transform messages streaming, example below
/tf_static              # no activity

typical tf message:

transforms: 
   header: 
      seq: 0
      stamp: 
        secs: 1526568019
        nsecs: 523587724
      frame_id: "link1_parent"
    child_frame_id: "link1"
    transform: 
      translation: 
        x: 1.0
        y: 0.0
        z: 0.0
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0

typical imu message:

header: 
  seq: 93788
  stamp: 
    secs: 1526572296
    nsecs: 264639593
  frame_id: "map"
orientation: 
  x: -0.0779868140817
  y: 0.0903641656041
  z: 0.0258044376969
  w: 0.992515265942
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]   
angular_velocity: 
  x: 0.00111111113802
  y: -0.0
  z: -0.0
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.186000004411
  y: -0.142000004649
  z: 0.972000002861
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

robot_localization launch file:

<launch>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
    <rosparam command="load" file="$(find robot_localization)/params/translate_ekf.yaml" />
  </node>

I have inserted the following code from FrozenCh here.  Was using regular TF.  Still get same error msg
<node pkg="tf2_ros" type="static_transform_publisher" name="tf2_imu_broadcaster" args="0 0 0 0 0 0 imu_frame base_link"/>

</launch>

translate_ekf.yaml:

frequency: 30
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false
map_frame: map              
odom_frame: odom            
base_link_frame: base_link  
world_frame: odom           
odom0: example/odom
odom0_config: [true,  true,  false,
           false, false, false,
           false, false, false,
           false, false, true,
           false, false, false]
odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1
odom1: example/another_odom
odom1_config: [false, false, true,
           false, false, false,
           false, false, false,
           false, false, true,
           false, false, false]
odom1_differential: false
odom1_relative: true
odom1_queue_size: 2
odom1_pose_rejection_threshold: 2
odom1_twist_rejection_threshold: 0.2
odom1_nodelay: false
pose0: example/pose
pose0_config: [true,  true,  false,
           false, false, false,
           false, false, false,
           false, false, false,
           false, false, false]
pose0_differential: true
pose0_relative: false
pose0_queue_size: 5
pose0_rejection_threshold: 2
pose0_nodelay: false
twist0: example/twist
twist0_config: [false, false, false,
            false, false, false,
            true,  true,  true,
            false, false, false,
            false, false, false]
twist0_queue_size: 3
twist0_rejection_threshold: 2
twist0_nodelay: false
imu0: /imu #changed to match rtimulib_ros output
imu0_config: [
          false, false, false,   # x, y, z
          true,  true,  true,    # roll, pitch, yaw
          false, false, false,   # x velocity,  y velocity, z velocity 
          true,  true,  true,    # roll velocity, pitch velocity, yaw velocity
          true,  true,  true]    # x accel, y accel, z accel
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8                
imu0_linear_acceleration_rejection_threshold: 0.8  
imu0_remove_gravitational_acceleration: false
use_control: true
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
process_noise_covariance: [omitted but the default small value diagonal]
initial_estimate_covariance: [omitted but the default small value diagonal]

Thanks for any ideas to get this running.

Asked by babazaroni on 2018-05-16 16:22:42 UTC

Comments

If you provide more details on how you're launching robot_localization (i.e., the specific launch file / configs) it will be easier to understand what you're doing and help.

Asked by stevejp on 2018-05-16 19:55:50 UTC

My question was rather ill defined. I'll post another question with the launch file and configs.

Asked by babazaroni on 2018-05-16 23:08:31 UTC

Couldn't you just edit this one?

Asked by gvdhoorn on 2018-05-17 02:23:09 UTC

try this in your launch to link base_link and imu_frame in tf.

<node pkg="tf2_ros" type="static_transform_publisher" name="tf2_imu_broadcaster" args="0 0 0 0 0 0 imu_frame base_link"/>

in which, the zeros means "x y z yaw pitch roll"

Asked by FrozenCh on 2018-05-17 03:13:16 UTC

@FrozenCh I am using the tf, instead of the tf2_ros. That should work right?

Asked by babazaroni on 2018-05-17 10:08:37 UTC

I have edited the question to include more details.

Asked by babazaroni on 2018-05-17 10:10:15 UTC

@FrozenCh I have put your code into the launch file, but still get the same results

Asked by babazaroni on 2018-05-17 10:23:31 UTC

Answers

The frame_id for the imu was set to "map". See the typical imu message in the op. There was a comment there that this was for Rviz. When the frame_id was set to imu_frame, the map error was fixed.

Asked by babazaroni on 2018-05-17 16:34:47 UTC

Comments