Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Global localization using robot_localization node and AR tags

Hi,

We are working on a project and we have big issues using robot_localization to work as a global localizer. We are using ROS Kinetic, Gazebo 7.0, a Kinect camera and ekf_localization_node as filter node. The final goal is get out from the filter absolute robot positions by fusing odometry sensor data and AR codes distance information coming from ar_track_alvar.

No matter how we feed the filter with marker's data, the map frame just heavily jumps, and as consequence the final robot's pose is erroneous. When no tags are detected, so only feeding Ekf with the odometry, the map frame stays in place, as seen here:

The ekf.param file is seen below:

frequency: 40
sensor_timeout: 1
two_d_mode: true
#transform_time_offset: 0.0
#print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: map          # Defaults to the value of odom_frame if unspecified

dynamic_process_noise_covariance: true

odom0: od
odom0_config: [true,  true,  false,
               false, false, true,
               false, false, false,
              false, false, false,
               false, false, false]
odom0_differential: true
odom0_relative: false
odom0_queue_size: 40
odom0_nodelay: false

odom1: marker1
odom1_config: [true,  true,  false,
              false, false, true,
               false, false, false,
              false, false, false,
               false, false, false]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 40
odom1_nodelay: false

process_noise_covariance: [0.0001, 0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.0001, 0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.0001, 0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.0001, 0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.0001, 0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.0001, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0.0001, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0.0001, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0.0001, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0.0001, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0.0001, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0.0001, 0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0.0001, 0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0.0001, 0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0.0001]


initial_estimate_covariance: [1, 0,    0,    0,    0,    0, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1, 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,    1, 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]

Marker1 topic is of type nav_msgs/Odometry and contains the distance information from the marker1 tag to the map frame, as a result of the output_frame parameter in the ar_track_alvar launch file. In this way the filter is feed with absolute data, and not relative distance data of the markers. Below the ar_track_alvar launch file:

<launch> <arg name="marker_size" default="5"/> <arg name="max_new_marker_error" default="0.02"/> <arg name="max_track_error" default="0.2"/>

<arg name="cam_image_topic" default="/camera/depth/points" />
<arg name="cam_info_topic" default="/camera/rgb/camera_info" />
<arg name="output_frame" default="/map" />
<arg name="marker_resolution" default="5" />

<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen">
    <param name="marker_size"           type="double" value="$(arg marker_size)" />
    <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
    <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
    <param name="output_frame"          type="string" value="$(arg output_frame)" />

    <remap from="camera_image"  to="$(arg cam_image_topic)" />
    <remap from="camera_info"   to="$(arg cam_info_topic)" />
</node>

</launch>

But as soon as a marker is detected, the /map frame gets lost, and starts rotating around the /odom frame. Below the tf tree and rqt_graph output:

image description

image description

We might have misunderstood how the ekf_localization_node is designed to work with absolute positioning, and for this reason we kindly ask someone, hopefully the package designers, to light up our minds on what we are missing/misunderstanding. For the specific case, the question would be: How to make the ekf know my markers' absolute position? How to relate marker's position to the map frame, which is our global reference? Broadcasting a tf is not allowed, as the tf tree would be complaining on marker having two parents (map and camera_link).

Thanks for anyone helping us out,

Regards

Global localization using robot_localization node and AR tags

Hi,

We are working on a project and we have big issues using robot_localization to work as a global localizer. We are using ROS Kinetic, Gazebo 7.0, a Kinect camera and ekf_localization_node as filter node. The final goal is get out from the filter absolute robot positions by fusing odometry sensor data and AR codes distance information coming from ar_track_alvar.

No matter how we feed the filter with marker's data, the map frame just heavily jumps, and as consequence the final robot's pose is erroneous. When no tags are detected, so only feeding Ekf with the odometry, the map frame stays in place, as seen here:

The ekf.param file is seen below:

frequency: 40
sensor_timeout: 1
two_d_mode: true
#transform_time_offset: 0.0
#print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: map          # Defaults to the value of odom_frame if unspecified

dynamic_process_noise_covariance: true

odom0: od
odom0_config: [true,  true,  false,
               false, false, true,
               false, false, false,
              false, false, false,
               false, false, false]
odom0_differential: true
odom0_relative: false
odom0_queue_size: 40
odom0_nodelay: false

odom1: marker1
odom1_config: [true,  true,  false,
              false, false, true,
               false, false, false,
              false, false, false,
               false, false, false]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 40
odom1_nodelay: false

process_noise_covariance: [0.0001, 0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.0001, 0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.0001, 0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.0001, 0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.0001, 0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.0001, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0.0001, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0.0001, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0.0001, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0.0001, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0.0001, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0.0001, 0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0.0001, 0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0.0001, 0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0.0001]


initial_estimate_covariance: [1, 0,    0,    0,    0,    0, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1, 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,    1, 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]

Marker1 topic is of type nav_msgs/Odometry and contains the distance information from the marker1 tag to the map frame, as a result of the output_frame parameter in the ar_track_alvar launch file. In this way the filter is feed with absolute data, and not relative distance data of the markers. Below the ar_track_alvar launch file:

<?xml version="1.0"?>
<launch>
    <arg name="marker_size" default="5"/>
default="5" />
    <arg name="max_new_marker_error" default="0.02"/> 
default="0.02" /> <!-- original 0.08-->
    <arg name="max_track_error" default="0.2"/> 

default="0.2" /> <!-- original 0.2-->

    <arg name="cam_image_topic" default="/camera/depth/points" />
 <arg name="cam_info_topic" default="/camera/rgb/camera_info" />
 <arg name="output_frame" default="/map" />
 <arg name="marker_resolution" default="5" />

 <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen">
     <param name="marker_size"           type="double" value="$(arg marker_size)" />
     <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
     <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
     <param name="output_frame"          type="string" value="$(arg output_frame)" />

     <remap from="camera_image"  to="$(arg cam_image_topic)" />
     <remap from="camera_info"   to="$(arg cam_info_topic)" />
 </node>

</launch>

</launch>

But as soon as a marker is detected, the /map frame gets lost, and starts rotating around the /odom frame. Below the tf tree and rqt_graph output:

image description

image description

We might have misunderstood how the ekf_localization_node is designed to work with absolute positioning, and for this reason we kindly ask someone, hopefully the package designers, to light up our minds on what we are missing/misunderstanding. For the specific case, the question would be: How to make the ekf know my markers' absolute position? How to relate marker's position to the map frame, which is our global reference? Broadcasting a tf is not allowed, as the tf tree would be complaining on marker having two parents (map and camera_link).

Thanks for anyone helping us out,

Regards

Global localization using robot_localization node and AR tags

Hi,

We are working on a project and we have big issues using robot_localization to work as a global localizer. We are using ROS Kinetic, Gazebo 7.0, a Kinect camera and ekf_localization_node as filter node. The final goal is get out from the filter absolute robot positions by fusing odometry sensor data and AR codes distance information coming from ar_track_alvar.

No matter how we feed the filter with marker's data, the map frame just heavily jumps, and as consequence the final robot's pose is erroneous. When no tags are detected, so only feeding Ekf with the odometry, the map frame stays in place, as seen here:

The ekf.param file is seen below:

frequency: 40
sensor_timeout: 1
two_d_mode: true
#transform_time_offset: 0.0
#print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: map          # Defaults to the value of odom_frame if unspecified

dynamic_process_noise_covariance: true

odom0: od
odom0_config: [true,  true,  false,
               false, false, true,
               false, false, false,
              false, false, false,
               false, false, false]
odom0_differential: true
odom0_relative: false
odom0_queue_size: 40
odom0_nodelay: false

odom1: marker1
odom1_config: [true,  true,  false,
              false, false, true,
               false, false, false,
              false, false, false,
               false, false, false]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 40
odom1_nodelay: false

process_noise_covariance: [0.0001, 0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.0001, 0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.0001, 0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.0001, 0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.0001, 0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.0001, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0.0001, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0.0001, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0.0001, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0.0001, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0.0001, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0.0001, 0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0.0001, 0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0.0001, 0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0.0001]


initial_estimate_covariance: [1, 0,    0,    0,    0,    0, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1, 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,    1, 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]

Marker1 topic is of type nav_msgs/Odometry and contains the distance information from the marker1 tag to the map frame, as a result of the output_frame parameter in the ar_track_alvar launch file. In this way the filter is feed with absolute data, and not relative distance data of the markers. Below the ar_track_alvar launch file:

<?xml version="1.0"?>
<launch>
    <arg name="marker_size" default="5" />
    <arg name="max_new_marker_error" default="0.02" /> <!-- original 0.08-->
    <arg name="max_track_error" default="0.2" /> <!-- original 0.2-->

    <arg name="cam_image_topic" default="/camera/depth/points" />
    <arg name="cam_info_topic" default="/camera/rgb/camera_info" />
    <arg name="output_frame" default="/map" />
    <arg name="marker_resolution" default="5" />

    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen">
        <param name="marker_size"           type="double" value="$(arg marker_size)" />
        <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
        <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
        <param name="output_frame"          type="string" value="$(arg output_frame)" />

        <remap from="camera_image"  to="$(arg cam_image_topic)" />
        <remap from="camera_info"   to="$(arg cam_info_topic)" />
    </node>

</launch>

But as soon as a marker is detected, the /map frame gets lost, and starts rotating around the /odom frame. Below the tf tree and rqt_graph output:

image description

image description

We might have misunderstood how the ekf_localization_node is designed to work with absolute positioning, and for this reason we kindly ask someone, hopefully the package designers, to light up our minds on what we are missing/misunderstanding. For the specific case, the question would be: How to make the ekf know my markers' absolute position? How to relate marker's position to the map frame, which is our global reference? Broadcasting a tf is not allowed, as the tf tree would be complaining on marker having two parents (map and camera_link).

Thanks for anyone helping us out,

Regards

Global localization using robot_localization node and AR tagstag

Hi,

We are working on a project and we have big issues using robot_localization to work as a global localizer. We are using ROS Kinetic, Gazebo 7.0, a Kinect camera and ekf_localization_node as filter node. The final goal is get out from the filter absolute robot positions by fusing odometry sensor data and AR codes distance information coming from ar_track_alvar.

No matter how we feed the filter with marker's data, the map frame just heavily jumps, and as consequence the final robot's pose is erroneous. When no tags are detected, so only feeding Ekf with the odometry, the map frame stays in place, as seen here:

The ekf.param file is seen below:

frequency: 40
sensor_timeout: 1
two_d_mode: true
#transform_time_offset: 0.0
#print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: map          # Defaults to the value of odom_frame if unspecified

dynamic_process_noise_covariance: true

odom0: od
odom0_config: [true,  true,  false,
               false, false, true,
               false, false, false,
              false, false, false,
               false, false, false]
odom0_differential: true
odom0_relative: false
odom0_queue_size: 40
odom0_nodelay: false

odom1: marker1
odom1_config: [true,  true,  false,
              false, false, true,
               false, false, false,
              false, false, false,
               false, false, false]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 40
odom1_nodelay: false

process_noise_covariance: [0.0001, 0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.0001, 0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.0001, 0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.0001, 0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.0001, 0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.0001, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0.0001, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0.0001, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0.0001, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0.0001, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0.0001, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0.0001, 0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0.0001, 0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0.0001, 0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0.0001]


initial_estimate_covariance: [1, 0,    0,    0,    0,    0, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1, 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,    1, 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]

Marker1 topic is of type nav_msgs/Odometry and contains the distance information from the marker1 tag to the map frame, as a result of the output_frame parameter in the ar_track_alvar launch file. In this way the filter is feed with absolute data, and not relative distance data of the markers. Below the ar_track_alvar launch file:

<?xml version="1.0"?>
<launch>
    <arg name="marker_size" default="5" />
    <arg name="max_new_marker_error" default="0.02" /> <!-- original 0.08-->
    <arg name="max_track_error" default="0.2" /> <!-- original 0.2-->

    <arg name="cam_image_topic" default="/camera/depth/points" />
    <arg name="cam_info_topic" default="/camera/rgb/camera_info" />
    <arg name="output_frame" default="/map" />
    <arg name="marker_resolution" default="5" />

    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen">
        <param name="marker_size"           type="double" value="$(arg marker_size)" />
        <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
        <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
        <param name="output_frame"          type="string" value="$(arg output_frame)" />

        <remap from="camera_image"  to="$(arg cam_image_topic)" />
        <remap from="camera_info"   to="$(arg cam_info_topic)" />
    </node>

</launch>

But as soon as a marker is detected, the /map frame gets lost, and starts rotating around the /odom frame. Below the tf tree and rqt_graph output:

image description

image description

We might have misunderstood how the ekf_localization_node is designed to work with absolute positioning, and for this reason we kindly ask someone, hopefully the package designers, to light up our minds on what we are missing/misunderstanding. For the specific case, the question would be: How to make the ekf know my markers' absolute position? How to relate marker's position to the map frame, which is our global reference? Broadcasting a tf is not allowed, as the tf tree would be complaining on marker having two parents (map and camera_link).

Thanks for anyone helping us out,

Regards

Global localization using How to make robot_localization node and to work with AR tagtags

Hi,

We are working on a project and we have big issues using robot_localization to work as a global localizer. We are using ROS Kinetic, Gazebo 7.0, a Kinect camera and ekf_localization_node as filter node. The final goal is get out from the filter absolute robot positions by fusing odometry sensor data and AR codes distance information coming from ar_track_alvar.

No matter how we feed the filter with marker's data, the map frame just heavily jumps, and as consequence the final robot's pose is erroneous. When no tags are detected, so only feeding Ekf with the odometry, the map frame stays in place, as seen here:

The ekf.param file is seen below:

frequency: 40
sensor_timeout: 1
two_d_mode: true
#transform_time_offset: 0.0
#print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: map          # Defaults to the value of odom_frame if unspecified

dynamic_process_noise_covariance: true

odom0: od
odom0_config: [true,  true,  false,
               false, false, true,
               false, false, false,
              false, false, false,
               false, false, false]
odom0_differential: true
odom0_relative: false
odom0_queue_size: 40
odom0_nodelay: false

odom1: marker1
odom1_config: [true,  true,  false,
              false, false, true,
               false, false, false,
              false, false, false,
               false, false, false]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 40
odom1_nodelay: false

process_noise_covariance: [0.0001, 0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.0001, 0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.0001, 0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.0001, 0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.0001, 0, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.0001, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0.0001, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0.0001, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0.0001, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0.0001, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0.0001, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0.0001, 0,    0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0.0001, 0,    0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0.0001, 0,
                           0,    0,    0,    0,    0,    0, 0,     0,     0,    0,    0,    0,    0,    0,    0.0001]


initial_estimate_covariance: [1, 0,    0,    0,    0,    0, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1, 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,    1, 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]

Marker1 topic is of type nav_msgs/Odometry and contains the distance information from the marker1 tag to the map frame, as a result of the output_frame parameter in the ar_track_alvar launch file. In this way the filter is feed with absolute data, and not relative distance data of the markers. Below the ar_track_alvar launch file:

<?xml version="1.0"?>
<launch>
    <arg name="marker_size" default="5" />
    <arg name="max_new_marker_error" default="0.02" /> <!-- original 0.08-->
    <arg name="max_track_error" default="0.2" /> <!-- original 0.2-->

    <arg name="cam_image_topic" default="/camera/depth/points" />
    <arg name="cam_info_topic" default="/camera/rgb/camera_info" />
    <arg name="output_frame" default="/map" />
    <arg name="marker_resolution" default="5" />

    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen">
        <param name="marker_size"           type="double" value="$(arg marker_size)" />
        <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
        <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
        <param name="output_frame"          type="string" value="$(arg output_frame)" />

        <remap from="camera_image"  to="$(arg cam_image_topic)" />
        <remap from="camera_info"   to="$(arg cam_info_topic)" />
    </node>

</launch>

But as soon as a marker is detected, the /map frame gets lost, and starts rotating around the /odom frame. Below the tf tree and rqt_graph output:

image description

image description

We might have misunderstood how the ekf_localization_node is designed to work with absolute positioning, and for this reason we kindly ask someone, hopefully the package designers, to light up our minds on what we are missing/misunderstanding. For the specific case, the question would be: How to make the ekf know my markers' absolute position? How to relate marker's position to the map frame, which is our global reference? Broadcasting a tf is not allowed, as the tf tree would be complaining on marker having two parents (map and camera_link).

Thanks for anyone helping us out,

Regards