3D Lidar SLAM not receiving data

classic Classic list List threaded Threaded
9 messages Options
Reply | Threaded
Open this post in threaded view
|

3D Lidar SLAM not receiving data

Masterman
Hello, I have been trying to integrate RTAB-Map into a simulation and perform SLAM with 3D Lidar and odometry, the simulation I am trying to implement it into is AirSim (https://github.com/microsoft/AirSim) with the AirSim ROS wrapper (https://microsoft.github.io/AirSim/airsim_ros_pkgs/).

When i run my launch file with the simulation + ROS wrapper I get the message

[ WARN] [1618571562.954980281]: /rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic 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"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=14).
/rtabmap subscribed to (approx sync):
   /airsim_node/drone_1/odom_local_enu \
   /airsim_node/drone_1/lidar/LidarCustom


Here is a screenshot of the rostopic hz for both

ODOM


LIDAR


After doing some research into the issue I believe the problem lies with my TF tree, I have tried to replicate the turtlebot example TF tree but I still get no data into RTAB,

Here is a screenshot of my TF tree


Here is a screenshot of my rqt_graph


Here is my launch file

<launch>
    <!-- Choose visualization -->
    <arg name="rviz" default="false" />
    <arg name="rtabmapviz" default="true" />
		
	<!-- TF -->
		<node pkg="tf" type="static_transform_publisher" name="rtabmap_map" args="0 0 0 1.57 0 3.14 map world_ned 100"/>



<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
 
  <param name="subscribe_rgbd"       type="bool" value="false"/>
  <param name="frame_id"             type="string" value="world_enu"/>
  <param name="subscribe_depth"      type="bool" value="false"/>
  <param name="subscribe_rgb"        type="bool" value="false"/>
  <param name="subscribe_scan_cloud" type="bool" value="true"/>
  <param name="approx_sync"          type="bool" value="true"/>
  <param name="visual_odometry"      type="bool" value="false"/>
  <param name="queue_size"           type="int"  value="14"/>
  <remap from="scan_cloud" to="/airsim_node/drone_1/lidar/LidarCustom"/>
  <remap from="odom" to="/airsim_node/drone_1/odom_local_enu"/>

  <remap from="grid_map" to="/map"/>

  <!-- RTAB-Map's parameters -->
  <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
  <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
  <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
  <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
  <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
  <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
  <param name="Mem/STMSize"                    type="string" value="30"/>

  <param name="Reg/Strategy"                   type="string" value="1"/>
  <param name="Grid/CellSize"                  type="string" value="0.2"/>
  <param name="Grid/RangeMax"                  type="string" value="20"/>
  <param name="Grid/ClusterRadius"             type="string" value="1"/>
  <param name="Grid/GroundIsObstacle"          type="string" value="false"/>
  <param name="Grid/RayTracing"                type="string" value="true"/>
  <param name="Grid/3D"                        type="string" value="false"/>
  <param name="Grid/MaxObstacleHeight"         type="string" value="1.5"/>

	
	
</node>
	
	        <!-- Visualisation RTAB-Map -->
  <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
  <param name="subscribe_rgbd"       type="bool" value="false"/>
  <param name="frame_id"             type="string" value="world_enu"/>
  <param name="subscribe_depth"      type="bool" value="false"/>
  <param name="subscribe_rgb"        type="bool" value="false"/>
  <param name="subscribe_scan_cloud" type="bool" value="true"/>
  <param name="approx_sync"          type="bool" value="true"/>
  <param name="visual_odometry"      type="bool" value="false"/>

  <remap from="scan_cloud" to="/airsim_node/drone_1/lidar/LidarCustom"/>
  <remap from="odom" to="/airsim_node/drone_1/odom_local_enu"/>
        </node>
	</launch>

And the error


leon@leon-All-Series:~/AirSim/ros$ roslaunch '/home/leon/AirSim/ros/src/SLAM/run.launch'
... logging to /home/leon/.ros/log/989b28f6-9ea3-11eb-9a46-54a050509395/roslaunch-leon-All-Series-28287.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://leon-All-Series:43563/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.10
 * /rtabmap/Grid/3D: false
 * /rtabmap/Grid/CellSize: 0.2
 * /rtabmap/Grid/ClusterRadius: 1
 * /rtabmap/Grid/GroundIsObstacle: false
 * /rtabmap/Grid/MaxObstacleHeight: 1.5
 * /rtabmap/Grid/RangeMax: 20
 * /rtabmap/Grid/RayTracing: true
 * /rtabmap/Mem/NotLinkedNodesKept: false
 * /rtabmap/Mem/STMSize: 30
 * /rtabmap/RGBD/AngularUpdate: 0.05
 * /rtabmap/RGBD/NeighborLinkRefining: false
 * /rtabmap/RGBD/ProximityMaxGraphDepth: 0
 * /rtabmap/RGBD/ProximityPathMaxNeighbors: 1
 * /rtabmap/Reg/Strategy: 1
 * /rtabmap/Rtabmap/DetectionRate: 1
 * /rtabmap/approx_sync: True
 * /rtabmap/frame_id: world_enu
 * /rtabmap/queue_size: 14
 * /rtabmap/subscribe_depth: False
 * /rtabmap/subscribe_rgb: False
 * /rtabmap/subscribe_rgbd: False
 * /rtabmap/subscribe_scan_cloud: True
 * /rtabmap/visual_odometry: False
 * /rtabmapviz/approx_sync: True
 * /rtabmapviz/frame_id: world_enu
 * /rtabmapviz/subscribe_depth: False
 * /rtabmapviz/subscribe_rgb: False
 * /rtabmapviz/subscribe_rgbd: False
 * /rtabmapviz/subscribe_scan_cloud: True
 * /rtabmapviz/visual_odometry: False

NODES
  /
    rtabmap (rtabmap_ros/rtabmap)
    rtabmapviz (rtabmap_ros/rtabmapviz)

ROS_MASTER_URI=http://localhost:11311

process[rtabmap-1]: started with pid [28302]
process[rtabmapviz-2]: started with pid [28303]
[ INFO] [1618571109.229979274]: Starting node...
[ INFO] [1618571109.267226458]: Initializing nodelet with 4 worker threads.
[ INFO] [1618571109.364050049]: Starting node...
[ INFO] [1618571109.409057322]: /rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1618571109.409281941]: /rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1618571109.409387261]: /rtabmap(maps): map_cleanup                = true
[ INFO] [1618571109.409468369]: /rtabmap(maps): map_always_update          = false
[ INFO] [1618571109.409544719]: /rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1618571109.409624308]: /rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1618571109.409697051]: /rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1618571109.409768398]: /rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1618571109.422593160]: rtabmap: frame_id      = world_enu
[ INFO] [1618571109.422738729]: rtabmap: map_frame_id  = map
[ INFO] [1618571109.422823800]: rtabmap: use_action_for_goal  = false
[ INFO] [1618571109.422903492]: rtabmap: tf_delay      = 0.050000
[ INFO] [1618571109.422978572]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1618571109.423051359]: rtabmap: odom_sensor_sync   = false
[ INFO] [1618571109.423373124]: rtabmap: gen_scan  = false
[ INFO] [1618571109.423466779]: rtabmap: gen_depth  = false
[ INFO] [1618571109.424000570]: rtabmap: scan_cloud_max_points = 0
[ INFO] [1618571109.479151692]: Setting RTAB-Map parameter "Grid/3D"="false"
[ INFO] [1618571109.479631861]: Setting RTAB-Map parameter "Grid/CellSize"="0.2"
[ INFO] [1618571109.479960944]: Setting RTAB-Map parameter "Grid/ClusterRadius"="1"
[ INFO] [1618571109.489064254]: Setting RTAB-Map parameter "Grid/GroundIsObstacle"="false"
[ INFO] [1618571109.492628900]: Setting RTAB-Map parameter "Grid/MaxObstacleHeight"="1.5"
[ INFO] [1618571109.499518017]: Setting RTAB-Map parameter "Grid/RangeMax"="20"
[ INFO] [1618571109.500710962]: Setting RTAB-Map parameter "Grid/RayTracing"="true"
[ INFO] [1618571109.603159512]: Setting RTAB-Map parameter "Mem/NotLinkedNodesKept"="false"
[ INFO] [1618571109.612059433]: Setting RTAB-Map parameter "Mem/STMSize"="30"
[ INFO] [1618571109.616745322]: rtabmapviz: Using configuration from "/home/leon/AirSim/ros/src/rtabmap_ros/launch/config/rgbd_gui.ini"
[ INFO] [1618571109.640546834]: Setting RTAB-Map parameter "RGBD/AngularUpdate"="0.05"
[ INFO] [1618571109.660972513]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="false"
[ INFO] [1618571109.672158799]: Setting RTAB-Map parameter "RGBD/ProximityMaxGraphDepth"="0"
[ INFO] [1618571109.675147469]: Setting RTAB-Map parameter "RGBD/ProximityPathMaxNeighbors"="1"
[ INFO] [1618571109.679853842]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[ INFO] [1618571109.682463102]: Setting RTAB-Map parameter "Rtabmap/DetectionRate"="1"
[ WARN] [1618571109.919671807]: Setting "Grid/FromDepth" parameter to false (default true) as "subscribe_scan", "subscribe_scan_cloud" or "gen_scan" is true. The occupancy grid map will be constructed from laser scans. To get occupancy grid map from cloud projection, set "Grid/FromDepth" to true. To suppress this warning, add <param name="Grid/FromDepth" type="string" value="false"/>
[ INFO] [1618571109.919836281]: Setting "Icp/PointToPlaneRadius" parameter to 0 (default 1.000000) as "subscribe_scan_cloud" is true.
[ INFO] [1618571109.921215212]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1618571109.921430793]: rtabmap: Deleted database "/home/leon/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1618571109.921468442]: rtabmap: Using database from "/home/leon/.ros/rtabmap.db" (0 MB).
[ INFO] [1618571109.996905922]: rtabmap: Database version = "0.20.7".
[ INFO] [1618571109.996983076]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[ INFO] [1618571110.031105429]: /rtabmap: subscribe_depth = false
[ INFO] [1618571110.031421694]: /rtabmap: subscribe_rgb = false
[ INFO] [1618571110.031615885]: /rtabmap: subscribe_stereo = false
[ INFO] [1618571110.031829681]: /rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[ INFO] [1618571110.031999506]: /rtabmap: subscribe_odom_info = false
[ INFO] [1618571110.032151385]: /rtabmap: subscribe_user_data = false
[ INFO] [1618571110.032301290]: /rtabmap: subscribe_scan = false
[ INFO] [1618571110.032791791]: /rtabmap: subscribe_scan_cloud = true
[ INFO] [1618571110.032946390]: /rtabmap: subscribe_scan_descriptor = false
[ INFO] [1618571110.033106103]: /rtabmap: queue_size    = 14
[ INFO] [1618571110.033256422]: /rtabmap: approx_sync   = true
[ INFO] [1618571110.033460376]: Setup scan callback
[ INFO] [1618571110.051046281]: 
/rtabmap subscribed to (approx sync):
   /airsim_node/drone_1/odom_local_enu \
   /airsim_node/drone_1/lidar/LidarCustom
[ WARN] [1618571110.051309654]: There is no image subscription, bag-of-words loop closure detection will be disabled...
[ WARN] [1618571110.051378692]: Setting Kp/MaxFeatures=-1 (bag-of-words disabled)
[ INFO] [1618571110.179749626]: rtabmap 0.20.10 started...
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
[ INFO] [1618571111.025257946]: rtabmapviz: Reading parameters from the ROS server...
[ INFO] [1618571111.217502550]: rtabmapviz: Parameters read = 341
[ INFO] [1618571111.217559181]: rtabmapviz: Parameters successfully read.
[ INFO] [1618571111.501804168]: /rtabmapviz: subscribe_depth = false
[ INFO] [1618571111.501883320]: /rtabmapviz: subscribe_rgb = false
[ INFO] [1618571111.501942419]: /rtabmapviz: subscribe_stereo = false
[ INFO] [1618571111.501969304]: /rtabmapviz: subscribe_rgbd = false (rgbd_cameras=1)
[ INFO] [1618571111.501999630]: /rtabmapviz: subscribe_odom_info = false
[ INFO] [1618571111.502022586]: /rtabmapviz: subscribe_user_data = false
[ INFO] [1618571111.502045240]: /rtabmapviz: subscribe_scan = false
[ INFO] [1618571111.502110360]: /rtabmapviz: subscribe_scan_cloud = true
[ INFO] [1618571111.502168473]: /rtabmapviz: subscribe_scan_descriptor = false
[ INFO] [1618571111.502183142]: /rtabmapviz: queue_size    = 10
[ INFO] [1618571111.502192132]: /rtabmapviz: approx_sync   = true
[ INFO] [1618571111.502206091]: Setup scan callback
[ INFO] [1618571111.508329738]: 
/rtabmapviz subscribed to (approx sync):
   /airsim_node/drone_1/odom_local_enu \
   /airsim_node/drone_1/lidar/LidarCustom
[ INFO] [1618571111.509273552]: rtabmapviz started.
[ WARN] [1618571115.051230546]: /rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic 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"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=14).
/rtabmap subscribed to (approx sync):
   /airsim_node/drone_1/odom_local_enu \
   /airsim_node/drone_1/lidar/LidarCustom
[ WARN] [1618571116.508534412]: /rtabmapviz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic 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"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmapviz subscribed to (approx sync):
   /airsim_node/drone_1/odom_local_enu \
   /airsim_node/drone_1/lidar/LidarCustom
[ WARN] [1618571120.051386916]: /rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic 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"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=14).

Additonal Information
The frame ID of my simulation world_enu
The base frame of my robot is drone_1
Lidar topic is/airsim_node/drone_1/lidar/LidarCustom
Odom topic is /airsim_node/drone_1/odom_local_enu


I am quite new to ROS so i apologise if this issue is due to something simple that I have missed, I would appreciate any help or information
Reply | Threaded
Open this post in threaded view
|

Re: 3D Lidar SLAM not receiving data

matlabbe
Administrator
Hi,

Can you record and share a small rosbag for testing?
rosbag record -a
Reply | Threaded
Open this post in threaded view
|

Re: 3D Lidar SLAM not receiving data

Masterman
Hello Dr. Labbé, Thankyou for taking the time to help me, I have recorded a 30 second bag of me flying around the environment with a lidar scanner attached below.

I saw your post here: https://answers.ros.org/question/366097/anyway-to-use-the-map-data-from-this-website-for-slam/
and managed to get scans published to RTAB-MAP although with ICP the odometry goes a bit crazy, when inputting my own odometry it returns the same message (Did not receive data since 5 seconds!)

The odom that AirSim publishes is in the format nav_msgs/Odometry, here is a screenshot of rostopic echo of the odom



 Bag file

https://filebin.net/srs3devx5nge9nnt/leon_airsim_RTAB.bag?t=5i4hjaqk

Kind regards,
Leon
Reply | Threaded
Open this post in threaded view
|

Re: 3D Lidar SLAM not receiving data

Masterman
In reply to this post by matlabbe
Hello Dr. Labbé

I was wondering if you managed to figure out any information regarding my issue
- What i am trying to do is map a 3D environment in AirSim with Lidar PointCloud2 and odometry data, I cannot use camera data as I am building this for an underwater agent, I also have access to IMU/GPS data.

The final artefact of my project will be an application of multi-agent SLAM running RTAB-MAP + a map merger

Kind regards
Masterman
Reply | Threaded
Open this post in threaded view
|

Re: 3D Lidar SLAM not receiving data

matlabbe
Administrator
Sorry I had quite busy past 3 weeks, it seems I missed your bag (link not working anymore). If you repost it I'll make sure to download it this week.

cheers,
Mathieu

Reply | Threaded
Open this post in threaded view
|

Re: 3D Lidar SLAM not receiving data

Masterman
Hello Dr Labbé

Thanks for your reply, I have reuploaded the bagfile to something a bit more reliable,
I tried ICP odometry following your comments here: https://answers.ros.org/question/366097/anyway-to-use-the-map-data-from-this-website-for-slam/ 
(which obviously didnt work for a 3D enviornment), however it did confirm that the pointcloud was getting picked up by RTAB-map

Video :https://www.youtube.com/watch?v=9VQlVLqXoYI

Bag file: https://drive.google.com/file/d/1JIi1uWIPyekjL_HkNCZt93tiLyakbCL_/view?usp=sharing

Just to confirm: I am trying to do SLAM soley with LiDAR PointCloud and either Odometry/Imu, I understand that ORB based loop closure requires visual data, however I saw from some other posts that LiDAR and Odometry is possible.

I also just read your paper propsing the storaging architecture for RTAB-map loop-closure which was very inspiring, im really happy to see how far this project has come from then.

Kind regards,
Leon
Reply | Threaded
Open this post in threaded view
|

Re: 3D Lidar SLAM not receiving data

matlabbe
Administrator
This post was updated on .
Hi Leon,

There are a couple of issues with this bag. The odometry and lidar stamps are not synchronized (odometry is like 15 sec in the past). The lidar frame_id is drone_1, but in rviz and odometry topic drone_1 frame is the "odom" fixed frame of the drone. The base frame of the drone seems to be drone_1/odom_local_enu, as drone_1 is fixed and child TF is drone_1/odom_local_enu:



Another issue is that the Lidar point clouds are in the wrong frame. They are in the fixed frame drone_1, while they should be in drone_1/LidarCustom frame. When mapping with only the lidar, the pose seems then not moving with the lidar.

Based on this post: this is what I could tested (not optimal because normal computation has error because of the wrong frame_id set in lidar topic):
$ roslaunch rtabmap_ros rtabmap.launch \
   subscribe_scan_cloud:=true \
   depth:=false \
   scan_cloud_topic:=/airsim_node/drone_1/lidar/LidarCustom \
   visual_odometry:=false  \
   approx_sync:=true \
   use_sim_time:=true \
   frame_id:=drone_1 \
   queue_size:=30 \
   icp_odometry:=true \
   scan_cloud_assembling:=true \
   scan_cloud_assembling_time:=0.5 \
   scan_cloud_assembling_voxel_size:=0 \
   args:="-d  \
      --RGBD/CreateOccupancyGrid false \
      --Rtabmap/DetectionRate 0 \
      --Odom/ScanKeyFrameThr 0.8 \
      --OdomF2M/ScanMaxSize 10000  \
      --OdomF2M/ScanSubtractRadius 1   \
      --Icp/PM true \
      --Icp/VoxelSize 1   \
      --Icp/MaxTranslation 4   \
      --Icp/MaxCorrespondenceDistance 3 \
      --Icp/PMOutlierRatio 0.7 \
      --Icp/Iterations 10 \
      --Icp/PointToPlane false \
      --Icp/PMMatcherKnn 3 \
      --Icp/PMMatcherEpsilon 1 \
      --Icp/Epsilon 0.0001 \
      --Icp/PointToPlaneK 10 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/CorrespondenceRatio 0.01"

$ rosbag play --clock leon_airsim_RTAB.bag

Reply | Threaded
Open this post in threaded view
|

Re: 3D Lidar SLAM not receiving data

Masterman
Dear Dr Labbe
Apologies for the delay in response I have just had my final exam period at university so I have not been able to work on this project for a few weeks.

I have implemented what you suggested into my project and it is working MUCH better, I am very greatful that you took the time to help me, however it is still not in an ideal situation, I believe that the simulation software I am using (AirSim) is not well optomised for implementations such as 3D mapping, It works great with more basic SLAM implementaitons such as Hector mapping and ORB_slam (I have linked below incase you are interested) but I have found it very tedious to use the input data from the simulation in more advanced SLAM packages such as your software and Google Cartographer.

I am able to produce a map of LiDAR data in RTAB-Viz of the enviornment however the software is not saving the observations - probably due to loop closure not computing correctly on the simulation data. I was wondering if it was possible to just map all LiDAR observations so as I can build a map to demonstrate the software to my supervisors.

Initial Observation


New Observation - initial observation is gone



Here is a video of what is happening now: https://www.youtube.com/watch?v=HZ5RbiHx0y8
I apologise for the poor framerate, my computer is quite outdated.

I am using the launch file that you sent above ^ in realtime (sim_time false)

Also - could you please explain how you were able to produce this image within the software?


I apprecaite all you've done

Kind regards,

Leon



Video of ORB Slam on Airsim: https://www.youtube.com/watch?v=-TG69O_LhKA
Video of Hector Mapping on AirSim: https://www.youtube.com/watch?v=4SEBt7sL1J8
Reply | Threaded
Open this post in threaded view
|

Re: 3D Lidar SLAM not receiving data

matlabbe
Administrator
Hi,

In my previous post, I had this issue:
Another issue is that the Lidar point clouds are in the wrong frame. They are in the fixed frame drone_1, while they should be in drone_1/LidarCustom frame. When mapping with only the lidar, the pose seems then not moving with the lidar.
The frame in the lidar point cloud was not the lidar frame, but in some world frame where the cloud is transformed depending on the current position of the drone in the simulator. The slam approach thinks that the robot is not moving, thus not adding new scans to map. If this cannot be fixed, you can set RGBD/LinearUpdate to 0.

For the visualization like that, the easiest way is to export the point cloud (from rtabmapviz, File->Export 3D Clouds), then open it in CloudCompare. In CloudCompare, enable EDL shader visualization, and do Edit->Colors->Height Ramp.

For this particular image, I did in rtabmapviz: Edit->View Point cloud, then hit key '5' for z gradient and right-click->Eye Dome Lighting shader (only available if rtabmap is built with VTK>=7, which is default on Ubuntu Focal)

cheers,
Mathieu