0
$\begingroup$

Rosanswers logo

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


Originally posted by simff on ROS Answers with karma: 98 on 2018-03-01

Post score: 0


Original comments

Comment by stevejp on 2018-03-05:
Can you post an example of the output from your marker1 topic? It sounds like that is where the problem is.

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

Please post a sample input message from every input.

You are trying to fuse a sensor-relative pose as an absolute pose. I'm not entirely familiar with ar_track_alvar, but in order to fuse a pose from a fiducial marker, you need to know the world-frame pose of that fiducial marker. For example, suppose you start your robot, and it can't see any markers. You drive around for a while, and you get to some pose, e.g., (10, 4) and with a yaw of 0.29 (I'm assuming a 2D use case). Now you see a fiducial marker. Your tracking package, presumably, produces a pose estimate relative to the camera, but that's not grounded in anything. In order for that to update your robot's pose, you need it to produce a world-frame pose estimate. In this case, there would have to be some world-frame pose associated with every single fiducial. Then, when your robot sees it at some relative pose, you'd apply that relative pose (inverse) to the fiducial world-frame pose, and you'd have the pose of your camera in the world frame. You'd have to apply the camera->base_link transform to get the pose of your robot.


Originally posted by Tom Moore with karma: 13689 on 2018-03-08

This answer was NOT ACCEPTED on the original site

Post score: 2

$\endgroup$
0
$\begingroup$

Rosanswers logo

Solved

Thanks for the reply Mr. Moore, the issue was indeed a missing reference of the fiducial markers to our world position. Thanks to your suggestion now it works! We simply add static transforms marker --> world_marker and then lookup the tf between world_marker and base_link. Finally we gave to the Ekf as input the corresponding message for that transform, by using as a message frame_id the map and that was it. The final output is now the base_link position seen from the map frame.

Cheers


Originally posted by simff with karma: 98 on 2018-03-12

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Tom Moore on 2018-03-19:
Glad it worked out.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.