0
$\begingroup$

Rosanswers logo

Hi,

I'm using ROS Kinetic and a Clearpath Husky robot. I have an already running ekf_localization_node on my robot that gives me the base_link -> odom and outputs an odometry/filtered topic. Now, on the top of it, I want to use robot_localization to fuse global absolute data with markers.

I want to use the odometry data for my second ekf_global, hence I want the odometry/filtered topic to be subscribed by this node. Here is my launch file:

<?xml version="1.0"?>
<launch> 
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
<rosparam command="load" file="$(find robot_localization)/params/my_ekf.yaml" />
<remap from="odometry/filtered" to="finalpose"/>
</launch>

The param file my_ekf.yaml is below:

frequency: 30

sensor_timeout: 1

two_d_mode: 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: false
odom0_relative: false
odom0_queue_size: 40

odom0_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: [1e-2, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1e-2, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    1e-2, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    1e-2, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    1e-2, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    1e-2, 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]

If I look at the result of rostopic info /finalpose, I see that the ekf is subscribing to itself, but actually I am expecting to have /odometry/filtered as a subscriber. The below picture highlights this:

image description

The odometry/filtered is not empty, two_d_mode is set to true. Why does this not work? According to the wiki it should be possible to use odometry as data to be fused.

Edit

I just noticed that the remap from="odometry/filtered" caused the problem. I did remap the odometry/filtered from ekf_localization (local ekf) to a topic called /od (see Figure below) and give /od as input to ekf_global. By removing the remap tag from the .launch file I got this result,

bb

Now the layout is correct, the /od topic in not empty, and I get from the ekf_global an output (by default is called /odometry/filtered, not to be confused with the one I remapped in the ekf_localization).

But when I try to echo /odometry/filtered it says: WARNING: no messages received and simulated time is active. Is /clock being published?

Why is it empty? How can I fix this? I am expecting to see here a similar output as the pure odometry of my sensors.

I don't get it, please help me out!

Simone.


Edit2 Hi @nikfio thanks for the reply. No I am not using a wifi connection, nor a laptop hardware. I am doing all using Gazebo and simulating my Husky within it.

The output of the tf is simply odom-->base_link, but I am actually expecting to see map-->odom-->base_link, as soon as at least one input is given to my global Ekf. My final goal is to fuse AR code markers with the odometry, hence markers will correct odometry drifts at the end.

SOLVED

I finally got map-->odom-->base_link by fusing the odometry output of my local ekf_localization node. Instead of fusing absolute poses, I fused linear velocities and the yaw velocity. The I set the _relative sensor property to true, and accordingly modified the initial_state_estimate covariances to be large (value = 1) for my corresponding entries.

Above the modified .yaml file

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

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,    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,    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,     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]

Originally posted by simff on ROS Answers with karma: 98 on 2018-02-17

Post score: 1


Original comments

Comment by gvdhoorn on 2018-02-18:
@simff: please review the support guidelines. Do not post screenshots/images of text files or consoles. Copy-paste it into your question.

Comment by simff on 2018-02-18:
Yes sorry, using Windows system I had some issues with that, I could not use Linux not being at my work place. I will correct it tomorrow.

Comment by DesertXGhost on 2020-04-28:
Is it possible to print the output data of /odometry/filtered on console cause I am trying to plot it combined with a noisy data?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Standard practice would be to fuse your wheel odometry + IMU + absolute pose information in the second EKF instead of feeding the filtered odometry from the first EKF into the second EKF.


Originally posted by stevejp with karma: 929 on 2018-02-23

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by simff on 2018-02-23:
Hi, good point, but for my case I want to have distinct map and odom frames :)

$\endgroup$

Your Answer

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