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.

[aninath93]

Hi Mathieu,

Thank you for your continued support, I need your assistance on one last topic please, using internal tags what is happening is that over a day or two somewhere into running --> map is getting re-optimized, meaning if we had cleaned the map or added obstacles they disappear to originally generated grid map which causes issue upstream in Nav2 path planning as all of sudden the path gets invalid.
I do not know if this is related to either memory management trigger which could re-optimize the map or Rtabmap/StartNewMapOnLoopClosure, but made sure mapping while localizing which was and is false and not calling any set_mapping_mode service as well, tried setting Rtabmap/StartNewMapOnLoopClosure to false as well but still the same.
Tried restarting it multiple times, the map stays the same meaning all the cleaned up and added obstacle changes stay to the desired but somewhere after a day or two its not the case. I disabled the memory management and looking to see if the issue comes back, but not sure yet. Can you please shed some light on this or advise where in code to add maybe debug statement or I could look deeper to see as this happens randomly over a day or so somewhere, I have made sure that there is no other node publishing to the map topic as well.
I have attached some of my launch files, nav2 is unnecessary here but still did
Launch and database files

[aninath93]

Hi Mathieu,

Just an update, this has nothing to do with memory trigger as I left the nodes running overnight with,
'Rtabmap/TimeThr' : '0',
'Mem/RehearsalSimilarity':'0',

and sometime this morning the map defaulted to stock again. Kindly please see my setup, as I have tried to ask this issue over and over but you were not able to replicate as this does not happen by starting or stopping Rtabmap, its happening somewhere along the line either after finding global loop closure with previous map and starts a new map even though its not supposed to ?



Then after until we re-import the exported map again, the map does not change.




The map again got reset, and this time if we try to import the map it says size does not matter, looks like somehow new nodes are getting appended somewhere causing the map to grow ?

Current map


Stock map which was altered yesterday,



I just ran one loop this time and the map already switched, created this 2 log files.
https://easyupload.io/lr5r9e

https://easyupload.io/6fspe0

Its after loop closure which is where the poses are getting added and optimized, OccupancyGrid is getting reassembled with stock map in cache and ignoring the current map in memory.

We see the tag --> loop closure --> poses get optimized --> occupancy grid gets reassembled to the saved map and not to the edited map.

[aninath93]

CONTENTS DELETED

The author has deleted this message.