Init jump with Robot_localization

asked 2019-09-20 09:13:49 -0500


i'm using a robot with IMU and GPS and robot_localization node with navsat_transform_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 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"/>

``` 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 initial_state param. Indeed, the initial_state 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 ?

edit retag flag offensive close merge delete