Interbotix / interbotix_ros_rovers

ROS Packages for Interbotix Rovers
BSD 3-Clause "New" or "Revised" License
31 stars 31 forks source link

[Question]: RTABMAP slam using only the RPLIDAR #4

Closed VenkataramanSubramanian closed 2 years ago

VenkataramanSubramanian commented 2 years ago

Question

Hi,

I am trying to do RTABMAP slam on the locobot by using only the RPLIDAR. Based on the documentation of the RTABMAP I updated the Navigation launch file by adding

--RGBD/Enabled false

and updating the values

 <param name="subscribe_depth"               value="false"/>
 <param name="subscribe_rgb"                 value="false"/>
 <param name="subscribe_rgbd"                value="false"/>
 <param name="subscribe_stereo"              value="false"/>
 <param name="subscribe_scan"                value="true"/>

The launch file executes and with commands

[ WARN] [1635860385.061111689]: There is no image subscription, bag-of-words loop closure detection will be disabled... [ WARN] [1635860385.061235989]: Setting Kp/MaxFeatures=-1 (bag-of-words disabled)

02/11 14:39:50,624 ERROR [139878276781824] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 130

02/11 14:39:53,955 ERROR [139878276781824] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 130

02/11 14:39:54,205 WARNING [139878671042304] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: No data available, number: 61

02/11 14:39:55,954 ERROR [139878276781824] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 130

02/11 14:39:56,256 WARNING [139878671042304] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11 02/11 14:39:56,306 WARNING [139878671042304] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11 02/11 14:39:56,953 ERROR [139878276781824] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 130

02/11 14:39:57,357 WARNING [139878671042304] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11 02/11 14:39:57,952 ERROR [139878276781824] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 130 [ WARN] (2021-11-02 14:39:58.243) util3d.cpp:605::cloudFromDepthRGB() Cloud with only NaN values created! [ WARN] (2021-11-02 14:39:58.274) util3d.cpp:605::cloudFromDepthRGB() Cloud with only NaN values created!

There is no map pubilshed in the topic /locobot/rtabmap/grid_map

Robot Model

locobot_wx200

Operating System

ubuntu 20.04

Anything Else

No response

lukeschmitt-tr commented 2 years ago

Hi @VenkataramanSubramanian. You can get rtabmap to create a map using only the lidar by modifying a few parameters and launch args:

Screenshot from 2021-11-02 12-17-11

Here is some of the relevant console output:

[ INFO] [1635873790.268935671]: 
/locobot/rtabmap/rtabmap subscribed to:
   /locobot/scan
[ WARN] [1635873790.268987394]: There is no image subscription, bag-of-words loop closure detection will be disabled...
[ WARN] [1635873790.269082484]: Setting Kp/MaxFeatures=-1 (bag-of-words disabled)
[ INFO] [1635873790.435140697]: rtabmap 0.20.9 started...
[ INFO] [1635873790.923616765]: rtabmap (1): Rate=1.00s, Limit=0.000s, Conversion=0.0028s, RTAB-Map=0.0141s, Maps update=0.0022s pub=0.0006s (local map=1, WM=1)
[ INFO] [1635873791.037094127]: rtabmap (2): Rate=1.00s, Limit=0.000s, Conversion=0.0007s, RTAB-Map=0.0120s, Maps update=0.0011s pub=0.0000s (local map=1, WM=1)
[ INFO] [1635873792.021386167]: rtabmap (3): Rate=1.00s, Limit=0.000s, Conversion=0.0007s, RTAB-Map=0.0125s, Maps update=0.0009s pub=0.0000s (local map=1, WM=1)
[ INFO] [1635873793.030623233]: rtabmap (4): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0134s, Maps update=0.0015s pub=0.0000s (local map=1, WM=1)
[ INFO] [1635873794.086537087]: rtabmap (5): Rate=1.00s, Limit=0.000s, Conversion=0.0009s, RTAB-Map=0.0134s, Maps update=0.0010s pub=0.0001s (local map=1, WM=1)
[ INFO] [1635873795.094988252]: rtabmap (6): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0139s, Maps update=0.0011s pub=0.0000s (local map=1, WM=1)
[ INFO] [1635873796.126238139]: rtabmap (7): Rate=1.00s, Limit=0.000s, Conversion=0.0008s, RTAB-Map=0.0132s, Maps update=0.0011s pub=0.0000s (local map=1, WM=1)
[ INFO] [1635873797.158613193]: rtabmap (8): Rate=1.00s, Limit=0.000s, Conversion=0.0006s, RTAB-Map=0.0138s, Maps update=0.0011s pub=0.0000s (local map=1, WM=1)
[ INFO] [1635873798.190300965]: rtabmap (9): Rate=1.00s, Limit=0.000s, Conversion=0.0007s, RTAB-Map=0.0132s, Maps update=0.0012s pub=0.0000s (local map=1, WM=1)

Note that global loop closure will be disabled if not using image data, though local loop closure will still work as normal.

VenkataramanSubramanian commented 2 years ago

Hi,

I am able to get it to work. Thanks you!!!