Hesai QT128 with Zed2i and external fused Odometry

[aninath93]

Hi Mathieu,

Had a issue when adding obstacles to map after mapping and starting in localization mode where Nav2 subscribes in transient locally. Can you please advise if I am missing step here ?

Created Map --> rtabmap-databaseViewer --> File --> Edit optimized 2D map -->  add obstacles --> No for the question about cropping --> Close databaseViewer and relaunch in localization mode in ROS side, relaunch rviz as well, but the map is original one minus obstacles.

[matlabbe]

Are you using rtabmap version with this commit https://github.com/introlab/rtabmap/commit/2a840fe340a31a62a9c4860bb22035f97e6a3f36 ?

Related issue: https://github.com/introlab/rtabmap_ros/issues/1173

[aninath93]

Hi Mathieu,

Re-basing to master branch fixed that issue, thank you again.

[aninath93]

Hi Mathieu,

The re-basing worked to load the map but as soon as the Nav2 gets loaded twice, the obstacles appear again which were cleared.

When obstacles was cleared from map which is looks like salt pepper noise in bottom of the map.


Loading Nav2 again, this obstacle appears,


Also I have noticed when I relaunch databaseviewer the obstacle which was cleared still appears in database map vs the not in the exported optimized map ? Can this be issue ?

These are my launch files for Nav2, map QoS is transient local.
nav2_bringup.py
nav2_params.yaml

[matlabbe]

I cannot reproduce your issue, I restarted nav2 stack many times while keeping rtabmap node running and the map was showing always my custom changes (guess what they are :) ):


I see that you may have a map_server node started by nav2 bringup. You may doublecheck that there are no other nodes publishing also a map.

In databaseViewer, the Graph View won't show the edited map. You can only see the edited map under File->"Edit optimized 2D map..."

cheers,
Mathieu

[aninath93]

Hi Mathieu,

As always, thank you . It worked correctly after clearing the workspace and re-building the workspace to your above mentioned commit vs previously I had just built on top of the existing build/install folder.

[aninath93]

Hi Mathieu,

During Localization, I would like to know how can I get landmark id which is detected by rtabmap so I can make sure that robot is good for further navigation and localized well w.r.t to tags as well due to complex environment, I am subscribed to localization_pose but would also like to confirm that tag id is detected at the very beginning (I can do this with Apriltag node but would like confirmation of rtabmap to make sure Nav2 can take over at this time reliably ). I tried echo with landmark_detection but did not work and landmarks just give pose data.

[matlabbe]

Not sure if it is exactly what you need, but there is a "landmarkId" field in rtabmap_msgs/Info, it should not be 0 when a landmark is observed again in the map (so when it relocalizes on the tag).

Note that "landmarkId" may still be set even if localization is rejected when it is the first localization for a while. If it is a problem, I may check deeper how to get robustly that info.

cheers,
Mathieu

[aninath93]

Hi Mathieu,

This works, thank you. The main intention is to know if Rtabmap recognized the tag considering Filters used with Marker/ parameters and the technician while deploying the Robot for first time did what he was supposed to for the start point of the robot.

[aninath93]

Good Morning Mathieu,

Thank you for continued interaction, I appreciate you. I have finally created all the Nav2 BT, customized for the operation needed and works well, and so for Rtabmap parameters with timing to best of my knowledge in this environment.
I wanted to get your final thoughts if you have a chance to peek at my database sometime in next couple weeks with the launch file params, if you were to do something different. I am quite satisfied but so often see enough drift if I do not see Apriltag, I wanted to seek your guidance in this area if anything could be changed for Icp side or for optimization side as the environment changes in terms of human walking and carts but not static machines.

Database and launch file

[matlabbe]

Hi,

I tested the database, here are my observations.

* For the tag detection, it seems with "Marker/VarianceAngular: 0.1" and "Marker/VarianceLinear: 0.001" we get less rejected landmark detections.

* For ICP, the max correspondence distance could be increased: "Icp/MaxCorrespondenceDistance: 1". Note that as a rule of thumb, I use often 10x the voxel size for that value. You have voxel_size=0.2, you may increase it even to 2 meters.

* When using icp_odometry, you don't need to refine the links, so you can set "RGBD/NeighborLinkRefining: false"

* The biggest issue is that the lidar extrinsics look wrong. At least for the lidar, we can clearly see it is tilted (actually a mix of tilt and roll). When restraining "Reg/Force3DoF: true", make sure the lidar is correctly aligned with the base/floor.



cheers,
Mathieu

[aninath93]

Good Morning,

Thank you Mathieu, I truly appreciate you and your time. Yes I am aware of that LIDAR bracket as its 3D printed and has a slight degree to it. Trying to get more reps in this environment and having people like this application over magtape last gen :) before going all production parts.
I posted issue on GITHUB as some where in my posts before I had mentioned, cleared dynamic obstacles after map creation come back which I found issue to be when we see tag second time and the map gets transformed and I had isolated Nav2 in this to make sure. Please if you can take a look at it, while I am trying to find in the code as well for Optimized/Prior side, somewhere along the line in releases something changed.

[aninath93]

Hi Mathieu,

Having one issue as I have turned off trigger to memory management, I see icp odometry delay overnight keeps exponentially growing causing the topics to not sync anymore, only way I see to solve is restart rtabmap node. Any thoughts on this please as topic size or other parameter don't make sense to and turning on memory management with Marker/Priors causes issue as I had explained in GITHUB and here ? I do not see any crash on kernel side for LIDAR or CAMERA in dmesg so I dont know how this could be resolved .

overnight log

[matlabbe]

Hi,

I think the lidar clock is drifting away from the computer clock over time. You can see it with the delay increasing over time while the update time stays the same:

[icp_odometry-2] [INFO] [1733347294.724916742] [amr_1.icp_odometry]: Odom: ratio=0.951245, std dev=0.001518m|0.000480rad, update time=0.045333s delay=0.154470s
[icp_odometry-2] [INFO] [1733347294.772856454] [amr_1.icp_odometry]: Odom: ratio=0.943212, std dev=0.001634m|0.000517rad, update time=0.046218s delay=0.152421s
[icp_odometry-2] [INFO] [1733347294.827291443] [amr_1.icp_odometry]: Odom: ratio=0.945505, std dev=0.001682m|0.000532rad, update time=0.049086s delay=0.156877s
[icp_odometry-2] [INFO] [1733347294.940438038] [amr_1.icp_odometry]: Odom: ratio=0.949428, std dev=0.001507m|0.000476rad, update time=0.060262s delay=0.170088s

...

[icp_odometry-2] [INFO] [1733405553.012221024] [amr_1.icp_odometry]: Odom: ratio=0.890234, std dev=0.002034m|0.000643rad, update time=0.053131s delay=0.583968s
[icp_odometry-2] [INFO] [1733405553.100193298] [amr_1.icp_odometry]: Odom: ratio=0.888394, std dev=0.002140m|0.000677rad, update time=0.042064s delay=0.571873s
[icp_odometry-2] [INFO] [1733405553.150810442] [amr_1.icp_odometry]: Odom: ratio=0.892912, std dev=0.002029m|0.000642rad, update time=0.044229s delay=0.572460s

until rtabmap cannot synchronize the camera and lidar anymore:

[rtabmap-3] [WARN] [1733391550.830491314] [amr_1.rtabmap]: rtabmap: Did not receive data since 5 seconds! Make sure
 the input topics are published ("$ ros2 topic hz my_topic") and the timestamps in their header are set. If topics are coming
 from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). Ajusting topic_queue_size
 (15) and sync_queue_size (5) can also help for better synchronization if framerates and/or delays are different. If topics are
 not published at the same rate, you could increase "sync_queue_size" and/or "topic_queue_size" parameters (current=5
 and 15 respectively).
[rtabmap-3] rtabmap subscribed to (approx sync):
[rtabmap-3]    /amr_1/rgbd_image \
[rtabmap-3]    /amr_1/odom_filtered_input_scan

What kind of lidar are you using? there is maybe a way to configure it in a mode to sync the clock over time (e.g., PTP sync) with the host computer / network interface.

[aninath93]

Good Morning,

Thank you for the reply, yes you are right and I overlooked it thinking being sync issue, its Hesai XT32, I am running hardware sync using linuxptp, but I guess I can revert back to my previous config as I had tweaked the clock servo parameters based on pmc feedback here for more accuracy and tight control but looks like previous config was better.

[matlabbe]

I assume that the camera is synced with computer clock, but yeah, ideally everything is sync on same clock.

[aninath93]

Good Morning,

Yes that is right, so far its been 1.5 days of runtime and have not seen jump in clock of LIDAR or drift in delay significantly from 110 ms to 150 ms. I have Hesai XT32, changing PTP config of clock class to 128 for high priority worked for me with hardware stamping.

Thank you for pointing that out.