Problem while integrating GPS data into robot_localization
Hi to Tom and everybody,
I'm currently making some experiments with robot_localization, and I'm having some problems with the integration of GPS data for global localization of a non-holonomic mobile robot constrained to a 2D plane (no Z coordinate and no roll/pitch).
Here is the overview of my setup for odometry which is very similar to the one described in this section of the wiki where the use of a dual EKF node is discussed.
- X and Y are bidimensional coordinates
- Vx and Vy are linear velocities
- H^, X^^ and Y^^ are angular velocity and linear accelerations, respectively
- The topic name is inside parentheses
- The provided TF transformation is inside square brackets
This is my launch file (only the part relevant to replay the bag file):
<launch>
<!-- Reference frames -->
<arg name="world_frame" default="earth"/>
<arg name="map_frame" default="map_link"/>
<arg name="odom_frame" default="odom_link"/>
<arg name="robot_frame" default="base_link"/>
<arg name="camera_frame" default="camera_link"/>
<!-- Odometry topics -->
<arg name="wheels_odom" default="/odometry/wheels"/>
<arg name="imu_odom" default="/odometry/imu"/>
<arg name="gps_odom" default="/odometry/gps"/>
<arg name="navsat_odom" default="/odometry/navsat"/>
<arg name="visual_odom" default="/odometry/visual"/>
<arg name="local_odom" default="/odometry/local"/>
<arg name="global_odom" default="/odometry/global"/>
<!-- Sensor fusion 1 -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf1_local" clear_params="true" output="screen">
<param name="frequency" value="50"/>
<param name="sensor_timeout" value="0.2"/>
<param name="two_d_mode" value="true"/>
<param name="map_frame" value="$(arg map_frame)"/>
<param name="odom_frame" value="$(arg odom_frame)"/>
<param name="base_link_frame" value="$(arg robot_frame)"/>
<param name="world_frame" value="$(arg odom_frame)"/>
<param name="use_control" value="false"/>
<param name="print_diagnostics" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="~/Log/ekf1.txt"/>
<remap from="odometry/filtered" to="$(arg local_odom)"/>
<param name="odom0" value="$(arg wheels_odom)"/>
<rosparam param="odom0_config">
[ false, false, false,
false, false, false,
true, true, false,
false, false, false,
false, false, false ]
</rosparam>
<param name="odom0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="odom0_queue_size" value="10"/>
<param name="odom0_nodelay" value="true"/>
<param name="imu0" value="$(arg imu_odom)"/>
<rosparam param="imu0_config">
[ false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, true, false ]
</rosparam>
<param name="imu0_differential" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_queue_size" value="10"/>
<param name="imu0_nodelay" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="false"/>
</node>
<!-- GPS coordinates transformation -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true" output="screen">
<param name="frequency" value="20"/>
<param name="yaw_offset" value="1.570796327"/>
<param name="zero_altitude" value="true"/>
<param name="publish_filtered_gps" value="false"/>
<param name="use_odometry_yaw" value="false"/>
<param name="wait_for_datum" value="true"/>
<param name="broadcast_utm_transform" value="false"/>
<param name="delay" value="3"/>
<param name="magnetic_declination_radians" value="0.00383972"/>
<rosparam param="datum">[43.542883, 11.571365, 0]</rosparam>
<remap from="imu/data" to="$(arg imu_odom)"/>
<remap from="odometry/filtered" to="$(arg local_odom)"/>
<remap from="gps/fix" to="$(arg gps_odom)"/>
<remap from="odometry/gps" to="$(arg navsat_odom)"/>
</node>
<!-- Sensor fusion 2 -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf2_global" clear_params="true" output="screen">
<param name="frequency" value="10"/>
<param name ...