ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
So there are a lot of issues here:
ekf_localization_node
into your odom_transform instance. That is probably not going to work well at all. wait_for_datum
set to true for navsat_transform_node
, but your other settings indicate that you don't actually want this set to true. I just realized that I need to document that parameter on the wiki, so that's my fault. In any case, it's not relevant for your use case.I did find a bug with the node crashinig when publish_filtered_gps
was set to true. I have fixed it and it is in the upstream repository. Please pull down that version until I can get a new release out.
Anyway, here are the settings that worked for me. Please note that I added a static transform publisher for the base_link->gps transform:
<launch>
<node name="bl_gps" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 1 base_link gps_link" />
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
<param name="magnetic_declination_radians" value="0.36878808"/>
<param name="zero_altitude" value="false"/>
<param name="publish_filtered_gps" value="true"/>
<param name="broadcast_utm_transform" value="true"/>
<param name="wait_for_datum" value="false"/>
<remap from="/gps/fix" to="/gps"/>
<remap from="/imu/data" to="/imu"/>
</node>
<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"/>
<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="odom"/>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, true,
true, 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"/>
</node>
</launch>