How to use fiducial_slam with robot_localisation

asked 2020-05-14 11:48:27 -0500

Reghard_Grobler gravatar image

Good day

I am trying to use the EKF node of robot_localization package with the fiducial_slam node for a continuous pose estimate for a uav. I want to fuse the pose topic published by fiducial_slam with the local_position topic published by the flight controller.

The fiducial_slam package appears to be working as expected, the pose of the uav is correctly published by this node. However this data is only published while a marker is visible by the onboard camera. I want to therefore fuse this non continuous pose estimate with the continuous pose published by the flight controller. The pose published by the flight controller will be inaccurate as it mainly relies on GPS but this is okay as long as the filter corrects this inaccuracy every now and then when a marker is visible. This is the man idea I am trying to accomplish...

The problem im having is that the output of the EKF filter appears to be correct when a marker is visible. But then the uav flies to a location where no markers are visable the EKF should fully rely on just the pose topic published by the flight controller. But in this scenario the output of the filter does not resemble the input pose topic of the flight controller. Instead the output seems to freeze on a fixed value with very very small variation.

I will leave my specific questions to the end of the post.

Here is my config file for robot_localization:

frequency: 50
publish_tf: false

odom_frame: local_origin
base_link_frame: fcu
world_frame: map
map_frame: map

#  x, y, z,
#  roll, pitch, yaw,
#  vx, vy, vz,
#  vroll, vpitch, vyaw,
#  ax, ay, az

pose0: /fiducial_pose
pose0_config: [true,true,true,
           true,true,true,
           false,false,false,
           false,false,false,
           false,false,false,]

pose1: /mavros/local_position/pose
pose1_config: [true,true,true,
           true,true,true,
           false,false,false,
           false,false,false,
           false,false,false,]
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.03, 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,    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 ...
(more)
edit retag flag offensive close merge delete

Comments

Please include a sample message from every sensor input.

Tom Moore gravatar image Tom Moore  ( 2020-06-29 04:38:24 -0500 )edit

Hi,

I'm using fiducial_slam with a mobile robot in a setup where the markers are also not always visible. It takes a bit of trying around and having a closer look at this manual: http://docs.ros.org/melodic/api/robot...

In your config file, I think the publish_tf: false might be the reason, because you want the ekf node to publish the filtered odom TF rather than the fiducial_slam directly. Attached a cutout from my config file. You also need a second ekf node for odom as described in this tutorial: http://docs.ros.org/melodic/api/robot...

frequency: 30

map_frame: map
odom_frame: odom
base_link_frame: base_footprint
world_frame: map
publish_tf: true

pose0: /fiducial_pose
pose0_config: [true,  true,  true,
               true, true, true,
               false, false, false,
               false, false, false,
               false, false, false]

pose0_differential: false
ivan140 gravatar image ivan140  ( 2020-07-31 10:57:35 -0500 )edit