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
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
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.
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