Hello.

I'm on ROS Noetic on 20.04 LTS.

I'll begin listing my problems and how I think about the solution and how it doesn't work.

I've been trying to make full 3D navigation system, for a mobile robot that's in a big open area with tags in certain locations. I used rtabmap package for mapping and it takes the odom frame published by robot_localization and publishes the map->odom transform (according to the tf tree) robot_localization is used to fuse some of the sensors (wheel odom velocities, IMU orientations, and a bit of vo velocities as well) It's been working fine. here's my ekf_3d.yaml config file.

frequency: 30
sensor_timeout: 0.2
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false

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

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

odom0: /wheel_odom_with_covariance
odom0_config: [false, false, false,
                false, false, false,
                true,  false, false,
                false, false, true,
                false, false, false]
odom0_differential: false
odom0_queue_size: 3
#odom0_rejection_threshold: 15
odom0_nodelay: false

odom1: /zed2/rgbd_odom
odom1_config: [false, false, false,
                false, false, false,
                false,  true, true,
                false, false, false,
                false, false, false]
odom1_differential: false
odom1_relative: false
odom1_queue_size: 3
#odom1_rejection_threshold: 15
odom1_nodelay: false

pose0: /pose_corrector
pose0_config: [true, true, false,
                false, false, false,
                false,  false, false,
                false, false, false,
                false, false, false]
pose0_differential: false
pose0_queue_size: 3
pose0_nodelay: false

imu0: imu/data
imu0_config: [false, false, false,
              true,  true,  false,
              false, false, false,
              true,  true,  true,
              true,  false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_remove_gravitational_acceleration: true

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.5,   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.4,  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]

But ultimately there was drift from ground_truth and the estimated pose. I have a set of ARTags in the map. It's actually a cuboid like pole with four sides each side has the same tag id. And that caused me headache. I tried publishing static tfs of these tags, there is total of 14 tags, so 14*4. I only worked with one four set for pole of id_1 for example and disregarded other detections. Wrote two nodes for two different experiments.

• One node tried to offset the odom frame based on estimated robot pose in map frame. I see tag_id 's pose in camera_frame using the ar_track_alvar, get it in base_frame, invert that pose, and use pose transform to transform the robot's pose in the tag frame to robot pose in map frame, using that static tf (real transform of tag pose from map) of that "estimated face of the four faces of that ID" -takes breath-

And then do more transforms to offset the odom frame from map frame. (Well that method didn't interface with robot_localization did it), the robot kept jumping heavily back and forth on detecting ARTags in the map. I began debugging heavily and saw that the method I used to identify which of the four faces I'm looking at is not working properly also I thought that the one who's publishing the map->odom transform was rtabmap and maybe they both were trying to publish on same transform and that caused the mess. To try to keep it shorter I ditched this method and tried instead using the translations only to estimate the XYZ only of the robot instead of the full pose XYZ-XYZW.

• Knowing the pole's XYZ in the map frame, I publish static frame (I know I can store each tag position somehow somewhere but I kept the ar_tag_poses yaml file and publish the centers of the poles, I detect the artag in the `cameraframetransform it to thebaseframeand invert that pose to getbaseframeintagidframeand push the the robot's pose in Z to offset that detection from the pole side to it's center roughly (because that's how the detected frame is oriented) and get that offset-ed tag pose (that should have roughly the XYZ of the pole center) inmapframecompared it with the real XYZ of that pole, then I know how much my rover needs to translate from it's previous position. I published the real pose on aPoseWithCovarianceStampedtopic/posecorrectorand have the EKF listen to it (As seen above), however when listening to this topic the pose is still offset-ed a bit from the ground truth and the rover just jumps suddenly back and forth or when the publishing of the pose stops (as for robot is not facing a tag anymore) the rover keeps moving/sliding in RViz in theodom` frame while it's apparently static in gazebo.

It's been very frustrating so far I spent several days writing the code. and reading the wikis and answers here I think the pose fusion question has been answered and that's how I wrote my code at first seemed simple but it's very tedious.

What I read is that I have to use two instances of ekf and that I've never used or tried and thought it's only with GPS configurations.

Also I don't know how to stop rtabmap from publishing map->odom

I already need rtabmap cause I'm more familiar with it and use it for global obstacle detection.

I'm really confused with the plethora of suggestions. I just want to estimate the real rover position using the ar tags' known poses and detected poses. And then used this estimate to correct my odom->base_link in the odom frame preferably leave my tfs (odom and map) on top of one another.

Any idea how to do that?

Thanks in advance