Ask Your Question

Revision history [back]

robot_localization use case with landmarks

I have a non holonomic mobile base that I want to localize. When I start it I get its odometry published to /odom but with covariances of 0 for both twist and pose. I also fixed a camera on it and can observe and identify landmarks at known global positions and then can estimate the pose of the robot in the world. If I understood well, I set the world frame in my launch file to the map frame as explaned here

If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then:

a- Set your world_frame to your map_frame value

b- MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another instance of a robot_localization state estimation node. However, that instance should not fuse the global data.

Here Is my current launch file:

<!-- Launch file for ekf_localization_node -->

<launch>
    <arg name="use_sim" default="true"/>
    <param name="/use_sim_time" type="bool" value="$(arg use_sim)" />

    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="1"/>
      <param name="two_d_mode" value="true"/>

      <!-- Defaults to "map" if unspecified -->
      <param name="map_frame" value="map"/>
      <!-- Defaults to "odom" if unspecified -->
      <param name="odom_frame" value="odom"/>
      <!-- Defaults to "base_link" if unspecified -->
      <param name="base_link_frame" value="base_link"/>
      <!-- Defaults to the value of "odom_frame" if unspecified -->
      <param name="world_frame" value="map"/>

      <param name="transform_time_offset" value="0.0"/>
      <param name="odom0" value="/odom"/>
      <param name="pose0" value="/my_pose"/>


      <rosparam param="odom0_config">[false, false, false,
                                      false, false, false,
                                      true,  true, false,
                                      false, false, true,
                                      false, false, false]</rosparam>

      <rosparam param="pose0_config">[true,  true,  false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <param name="odom0_differential" value="false"/>
      <param name="pose0_differential" value="false"/>

      <param name="odom0_relative" value="false"/>
      <param name="pose0_relative" value="false"/>


      <param name="print_diagnostics" value="true"/>
      <!-- If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to control
           how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
           numeric_limits<double>::max() if unspecified. -->
      <param name="odom0_pose_rejection_threshold" value="5"/>
      <param name="odom0_twist_rejection_threshold" value="1"/>
      <param name="pose0_rejection_threshold" value="2"/>

      <!-- Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file
           specified by debug_out_file. I hope you like matrices! Defaults to false if unspecified. -->
      <param name="debug"           value="false"/>
      <!-- Defaults to "robot_localization_debug.txt" if unspecified. -->
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <!-- The process noise covariance matrix can be difficult to tune, and can vary for each application, so it
           is exposed as a configuration parameter. The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
           vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if unspecified. -->
      <rosparam param="process_noise_covariance">[0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]</rosparam>

      <!-- This represents the initial value for the state estimate error covariance matrix. Setting a diagonal value (a
           variance) to a large value will result in early measurements for that variable being accepted quickly. Users should
           take care not to use large values for variables that will not be measured directly. The values are ordered as x, y,
           z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if unspecified. -->
           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>


      <!--  Placeholder for output topic remapping
      <remap from="odometry/filtered" to=""/>
      -->

    </node>

</launch>

I play a bag providing /odom and the odom->base_link transform and then launch robot_localization. The topic odometry_filtered is there but nothing gets published on it. At this stage there is nothing published to /my_pose as no landmarks are observed.

What is missing?

robot_localization use case with landmarks

I have a non holonomic mobile base that I want to localize. When I start it I get its odometry published to /odom but with covariances of 0 for both twist and pose. I also fixed a camera on it and can observe and identify landmarks at known global positions and then can estimate the pose of the robot in the world. If I understood well, I set the world frame in my launch file to the map frame as explaned here

If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then:

a- Set your world_frame to your map_frame value

b- MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another instance of a robot_localization state estimation node. However, that instance should not fuse the global data.

Here Is my current launch file:

<!-- Launch file for ekf_localization_node -->

<launch>
    <arg name="use_sim" default="true"/>
    <param name="/use_sim_time" type="bool" value="$(arg use_sim)" />

    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="1"/>
      <param name="two_d_mode" value="true"/>

      <!-- Defaults to "map" if unspecified -->
      <param name="map_frame" value="map"/>
      <!-- Defaults to "odom" if unspecified -->
      <param name="odom_frame" value="odom"/>
      <!-- Defaults to "base_link" if unspecified -->
      <param name="base_link_frame" value="base_link"/>
      <!-- Defaults to the value of "odom_frame" if unspecified -->
      <param name="world_frame" value="map"/>

      <param name="transform_time_offset" value="0.0"/>
      <param name="odom0" value="/odom"/>
      <param name="pose0" value="/my_pose"/>

       <rosparam param="odom0_config">[false, false, false,
                                      false, false, false,
                                      true,  true, false,
                                      false, false, true,
                                      false, false, false]</rosparam>

      <rosparam param="pose0_config">[true,  true,  false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <param name="odom0_differential" value="false"/>
      <param name="pose0_differential" value="false"/>

      <param name="odom0_relative" value="false"/>
      <param name="pose0_relative" value="false"/>

       <param name="print_diagnostics" value="true"/>
      <!-- If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to control
           how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
           numeric_limits<double>::max() if unspecified. -->
       <param name="odom0_pose_rejection_threshold" value="5"/>
      <param name="odom0_twist_rejection_threshold" value="1"/>
      <param name="pose0_rejection_threshold" value="2"/>

      <!-- Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file
           specified by debug_out_file. I hope you like matrices! Defaults to false if unspecified. -->
      <param name="debug"           value="false"/>
      <!-- Defaults to "robot_localization_debug.txt" if unspecified. -->
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <!-- The process noise covariance matrix can be difficult to tune, and can vary for each application, so it
           is exposed as a configuration parameter. The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
           vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if unspecified. -->
      <rosparam param="process_noise_covariance">[0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]</rosparam>

      <!-- This represents the initial value for the state estimate error covariance matrix. Setting a diagonal value (a
           variance) to a large value will result in early measurements for that variable being accepted quickly. Users should
           take care not to use large values for variables that will not be measured directly. The values are ordered as x, y,
           z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if unspecified. -->
           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>


      <!--  Placeholder for output topic remapping
      <remap from="odometry/filtered" to=""/>
      -->

    </node>

</launch>

I play a bag providing /odom and the odom->base_link transform and then launch robot_localization. The topic odometry_filtered is there but nothing gets published on it. At this stage there is nothing published to /my_pose as no landmarks are observed.

What is missing?

robot_localization use case with landmarks

I have a non holonomic mobile base that I want to localize. When I start it I get its odometry published to /odom but with covariances of 0 for both twist and pose. I also fixed a camera on it and can observe and identify landmarks at known global positions and then can estimate the pose of the robot in the world. If I understood well, I set the world frame in my launch file to the map frame as explaned here

If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then:

a- Set your world_frame to your map_frame value

b- MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another instance of a robot_localization state estimation node. However, that instance should not fuse the global data.

Here Is my current launch file:

<launch>
    <arg name="use_sim" default="true"/>
    <param name="/use_sim_time" type="bool" value="$(arg use_sim)" />

    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="1"/>
      <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"/>

      <param name="transform_time_offset" value="0.0"/>
      <param name="odom0" value="/odom"/>
      <param name="pose0" value="/my_pose"/>

      <rosparam param="odom0_config">[false, false, false,
                                      false, false, false,
                                      true,  true, false,
                                      false, false, true,
                                      false, false, false]</rosparam>

      <rosparam param="pose0_config">[true,  true,  false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <param name="odom0_differential" value="false"/>
      <param name="pose0_differential" value="false"/>

      <param name="odom0_relative" value="false"/>
      <param name="pose0_relative" value="false"/>

      <param name="print_diagnostics" value="true"/>

      <param name="odom0_pose_rejection_threshold" value="5"/>
      <param name="odom0_twist_rejection_threshold" value="1"/>
      <param name="pose0_rejection_threshold" value="2"/>

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

    </node>

</launch>

I play a bag providing /odom and the odom->base_link transform and then launch robot_localization. The topic odometry_filtered is there but nothing gets published on it. At this stage there is nothing published to /my_pose as no landmarks are observed.

What is missing?

My odom message looks like this:

header: 
  seq: 4701
  stamp: 
    secs: 1439389120
    nsecs: 950957354
  frame_id: /odom
child_frame_id: /base_footprint
pose: 
  pose: 
    position: 
      x: 17.6170005798
      y: 0.433000028133
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.00999983404194
      w: 0.99995000041
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: -0.286000013351
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

robot_localization use case with landmarks

I have a non holonomic mobile base that I want to localize. When I start it I get its odometry published to /odom but with covariances of 0 for both twist and pose. I also fixed a camera on it and can observe and identify landmarks at known global positions and then can estimate the pose of the robot in the world. If I understood well, I set the world frame in my launch file to the map frame as explaned here

If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark observations) then:

a- Set your world_frame to your map_frame value

b- MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another instance of a robot_localization state estimation node. However, that instance should not fuse the global data.

Here Is my current launch file:

<launch>
    <arg name="use_sim" default="true"/>
    <param name="/use_sim_time" type="bool" value="$(arg use_sim)" />

    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">

      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="1"/>
      <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"/>

      <param name="transform_time_offset" value="0.0"/>
      <param name="odom0" value="/odom"/>
      <param name="pose0" value="/my_pose"/>

      <rosparam param="odom0_config">[false, false, false,
                                      false, false, false,
                                      true,  true, false,
                                      false, false, true,
                                      false, false, false]</rosparam>

      <rosparam param="pose0_config">[true,  true,  false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <param name="odom0_differential" value="false"/>
      <param name="pose0_differential" value="false"/>

      <param name="odom0_relative" value="false"/>
      <param name="pose0_relative" value="false"/>

      <param name="print_diagnostics" value="true"/>

      <param name="odom0_pose_rejection_threshold" value="5"/>
      <param name="odom0_twist_rejection_threshold" value="1"/>
      <param name="pose0_rejection_threshold" value="2"/>

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

    </node>

</launch>

I play a bag providing /odom and the odom->base_link transform and then launch robot_localization. The topic odometry_filtered is there but nothing gets published on it. At this stage there is nothing published to /my_pose as no landmarks are observed.

What is missing?

My odom message looks like this:

header: 
  seq: 4701
  stamp: 
    secs: 1439389120
    nsecs: 950957354
  frame_id: /odom
child_frame_id: /base_footprint
pose: 
  pose: 
    position: 
      x: 17.6170005798
      y: 0.433000028133
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.00999983404194
      w: 0.99995000041
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: -0.286000013351
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

And here is the tf tree coming from the bag, the transform between map and odom was being published by amcl:

image description