Question

Hi,

i'm using a robot with IMU and GPS and robotlocalization node with navsattransform_node works perfectly expect the first published TF.

Here is the launcher I use :

    <node pkg="tf2_ros" type="static_transform_publisher" name="link_base_gps" args="0 0 0 0 0 0 base_link gps" />

    <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">
        <param name="zero_altitude" value="false"/>
        <param name="yaw_offset" value="0" />
        <param name="publish_filtered_gps" value="true"/>
        <param name="broadcast_utm_transform" value="true"/>
        <param name="wait_for_datum" value="true"/>
        <rosparam param="datum">[40.3, 1.05, 0.0]</rosparam>
    </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/data"/>

        <param name="frequency" value="30"/>
        <param name="sensor_timeout" value="2"/>
        <param name="two_d_mode" value="true"/>
        <param name="delay" value="5.0" />

        <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,
                                 true, true, true,
                                 true, true, true,
                                 false, false, false,
                                 true, true, true]</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"/>
    </node>

The odom->base_link is published by the robot node.

The problem I got is that the first map->odom published TF is a zero TF, then (depending on the initial covariance) it slowly goes to the good TF. Even if I set init cov to 1e9 in every direction, the first map->odom TF will be zero and the odom will be at the same position than map. The problem is that I use the TF to create a occupancy grid and it project the point cloud with this wrong TF at the beginning ...

Screenshot from 2019-09-20 15-59-17

How can I changed that without use the initialstate param. Indeed, the initialstate isn't known so I can hard write this in the launch file.

Is there any parameter to disable the first map->odom TF to be published ?

Asked by ClementLB on 2019-09-20 09:13:49 UTC

Answer 1

Without seeing sensor data inputs (please include those in the future), my guess is that you are seeing this:

Time t0: filter initialises. Internal state is all 0, but we don't publish anythning.
Time t1: receive IMU message. We only have orientation, velocity, and acceleration. We publish the first output, which has all 0s for position.
Time t2: navsat_transform_node receives the same IMU data, along with the GPS data, and produces a non-zero pose estimate. This is passed to the EKF.
Time t3: EKF receives pose data from navsat_transform_node, and fuses it, causing the EKF output to move towards the GPS pose. Each subsequent measurement drags the state estimate closer to the GPS pose.

There isn't currently a way to force the EKF to wait for a specific sensor input before it begins fusion. This could be added pretty trivially, though, and if it's a problem for you, I'd welcome a PR.

In the meantime, you could also modify your other nodes to wait until the first EKF message that has a time stamp after the first navsat_transform_node output message.

Asked by Tom Moore on 2019-11-18 04:31:34 UTC

Init jump with Robot_localization

Comments

Answers

Comments