i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master->ros2. Updated turtlebot3 RGB-D examples (added instructions to add depth camera instead of copying sdf from another project)

Browse Source
This commit is contained in:
matlabbe
2022-01-29 18:01:08 -05:00
parent 0d7529332b 62b1058218
commit 324a0fe49a
18 changed files with 906 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -56,7 +56,7 @@ find_package(octomap_msgs)
#find_package(fiducial_msgs)
## System dependencies are found with CMake's conventions
find_package(RTABMap 0.20.17 REQUIRED)
find_package(RTABMap 0.20.18 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system) # dependencies from PCL
find_package(PCL 1.7 REQUIRED COMPONENTS kdtree) #This crashes idl generation if all components are found?! see https://github.com/ros2/rosidl/issues/402#issuecomment-565586908
README.md
+7
View File
@@ -80,4 +80,11 @@ $ ros2 launch rtabmap_ros rtabmap.launch.py \
args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" \
use_sim_time:=true
```
3. Launch navigation (`nav2_bringup` package should be installed):
```
$ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True
$ ros2 launch nav2_bringup rviz_launch.py
```
See [launch/ros2](https://github.com/introlab/rtabmap_ros/tree/ros2/launch/ros2) subfolder for some other ROS2 examples with turtlebot3 in simulation and a RGB-D camera.
include/rtabmap_ros/PreferencesDialogROS.h
+4
View File
@@ -42,6 +42,10 @@ public:
virtual QString getIniFilePath() const;
virtual QString getTmpIniFilePath() const;
bool hasAllParameters();
public slots:
void readRtabmapNodeParameters();
protected:
virtual QString getParamMessage();
launch/config/catvehicle.rviz
+367
View File
@@ -0,0 +1,367 @@
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
Splitter Ratio: 0.522352933883667
Tree Height: 297
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Image
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 100
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
back_left_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
back_right_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
camera_left_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
camera_right_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_laser_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_left_steering_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_left_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_right_steering_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_right_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
main_mass:
Alpha: 1
Show Axes: false
Show Trail: false
triclops_left_optical_link:
Alpha: 1
Show Axes: false
Show Trail: false
triclops_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
velodyne:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
velodyne_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
velodyne_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: catvehicle/robot_description
TF Prefix: catvehicle
Update Interval: 0
Value: true
Visual Enabled: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 164; 0; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /catvehicle/path
Unreliable: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: -9999
Min Value: 9999
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.5
Style: Spheres
Topic: /catvehicle/front_laser_points
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: ring
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /catvehicle/velodyne_points
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /catvehicle/map
Unreliable: false
Use Timestamp: false
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /catvehicle/triclops/left/image_rect_color
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 12.792472839355469
Min Value: 0.22430419921875
Value: true
Axis: Z
Channel Name: z
Class: rtabmap_ros/MapCloud
Cloud decimation: 1
Cloud from scan: true
Cloud max depth (m): 0
Cloud min depth (m): 1
Cloud voxel size (m): 0.20000000298023224
Color: 255; 255; 255
Color Transformer: AxisColor
Download graph: false
Download map: false
Download namespace: catvehicle
Enabled: true
Filter ceiling (m): 0
Filter floor (m): 0
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: MapCloud
Node filtering angle (degrees): 30
Node filtering radius (m): 0
Position Transformer: XYZ
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /catvehicle/mapData
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Class: rtabmap_ros/MapGraph
Enabled: true
Global loop closure: 255; 0; 0
Landmark: 0; 128; 0
Local loop closure: 255; 255; 0
Merged neighbor: 255; 170; 0
Name: MapGraph
Neighbor: 0; 0; 255
Topic: /catvehicle/mapGraph
Unreliable: false
User: 255; 0; 0
Value: true
Virtual: 255; 0; 255
Enabled: true
Global Options:
Background Color: 211; 215; 207
Default Light: true
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rtabmap_ros/OrbitOriented
Distance: 32.8247184753418
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5253984332084656
Target Frame: catvehicle/base_link
Value: OrbitOriented (rtabmap_ros)
Yaw: 2.720399856567383
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 959
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd00000004000000000000020c00000365fc020000000bfb000000100044006900730070006c006100790073000000003d00000166000000c900fffffffb0000000a0049006d006100670065010000003d000003650000001600fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d0061006700650000000200000001b50000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000319000000cb0000000000000000000000010000010f00000365fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000365000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000001c0fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003cfc0100000002fb0000000800540069006d00650000000000000005a0000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000002470000036500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1113
X: 807
Y: 27
launch/config/catvehicle_triclops.gazebo
+110
View File
@@ -0,0 +1,110 @@
<?xml version="1.0"?>
<!--
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren, Rahul Bhadani
Hannah Grace Mason, Joe Macinnes, Landon Chase Bentley
Copyright (c) 2015-2018 Arizona Board of Regents
All rights reserved.
Permission is hereby granted, without written agreement and without
license or royalty fees, to use, copy, modify, and distribute this
software and its documentation for any purpose, provided that the
above copyright notice and the following two paragraphs appear in
all copies of this software.
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
Summary:
This file includes gazebo reference for front stereo camera simulation mounted
on the top of the car. For more information and for the tutorials used to create
this file, see
http://gazebosim.org/tutorials/?tut=ros_control#Prerequisites
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="M_PI" value="3.1415926535897931" />
<!--triclops camera-->
<gazebo reference="triclops_link">
<mu1>0.6</mu1>
<mu2>0.5</mu2>
</gazebo>
<joint name="triclops_optical" type="fixed">
<origin xyz="0 0 0" rpy="-${M_PI/2} 0 -${M_PI/2}"/>
<parent link="triclops_link"/>
<child link="triclops_left_optical_link"/>
</joint>
<link name="triclops_left_optical_link"/>
<gazebo reference="triclops_link">
<sensor type="multicamera" name="triclops">
<update_rate>16.0</update_rate>
<camera name="left">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>960</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<camera name="right">
<pose>0 -0.07 0 0 0 0</pose>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>960</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_triclops_controller" filename="libgazebo_ros_multicamera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<robotNamespace>/$(arg roboname)</robotNamespace>
<cameraName>triclops</cameraName>
<imageTopicName>image_rect_color</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>triclops_left_optical_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</robot>
launch/config/catvehicle_velodyne_points.gazebo
+43
View File
@@ -0,0 +1,43 @@
<?xml version="1.0"?>
<!--
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
Copyright (c) 2015 Arizona Board of Regents
All rights reserved.
Permission is hereby granted, without written agreement and without
license or royalty fees, to use, copy, modify, and distribute this
software and its documentation for any purpose, provided that the
above copyright notice and the following two paragraphs appear in
all copies of this software.
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGE.
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
Summary:
This file includes the control interfaces for ROS-based control
through Gazebo. For more information and for the tutorials used to create
this file, see
http://gazebosim.org/tutorials/?tut=ros_control#Prerequisites
Sensors are included separately, based on the arguments passed to the xacro include
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
<xacro:VLP-16 parent="velodyne_link" name="velodyne" topic="/$(arg roboname)/velodyne_points" organize_cloud="false" hz="10" samples="440" gpu="true">
<origin xyz="0 0 0.0" rpy="0 0 0" />
</xacro:VLP-16>
</robot>
launch/demo/demo_catvehicle_mapping.launch
+124
View File
@@ -0,0 +1,124 @@
<?xml version="1.0"?>
<launch>
<!-- 1) Make sure rtabmap is built with libpointmatcher for lidar:=true option (lidar SLAM)
2) Install https://github.com/jmscslgroup/catvehicle
We need a stereo camera and velodyne point cloud looking like the real
sensor (ring-like pattern), so we have to modifiy the default catvehicle
urdf. First we have to install velodyne_simulator package to have the
right gazebo plugin and copy this modified velodyne config:
* sudo apt install ros-$ROS_DISTRO-velodyne-simulator
* cp ~/catkin_ws/src/rtabmap_ros/launch/config/catvehicle_velodyne_points.gazebo ~/catkin_ws/src/catvehicle/urdf/.
Secondly, we have to modify the triclops config to make it stereo:
* cp ~/catkin_ws/src/rtabmap_ros/launch/config/catvehicle_triclops.gazebo ~/catkin_ws/src/catvehicle/urdf/.
Set world->map frame instead of world->odom frame here:
* https://github.com/jmscslgroup/catvehicle/blob/f58cc64103538bc93cd42dd30f59c4b937151f88/launch/catvehicle.launch#L104
3) roslaunch catvehicle catvehicle_city.launch gui:=true
4) roslaunch catvehicle catvehicle_spawn.launch velodyne_points:=true triclops:=true Z:=5
5) roslaunch catvehicle joystick.launch
6) SLAM, 3 choices:
A) LiDAR-SLAM + Visual loop closure detection:
roslaunch rtabmap_ros demo_catvehicle_mapping.launch
B) LiDAR-SLAM without camera:
roslaunch rtabmap_ros demo_catvehicle_mapping.launch camera:=false
C) Visual-SLAM without lidar:
roslaunch rtabmap_ros demo_catvehicle_mapping.launch lidar:=false
Note: Gazebo real-time factor should be equal or below 1. If it is over 1,
in gazebo client, select Physics, then change real time update rate to 500.
-->
<arg name="camera" default="true" />
<arg name="lidar" default="true" />
<arg name="cell_size" default="0.2" />
<arg name="rtabmapviz" default="true" />
<arg name="rviz" default="true" />
<arg name="light" default="false" /> <!-- Don't record all scans if false -->
<arg name="altitude" default="0" /> <!-- assemble occupancy grids by altitude (radius in meters, 0=disabled) -->
<arg name="localization" default="false" />
<!-- Common parameters -->
<arg name="common_args" value="
--Rtabmap/DetectionRate 2
--Mem/STMSize 30
--Mem/LaserScanNormalK 10
--OdomF2M/ScanMaxSize 30000
--Odom/ScanKeyFrameThr 0.6
--Icp/VoxelSize $(arg cell_size)
--Icp/Iterations 10
--Icp/MaxTranslation 2
--Icp/RangeMin 1
--Icp/PointToPlaneGroundNormalsUp 0.9
--Icp/PointToPlaneRadius 0
--Icp/MaxCorrespondenceDistance 1
--Grid/ClusterRadius 1
--Grid/RangeMax 20
--Grid/RangeMin 2
--Grid/RayTracing true
--Grid/CellSize $(arg cell_size)
--Grid/PreVoxelFiltering false
--Grid/3D false
--Grid/DepthRoiRatios '0 0 0 0.3'
--Grid/MaxObstacleHeight 4
--Kp/RoiRatios '0 0 0 0.3'
--Vis/RoiRatios '0 0 0 0.3'
--RGBD/LinearUpdate 0.2
--RGBD/OptimizeMaxError 1
--GridGlobal/AltitudeDelta $(arg altitude)"/>
<arg if="$(arg localization)" name="clear_db" value="" />
<arg unless="$(arg localization)" name="clear_db" value="-d" />
<!-- RTAB-Map -->
<remap from="/catvehicle/grid_map" to="/catvehicle/map"/>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="namespace" value="catvehicle"/>
<!-- LiDAR SLAM parameters -->
<arg if="$(arg lidar)" name="args" value="$(arg clear_db) $(arg common_args)
--Reg/Strategy 1"/>
<!-- Visual SLAM parameters -->
<arg unless="$(arg lidar)" name="args" value="$(arg clear_db) $(arg common_args)
--Reg/Strategy 0
--Grid/NormalsSegmentation false
--Grid/MaxGroundHeight 0.7
--Grid/NoiseFilteringRadius 0.5"/>
<arg name="localization" value="$(arg localization)"/>
<arg name="subscribe_scan_cloud" value="$(arg lidar)"/>
<arg name="stereo" value="$(arg camera)"/>
<arg unless="$(arg camera)" name="depth" value="false"/>
<arg unless="$(arg camera)" name="subscribe_rgb" value="false"/>
<arg if="$(arg lidar)" name="icp_odometry" value="true"/>
<arg unless="$(arg lidar)" name="visual_odometry" value="true"/>
<arg if="$(arg lidar)" name="odom_topic" value="lidar_odom"/>
<arg unless="$(arg lidar)" name="odom_topic" value="visual_odom"/>
<arg name="odom_guess_frame_id" value="catvehicle/odom"/>
<arg name="frame_id" value="catvehicle/base_link"/>
<arg name="map_frame_id" value="catvehicle/map"/>
<arg if="$(arg lidar)" name="vo_frame_id" value="catvehicle/lidar_odom"/>
<arg unless="$(arg lidar)" name="vo_frame_id" value="catvehicle/visual_odom"/>
<arg name="stereo_namespace" value="/catvehicle/triclops"/>
<arg name="right_image_topic" value="/catvehicle/triclops/right/image_rect_color"/>
<arg name="scan_cloud_topic" value="/catvehicle/velodyne_points"/>
<arg name="rgbd_sync" value="$(arg camera)"/>
<arg name="approx_rgbd_sync" value="false"/>
<arg name="approx_sync" value="$(eval camera and lidar)"/>
<arg name="use_sim_time" value="true"/>
<arg name="wait_for_transform" value="0.3"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
<arg name="scan_cloud_assembling" value="$(eval lidar and not light)"/>
<arg name="scan_cloud_assembling_fixed_frame" value="catvehicle/lidar_odom"/>
<arg name="scan_cloud_assembling_range_max" value="60"/>
<arg name="scan_cloud_assembling_voxel_size" value="$(arg cell_size)"/>
<arg name="scan_cloud_assembling_range_min" value="2"/>
</include>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/catvehicle.rviz"/>
</launch>
launch/demo/demo_isaac_carter_navigation.launch
+10 -3
View File
@@ -26,6 +26,8 @@
<arg name="lidar3d_ray_tracing" default="true" />
<arg name="lidar3d_grid3d" default="true" />
<arg name="camera" default="true" />
<!-- Load Robot Description -->
<arg name="model" default="$(find carter_description)/urdf/carter.urdf"/>
<param name="robot_description" textfile="$(arg model)" />
@@ -33,17 +35,19 @@
<!-- Lidar 3D based on demo_husky.launch -->
<arg if="$(arg lidar3d)" name="cell_size" default="0.2" />
<arg unless="$(arg lidar3d)" name="cell_size" default="0.05" />
<arg if="$(arg lidar3d)" name="args3d" value="--Icp/Iterations 10 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/PointToPlaneRadius 0 --Icp/MaxCorrespondenceDistance 1 --Grid/ClusterRadius 1 --Grid/RangeMax 10 --Grid/RayTracing $(arg lidar3d_ray_tracing) --Grid/CellSize $(arg cell_size) --Mem/LaserScanNormalK --Grid/3D $(arg lidar3d_grid3d)" />
<arg if="$(arg lidar3d)" name="args3d" value="--Icp/Iterations 10 --Icp/PointToPlaneGroundNormalsUp 0.9 --Icp/PointToPlaneRadius 0 --Icp/MaxCorrespondenceDistance 1 --Grid/ClusterRadius 1 --Grid/RangeMax 10 --Grid/RayTracing $(arg lidar3d_ray_tracing) --Grid/CellSize $(arg cell_size) --Mem/LaserScanNormalK 10 --Grid/3D $(arg lidar3d_grid3d)" />
<arg unless="$(arg lidar3d)" name="args3d" value="--Grid/RangeMax 10" /> <!-- 2d scan, use default params -->
<!-- RTAB-Map -->
<remap from="/rtabmap/grid_map" to="/map"/>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg if="$(arg localization)" name="args" value="--Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true $(arg args3d)"/>
<arg unless="$(arg localization)" name="args" value="-d --Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true $(arg args3d)"/>
<arg if="$(arg localization)" name="args" value="--Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true --Icp/MaxTranslation 0.5 $(arg args3d) --Kp/MaxFeatures -1 --RGBD/ProximityBySpace false --Rtabmap/DetectionRate 3"/>
<arg unless="$(arg localization)" name="args" value="-d --Reg/Strategy 1 --Reg/Force3DoF true --RGBD/NeighborLinkRefining true --Icp/MaxTranslation 0.5 $(arg args3d)"/>
<arg name="localization" value="$(arg localization)"/>
<arg if="$(arg lidar3d)" name="subscribe_scan_cloud" value="true"/>
<arg unless="$(arg lidar3d)" name="subscribe_scan" value="true"/>
<arg name="subscribe_depth" value="$(arg camera)"/>
<arg name="subscribe_rgb" value="$(arg camera)"/>
<arg name="visual_odometry" value="false"/>
<arg name="odom_topic" value="/odom"/>
<arg name="frame_id" value="base_link"/>
@@ -55,6 +59,9 @@
<arg name="rgbd_sync" value="true"/>
<arg name="approx_rgbd_sync" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="tag_linear_variance" value="0.01"/>
<arg name="tag_angular_variance" value="1"/>
<arg name="scan_cloud_assembling" value="$(arg lidar3d)"/>
<arg name="scan_cloud_assembling_fixed_frame" value="odom"/>
launch/ros2/turtlebot3_rgbd.launch.py
+21 -5
View File
@@ -1,6 +1,22 @@
# Requirements:
# Install Turtlebot3 packages
# Install https://github.com/mlherd/ros2_turtlebot3_waffle_intel_realsense
# Modify turtlebot3_waffle SDF:
# 1) Edit turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
# 2) Add
# <joint name="camera_rgb_optical_joint" type="fixed">
# <parent>camera_rgb_frame</parent>
# <child>camera_rgb_optical_frame</child>
# <pose>0 0 0 -1.57079632679 0 -1.57079632679</pose>
# <axis>
# <xyz>0 0 1</xyz>
# </axis>
# </joint>
# 3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
# 4) Add <link name="camera_rgb_frame"/>
# 5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
# 6) Change image width/height from 1920x1080 to 640x480
# 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans
# hitting the robot itself
# Example:
# $ export TURTLEBOT3_MODEL=waffle
# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
@@ -8,7 +24,7 @@
# SLAM:
# $ ros2 launch rtabmap_ros turtlebot3_rgbd.launch.py
# OR
# $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint odom_topic:=/odom args:="-d" use_sim_time:=true rgb_topic:=/intel_realsense_r200_depth/image_raw depth_topic:=/intel_realsense_r200_depth/depth/image_raw camera_info_topic:=/intel_realsense_r200_depth/camera_info approx_sync:=true
# $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint odom_topic:=/odom args:="-d" use_sim_time:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info approx_sync:=true
#
# Navigation (install nav2_bringup package):
# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True
@@ -41,9 +57,9 @@ def generate_launch_description():
}
remappings=[
('rgb/image', '/intel_realsense_r200_depth/image_raw'),
('rgb/camera_info', '/intel_realsense_r200_depth/camera_info'),
('depth/image', '/intel_realsense_r200_depth/depth/image_raw')]
('rgb/image', '/camera/image_raw'),
('rgb/camera_info', '/camera/camera_info'),
('depth/image', '/camera/depth/image_raw')]
return LaunchDescription([
launch/ros2/turtlebot3_rgbd_sync.launch.py
+23 -6
View File
@@ -1,6 +1,22 @@
# Requirements:
# Install Turtlebot3 packages
# Install https://github.com/mlherd/ros2_turtlebot3_waffle_intel_realsense
# Modify turtlebot3_waffle SDF:
# 1) Edit turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
# 2) Add
# <joint name="camera_rgb_optical_joint" type="fixed">
# <parent>camera_rgb_frame</parent>
# <child>camera_rgb_optical_frame</child>
# <pose>0 0 0 -1.57079632679 0 -1.57079632679</pose>
# <axis>
# <xyz>0 0 1</xyz>
# </axis>
# </joint>
# 3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
# 4) Add <link name="camera_rgb_frame"/>
# 5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
# 6) Change image width/height from 1920x1080 to 640x480
# 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans
# hitting the robot itself
# Example:
# $ export TURTLEBOT3_MODEL=waffle
# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
@@ -8,7 +24,7 @@
# SLAM:
# $ ros2 launch rtabmap_ros turtlebot3_rgbd_sync.launch.py
# OR
# $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" use_sim_time:=true rgbd_sync:=true rgb_topic:=/intel_realsense_r200_depth/image_raw depth_topic:=/intel_realsense_r200_depth/depth/image_raw camera_info_topic:=/intel_realsense_r200_depth/camera_info
# $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true approx_sync:=true approx_rgbd_sync:=false odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true rgbd_sync:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info
#
# Navigation (install nav2_bringup package):
# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True
@@ -43,13 +59,14 @@ def generate_launch_description():
'Reg/Strategy':'1',
'Reg/Force3DoF':'true',
'RGBD/NeighborLinkRefining':'True',
'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
}
remappings=[
('rgb/image', '/intel_realsense_r200_depth/image_raw'),
('rgb/camera_info', '/intel_realsense_r200_depth/camera_info'),
('depth/image', '/intel_realsense_r200_depth/depth/image_raw')]
('rgb/image', '/camera/image_raw'),
('rgb/camera_info', '/camera/camera_info'),
('depth/image', '/camera/depth/image_raw')]
return LaunchDescription([
@@ -69,7 +86,7 @@ def generate_launch_description():
# Nodes to launch
Node(
package='rtabmap_ros', executable='rgbd_sync', output='screen',
parameters=[{'approx_sync':True, 'use_sim_time':use_sim_time, 'qos':qos}],
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time, 'qos':qos}],
remappings=remappings),
# SLAM Mode:
launch/ros2/turtlebot3_scan.launch.py
+5 -1
View File
@@ -1,5 +1,8 @@
# Requirements:
# Install Turtlebot3 packages
# Note that we can edit turtlebot3_gazebo/models/turtlebot_waffle/model.sdf
# to increase min scan range from 0.12 to 0.2 to avoid having scans
# hitting the robot itself
# Example:
# $ export TURTLEBOT3_MODEL=waffle
# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
@@ -7,7 +10,7 @@
# SLAM:
# $ ros2 launch rtabmap_ros turtlebot3_scan.launch.py
# OR
# $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true depth:=false approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" use_sim_time:=true
# $ ros2 launch rtabmap_ros rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true depth:=false approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true
#
# Navigation (install nav2_bringup package):
# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True
@@ -41,6 +44,7 @@ def generate_launch_description():
'Reg/Strategy':'1',
'Reg/Force3DoF':'true',
'RGBD/NeighborLinkRefining':'True',
'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
}
launch/rtabmap.launch
+1
View File
@@ -321,6 +321,7 @@
<param name="range_max" type="double" value="$(arg scan_cloud_assembling_range_max)"/>
<param name="noise_radius" type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
<param name="noise_min_neighbors" type="int" value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
</node>
<!-- Visual SLAM (robot side) -->
package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rtabmap_ros</name>
<version>0.20.17</version>
<version>0.20.18</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
src/GuiWrapper.cpp
+6
View File
@@ -222,6 +222,12 @@ void GuiWrapper::goalReachedCallback(
void GuiWrapper::processRequestedMap(const rtabmap_ros::msg::MapData & map)
{
// Make sure parameters are loaded
if(((PreferencesDialogROS*)prefDialog_)->hasAllParameters())
{
QMetaObject::invokeMethod(((PreferencesDialogROS*)prefDialog_), "readRtabmapNodeParameters");
}
std::map<int, Signature> signatures;
std::map<int, Transform> poses;
std::multimap<int, rtabmap::Link> constraints;
src/MapAssemblerNode.cpp
+147 -23
View File
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/MsgConversion.h"
#include "rtabmap_ros/MapsManager.h"
#include "rtabmap_ros/GetMap.h"
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
@@ -44,6 +45,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <nav_msgs/OccupancyGrid.h>
#include <std_srvs/Empty.h>
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
#include <octomap_msgs/GetOctomap.h>
#include <octomap_msgs/conversions.h>
#include <rtabmap/core/OctoMap.h>
#endif
#endif
using namespace rtabmap;
class MapAssembler
@@ -63,7 +72,10 @@ public:
//parameters
rtabmap::ParametersMap parameters;
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
if(!configPath.empty())
{
if(UFile::exists(configPath.c_str()))
@@ -112,12 +124,6 @@ public:
ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
iter->second = uNumber2Str(vInt);
}
if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
{
ROS_WARN( "Parameter min_inliers must be >= 8, setting to 8...");
iter->second = uNumber2Str(8);
}
}
rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
@@ -162,15 +168,75 @@ public:
}
}
// set private parameters
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
pnh.setParam(iter->first, iter->second);
}
ROS_INFO("%s: regenerate_local_grids = %s", ros::this_node::getName().c_str(), localGridsRegenerated_?"true":"false");
mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
mapsManager_.backwardCompatibilityParameters(pnh, parameters);
mapsManager_.setParameters(parameters);
std::list<std::string> splitName = uSplit(nh.resolveName("mapData"), '/');
std::string rtabmapNs;
for(std::list<std::string>::iterator iter=splitName.begin(); iter!=splitName.end() && iter!=--splitName.end(); ++iter)
{
if(!rtabmapNs.empty())
{
rtabmapNs += "/";
}
rtabmapNs += *iter;
}
ROS_INFO("Rtabmap namespace is \"%s\", deduced from topic \"%s\"", rtabmapNs.c_str(), nh.resolveName("mapData").c_str());
if(rtabmapNs.empty())
{
rtabmapNs = "get_map_data";
}
else
{
rtabmapNs += "/get_map_data";
}
rtabmap_ros::GetMap getMapSrv;
getMapSrv.request.global = false;
getMapSrv.request.optimized = true;
getMapSrv.request.graphOnly = false;
if(ros::service::waitForService(rtabmapNs, 5000))
{
if(!ros::service::call(rtabmapNs, getMapSrv))
{
ROS_WARN("Cannot call \"%s\" service", rtabmapNs.c_str());
}
else
{
ROS_INFO("Called \"%s\" service, initializing cache...", rtabmapNs.c_str());
processMapData(getMapSrv.response.data);
ROS_INFO("Called \"%s\" service, initializing cache... done! The map"
" will be assembled on next subscriber connection.", rtabmapNs.c_str());
}
}
else
{
ROS_WARN("Service \"%s\" not available after waiting for 5 seconds, "
"may not be a problem if rtabmap is started afterwards. If rtabmap "
"is started after in localization mode, call /rtabmap/publish_maps "
"service with graph_only=false to make sure map_assembler has all the data.", rtabmapNs.c_str());
}
mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
// private service
// private services
resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
octomapBinarySrv_ = pnh.advertiseService("octomap_binary", &MapAssembler::octomapBinaryCallback, this);
octomapFullSrv_ = pnh.advertiseService("octomap_full", &MapAssembler::octomapFullCallback, this);
#endif
#endif
}
~MapAssembler()
@@ -178,25 +244,29 @@ public:
}
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
{
processMapData(*msg);
}
void processMapData(const rtabmap_ros::MapData & msg)
{
UTimer timer;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
Transform mapOdom;
rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
for(unsigned int i=0; i<msg->nodes.size(); ++i)
rtabmap_ros::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
for(unsigned int i=0; i<msg.nodes.size(); ++i)
{
if(msg->nodes[i].image.size() ||
msg->nodes[i].depth.size() ||
msg->nodes[i].laserScan.size())
if(msg.nodes[i].image.size() ||
msg.nodes[i].depth.size() ||
msg.nodes[i].laserScan.size())
{
Signature data = rtabmap_ros::nodeDataFromROS(msg->nodes[i]);
Signature data = rtabmap_ros::nodeDataFromROS(msg.nodes[i]);
if(localGridsRegenerated_)
{
data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
}
uInsert(nodes_, std::make_pair(msg->nodes[i].id, data));
uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
}
}
@@ -211,16 +281,23 @@ public:
}
// Update maps
poses = mapsManager_.updateMapCaches(
poses,
0,
false,
false,
nodes_);
if(!nodes_.empty())
{
poses = mapsManager_.updateMapCaches(
poses,
0,
false,
false,
nodes_);
}
double updateTime = timer.ticks();
mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
mapFrameId_ = msg.header.frame_id;
optimizedPoses_ = poses;
ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
mapsManager_.publishMaps(poses, msg.header.stamp, msg.header.frame_id);
ROS_INFO("map_assembler: Updating = %fs, Publishing data = %fs (subscribers=%s)", updateTime, timer.ticks(), mapsManager_.hasSubscribers()?"true":"false");
}
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -230,20 +307,64 @@ public:
return true;
}
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
bool octomapBinaryCallback(
octomap_msgs::GetOctomap::Request &req,
octomap_msgs::GetOctomap::Response &res)
{
ROS_INFO("Sending binary map data on service request");
res.map.header.frame_id = mapFrameId_;
res.map.header.stamp = ros::Time::now();
mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
return success;
}
bool octomapFullCallback(
octomap_msgs::GetOctomap::Request &req,
octomap_msgs::GetOctomap::Response &res)
{
ROS_INFO("Sending full map data on service request");
res.map.header.frame_id = mapFrameId_;
res.map.header.stamp = ros::Time::now();
mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
return success;
}
#endif
#endif
private:
MapsManager mapsManager_;
std::map<int, Signature> nodes_;
std::map<int, Transform> optimizedPoses_;
std::string mapFrameId_;
ros::Subscriber mapDataTopic_;
ros::ServiceServer resetService_;
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
ros::ServiceServer octomapBinarySrv_;
ros::ServiceServer octomapFullSrv_;
#endif
#endif
bool localGridsRegenerated_;
};
int main(int argc, char** argv)
{
ULogger::setLevel(ULogger::kError);
ULogger::setType(ULogger::kTypeConsole);
ros::init(argc, argv, "map_assembler");
// process "--params" argument
@@ -253,7 +374,10 @@ int main(int argc, char** argv)
{
rtabmap::ParametersMap parameters;
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("GridGlobal"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoSGBM"));
uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(rtabmap::Parameters::defaultIcpPointToPlaneGroundNormalsUp())));
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
src/PreferencesDialogROS.cpp
+14
View File
@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QMessageBox>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UThread.h>
using namespace rtabmap;
@@ -67,6 +68,11 @@ QString PreferencesDialogROS::getTmpIniFilePath() const
return QDir::homePath()+"/.ros/"+QFileInfo(configFile_).fileName()+".tmp";
}
void PreferencesDialogROS::readRtabmapNodeParameters()
{
readCoreSettings(getTmpIniFilePath());
}
void PreferencesDialogROS::readCameraSettings(const QString &)
{
this->setInputRate(0);
@@ -77,6 +83,13 @@ QString PreferencesDialogROS::getParamMessage()
return tr("Reading parameters from the ROS server...");
}
bool PreferencesDialogROS::hasAllParameters()
{
auto node = std::make_shared<rclcpp::Node>("rtabmapviz");
auto client = std::make_shared<rclcpp::AsyncParametersClient>(node, rtabmapNodeName_);
return client->service_is_ready();
}
bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
{
QString path = getIniFilePath();
@@ -102,6 +115,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
}
std::vector<std::string> rosParameters;
for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
{
if(i->first.compare(rtabmap::Parameters::kRtabmapWorkingDirectory()) == 0)
src/rviz/MapCloudDisplay.cpp
+19 -5
View File
@@ -137,6 +137,7 @@ MapCloudDisplay::MapCloudDisplay()
cloud_from_scan_ = new rviz_common::properties::BoolProperty( "Cloud from scan", false,
"Create the cloud from laser scans instead of the RGB-D/Stereo images.",
this, SLOT( updateCloudParameters() ), this );
fromScan_ = cloud_from_scan_->getBool();
cloud_decimation_ = new rviz_common::properties::IntProperty( "Cloud decimation", 4,
"Decimation of the input RGB and depth images before creating the cloud.",
@@ -269,6 +270,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::msg::MapData& map)
// Add new clouds...
bool fromDepth = !cloud_from_scan_->getBool();
std::set<int> nodeDataReceived;
for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
{
int id = map.nodes[i].id;
@@ -366,6 +368,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::msg::MapData& map)
}
}
}
nodeDataReceived.insert(id);
}
// Update graph
@@ -380,6 +383,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::msg::MapData& map)
std::unique_lock<std::mutex> lock(current_map_mutex_);
current_map_ = poses;
current_map_updated_ = true;
nodeDataReceived_.insert(nodeDataReceived.begin(), nodeDataReceived.end());
}
}
@@ -486,7 +490,14 @@ void MapCloudDisplay::updateBillboardSize()
void MapCloudDisplay::updateCloudParameters()
{
// do nothing... only take effect on next generated clouds
// do nothing for most parameters... only take effect on next generated clouds
// if we change the kind of map, clear
if(fromScan_ != cloud_from_scan_->getBool())
{
reset();
}
fromScan_ = cloud_from_scan_->getBool();
}
void MapCloudDisplay::downloadMap(bool /*graphOnly*/)
@@ -706,7 +717,8 @@ void MapCloudDisplay::update( float, float )
cloudInfoIt->second->pose_ = it->second;
Ogre::Vector3 framePosition;
Ogre::Quaternion frameOrientation;
if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header.frame_id, framePosition, frameOrientation))
std::string error;
if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header.frame_id, cloudInfoIt->second->message_->header.stamp, framePosition, frameOrientation))
{
// Multiply frame with pose
Ogre::Matrix4 frameTransform;
@@ -726,15 +738,16 @@ void MapCloudDisplay::update( float, float )
cloudInfoIt->second->scene_node_->setVisible(true);
++totalNodesShown;
}
else
else if(context_->getFrameManager()->transformHasProblems(cloudInfoIt->second->message_->header.frame_id, cloudInfoIt->second->message_->header.stamp, error))
{
RVIZ_COMMON_LOG_ERROR(uFormat("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\", set fixed frame in global options to \"%s\")",
RVIZ_COMMON_LOG_ERROR(uFormat("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\" (reason=%s), set fixed frame in global options to \"%s\")",
it->first,
cloudInfoIt->second->message_->header.frame_id.c_str(),
error.c_str(),
cloudInfoIt->second->message_->header.frame_id.c_str()));
}
}
else if(it->first>0 && current_map_updated_)
else if(it->first>0 && current_map_updated_&& nodeDataReceived_.find(it->first) == nodeDataReceived_.end())
{
missingNodes.push_back(it->first);
}
@@ -792,6 +805,7 @@ void MapCloudDisplay::reset()
std::unique_lock<std::mutex> lock(current_map_mutex_);
current_map_.clear();
current_map_updated_ = false;
nodeDataReceived_.clear();
}
}
src/rviz/MapCloudDisplay.h
+3
View File
@@ -182,6 +182,9 @@ private:
std::map<int, CloudInfoPtr> new_cloud_infos_;
std::mutex new_clouds_mutex_;
std::set<int> nodeDataReceived_;
bool fromScan_;
std::map<int, rtabmap::Transform> current_map_;
std::mutex current_map_mutex_;
bool current_map_updated_;