robot_localization no output

asked 2021-09-10 16:10:00 -0500

jephph gravatar image

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

This link is rqt_graph

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"/>

  < ...
(more)
edit retag flag offensive close merge delete

Comments

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.

jephph gravatar image jephph  ( 2021-09-10 16:17:22 -0500 )edit

Try enabling diagnostics and listening to that topic (It should already be enabled, try listening to the /diagnostics topic)

bob-ROS gravatar image bob-ROS  ( 2021-09-11 14:47:39 -0500 )edit