Our system is attempting to use rtabmap to map and localize with a 2D Lidar and a pair of Realsense cameras (a D435 and a T265). Rtabmap also receives odom info from a state estimator (we feed our robot's imu and odometry topics into robot_localization, which outputs to /odometry/filtered). As far as we can tell, /odometry/filtered outputs its data fine. When viewing the camera feeds via RealsenseViewer, they display fine. Lidar produced point-clouds seem to be produced correctly as well.
The issue we still run into is that rtabmap will repeatedly fail on its odometry, as indicated in its GUI when the odometry panel flashes red every two or three seconds. Looking at the live map being produced confirms that the mapping is failing (see the screenshot attached). We have also tried using a 3D Lidar, to no different effect.
Additionally, we get this warning during run time:
[ WARN] [1565188919.498007946]: Could not get transform from RLodom to base_link after 0.400000 seconds (for stamp=1565188919.076626)! Error="Could not find a connection between 'RLodom' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.402265 timeout was 0.4.".
[ WARN] (2019-08-07 10:42:00.006) Memory.cpp:810::addSignatureToStm() Very large angular variance (8.190147) detected! Please fix odometry twist covariance, otherwise poor graph optimizations are expected and wrong loop closure detections creating a lot of errors in the map could be accepted.
[ INFO] [1565188920.017104185]: rtabmap (567): Rate=1.00s, Limit=0.700s, RTAB-Map=0.0554s, Maps update=0.0019s pub=0.0002s (local map=106, WM=106).
roswtf produces this output:
"WARNING The following nodes are unexpectedly connected:
* /cam_d435/realsense2_camera_manager->/rtabmap/rtabmapviz (/tf_static)
* /cam_d435/realsense2_camera_manager->/rtabmap/rtabmap (/tf_static)
* /cam_t265/realsense2_camera_manager->/rtabmap/rtabmap (/tf_static)
* /cam_t265/realsense2_camera_manager->/rtabmap/rtabmapviz (/tf_static)
WARNING These nodes have respawned at least once:
* microstrain_comm_35-3 (2)
WARNING Received out-of-date/future transforms:
* receiving transform from [/imu_filter] that differed from ROS time by 1.447143925s
* receiving transform from [/cam_t265/realsense2_camera_manager] that differed from ROS time by 1.091286898s
* receiving transform from [/odom2] that differed from ROS time by 1.446536843s"
Our source code is below:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86 | <launch>
<arg name="gui_cfg" default="~/.ros/rtabmap_gui.ini" />
<arg name="launch_prefix" default=""/>
<arg name="output" default="screen"/>
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_link"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="false"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="false"/>
<!-- Add param name="subscribe_odom_info" type="bool" value="true"/> to fix problems with rtabmap subscribing to odom_info -->
<remap from="odom" to="/odometry/filtered"/>
<remap from="scan" to="/scan"/>
<!--remap from="rgb/image" to="/camera/color/image_raw"/-->
<!--remap from="depth/image" to="/camera/aligned_depth_to_color/image_raw"/-->
<!--remap from="rgb/camera_info" to="/camera/color/camera_info"/-->
<remap from="rgb/image" to="/cam_d435/color/image_raw"/>
<remap from="depth/image" to="/cam_d435/aligned_depth_to_color/image_raw"/>
<remap from="rgb/camera_info" to="/cam_d435/color/camera_info"/>
<param name="queue_size" type="int" value="100"/>
<!-- RTAB-Map's parameters -->
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/AngularUpdate" type="string" value="0.01"/>
<param name="RGBD/LinearUpdate" type="string" value="0.01"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="Reg/Force3DoF" type="string" value="true"/> <!--same as optimizer/slam2D -->
<param name="Reg/Strategy" type="string" value="1"/> <!-- 1=ICP -->
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Vis/MinInliers" type="string" value="5"/>
<param name="Vis/InlierDistance" type="string" value="0.1"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Grid/CellSize" type="string" value="0.05"/> <!-- 5cm voxel default-->
<param name="Kp/MaxFeatures" type="string" value="400"/>
<param name="Optimizer/Strategy" type="string" value="0"/> <!-- 0 is TORO, Default G20-->
<param name="RGBD/OptimizeMaxError" type="string" value="0"/> <!--When using TORO, seto to 0,otherwise set to 1-->
<param name="Kp/DetectorStrategy" type="string" value="6"/> <!--0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF(default) 7=BRISK 8=GFTT/ORB 9=KAZE-->
<param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/> <!--extract features even if there are some already in the node-->
<param name="Grid/FromDepth" type="string" value="false"/> <!--suppress warning (subscribe_scan=true)-->
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> <!--suppress warning (subscribe_scan = true and Reg/Strategy=1-->
<param name="Vis/FeatureType" type="string" value="6"/> <!--suppress warning,want to be same as KP/detectorstrategy -->
</node>
<!-- Visualisation RTAB-Map -->
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_rgbd" type="bool" value="false"/>
<param name="subscribe_odom_info" type="bool" value="false"/>
<param name="subscribe_scan_cloud" type="bool" value="false"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="odom_frame_id" type="string" value="RLodom"/>
<param name="wait_for_transform_duration" type="double" value="0.4"/>
<param name="queue_size" type="int" value="50"/>
<param name="approx_sync" type="bool" value="true"/>
<!--remap from="rgb/image" to="/camera/rgb/image_rect_color"/-->
<!--remap from="depth/image" to="/camera/depth_registered/image_raw"/-->
<!--remap from="rgb/camera_info" to="/camera/rgb/camera_info"/-->
<!--remap from="rgbd_image" to="/camera/rgb/image_rect_color"/-->
<remap from="rgb/image" to="/cam_d435/color/image_raw"/>
<remap from="depth/image" to="/cam_d435/aligned_depth_to_color/image_raw"/>
<remap from="rgb/camera_info" to="/cam_d435/color/camera_info"/>
<remap from="odom" to="/odometry/filtered"/>
<remap from="scan" to="/scan"/>
</node>
</group>
</launch>
|
Thank you in advance for any help!
Locked
