robot_localization no output
I am using robot_localization package to fuse a GPS and 9-axis IMU. My robot does not have encoders. I am using navsat_transform_node
and ekf_localization_node
It is not throwing any error when roslaunch, but it also does not output anything into the topics /gps/filtered
, /odometry/filtered
, /odometry/gps
, and /tf
.
Here is the content in my /fix and /imu/data topics as inputs to the nodes:
/fix 1Hz
header:
seq: 1124
stamp:
secs: 1631306868
nsecs: 997335910
frame_id: "gps_link"
status:
status: 1
service: 1
latitude: 53.3122313333
longitude: -113.580674167
altitude: 713.6
position_covariance: [9.4864, 0.0, 0.0, 0.0, 2.3716, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 1
/imu/data 50Hz
header:
seq: 91253
stamp:
secs: 1631306937
nsecs: 299692392
frame_id: "imu_link"
orientation:
x: -0.0831276113812
y: -0.00292960744956
z: -0.50737139017
w: 0.857703614346
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: 0.0
y: 0.0
z: 0.0
angular_velocity_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: 0.85
y: -1.32
z: 9.49
linear_acceleration_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Here is the output of roslaunch, it just stops like this for hours.
Here is my launch file, it has 2 robot localization nodes and 2 static tf publishers.
<launch>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" clear_params="true">
<param name="magnetic_declination_radians" value="0"/>
<param name="zero_altitude" value="false"/>
<param name="publish_filtered_gps" value="true"/>
<param name="broadcast_cartesian_transform" value="true"/>
<param name="wait_for_datum" value="false"/>
<param name="navsat_transform_node" value="false"/>
<remap from="imu/data" to="/imu/data"/>
<remap from="gps/fix" to="/fix"/>
</node>
<node name="bl_gps" pkg="tf2_ros" type="static_transform_publisher" args="0 0 1.0 0 0 0 1 base_link gps_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="imutf" args="0 0 0.5 0 0 0 1 base_link imu_link "/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom" clear_params="true">
<param name="odom0" value="/odometry/gps"/>
<param name="imu0" value="/imu/data"/>
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="2"/>
<param name="two_d_mode" value="true"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="map"/>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<rosparam param="odom0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="print_diagnostics" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="$(env HOME)/adroit_files/debug_ekf_localization.txt"/>
< ...
For my IMU, its model is BNO055. I think it is in ENU mode. Its Z-angle in Euler angle is ~0 when the +y axis point to North.
Try enabling diagnostics and listening to that topic (It should already be enabled, try listening to the /diagnostics topic)