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

rtabmap node publishes now directly /rtabmap/cloud_map, /rtabmap/proj_map and /rtabmap/grid_map (added all related rosparam), update grid_map_assembler node with the new efficient way of creating map

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-11 17:09:29 -05:00
parent 766f84028f
commit c139c263db
10 changed files with 682 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/config/demo_find_object.rviz
+50 -88
View File
@@ -7,7 +7,7 @@ Panels:
- /Global Options1 - /Global Options1
- /TF1/Frames1 - /TF1/Frames1
Splitter Ratio: 0.434783 Splitter Ratio: 0.434783
Tree Height: 410 Tree Height: 590
- Class: rviz/Selection - Class: rviz/Selection
Name: Selection Name: Selection
- Class: rviz/Tool Properties - Class: rviz/Tool Properties
@@ -82,24 +82,36 @@ Visualization Manager:
Frame Timeout: 15 Frame Timeout: 15
Frames: Frames:
All Enabled: false All Enabled: false
az3_base_link:
Value: true
az3_odom:
Value: true
base_footprint: base_footprint:
Value: true Value: true
base_laser_link: base_laser_link:
Value: true Value: true
base_link: base_link:
Value: true Value: true
camera_depth_frame:
Value: true
camera_depth_optical_frame:
Value: true
camera_link:
Value: true
camera_rgb_frame:
Value: true
camera_rgb_optical_frame:
Value: true
map: map:
Value: true Value: true
object_4:
Value: true
object_5:
Value: true
object_6:
Value: true
object_7:
Value: true
object_8:
Value: true
odom: odom:
Value: true Value: true
stereo_camera:
Value: true
stereo_camera_base:
Value: true
wheelLB_linkWheel_link: wheelLB_linkWheel_link:
Value: true Value: true
wheelLB_wheel_link: wheelLB_wheel_link:
@@ -128,9 +140,22 @@ Visualization Manager:
base_link: base_link:
base_laser_link: base_laser_link:
{} {}
stereo_camera_base: camera_link:
stereo_camera: camera_depth_frame:
{} camera_depth_optical_frame:
{}
camera_rgb_frame:
camera_rgb_optical_frame:
object_4:
{}
object_5:
{}
object_6:
{}
object_7:
{}
object_8:
{}
wheelLB_linkWheel_link: wheelLB_linkWheel_link:
wheelLB_wheel_link: wheelLB_wheel_link:
{} {}
@@ -148,8 +173,8 @@ Visualization Manager:
- Alpha: 1 - Alpha: 1
Autocompute Intensity Bounds: true Autocompute Intensity Bounds: true
Autocompute Value Bounds: Autocompute Value Bounds:
Max Value: 3.66965 Max Value: -0.574923
Min Value: -0.144665 Min Value: -5.11672
Value: true Value: true
Axis: X Axis: X
Channel Name: intensity Channel Name: intensity
@@ -185,85 +210,30 @@ Visualization Manager:
- Alpha: 1 - Alpha: 1
Class: rviz/RobotModel Class: rviz/RobotModel
Collision Enabled: false Collision Enabled: false
Enabled: true Enabled: false
Links: Links:
All Links Enabled: true All Links Enabled: true
Expand Joint Details: false Expand Joint Details: false
Expand Link Details: false Expand Link Details: false
Expand Tree: false Expand Tree: false
Link Tree Style: Links in Alphabetic Order Link Tree Style: Links in Alphabetic Order
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_laser_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelLB_linkWheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelLB_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelLF_linkWheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelLF_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelRB_linkWheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelRB_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelRF_linkWheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheelRF_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel Name: RobotModel
Robot Description: robot_description Robot Description: robot_description
TF Prefix: "" TF Prefix: ""
Update Interval: 0 Update Interval: 0
Value: true Value: false
Visual Enabled: true Visual Enabled: true
- Class: rviz/Image - Class: rviz/Image
Enabled: true Enabled: false
Image Topic: /camera/data_throttled_image_relay Image Topic: /camera/data_throttled_image
Max Value: 1 Max Value: 1
Median window: 5 Median window: 5
Min Value: 0 Min Value: 0
Name: Image Name: Image
Normalize Range: true Normalize Range: true
Queue Size: 2 Queue Size: 2
Transport Hint: raw Transport Hint: compressed
Value: true Value: false
- Alpha: 1 - Alpha: 1
Autocompute Intensity Bounds: true Autocompute Intensity Bounds: true
Autocompute Value Bounds: Autocompute Value Bounds:
@@ -303,14 +273,6 @@ Visualization Manager:
Name: Info Name: Info
Topic: /rtabmap/info Topic: /rtabmap/info
Value: true Value: true
- Alpha: 0.7
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /rtabmap/grid_projection_map
Value: true
Enabled: true Enabled: true
Global Options: Global Options:
Background Color: 48; 48; 48 Background Color: 48; 48; 48
@@ -331,7 +293,7 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rtabmap_ros/OrbitOriented Class: rtabmap_ros/OrbitOriented
Distance: 7.83194 Distance: 7.89956
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.06 Stereo Eye Separation: 0.06
Stereo Focal Distance: 1 Stereo Focal Distance: 1
@@ -343,10 +305,10 @@ Visualization Manager:
Z: -0.0896359 Z: -0.0896359
Name: Current View Name: Current View
Near Clip Distance: 0.01 Near Clip Distance: 0.01
Pitch: 0.699796 Pitch: 0.819797
Target Frame: base_link Target Frame: base_link
Value: OrbitOriented (rtabmap) Value: OrbitOriented (rtabmap)
Yaw: 3.19875 Yaw: 3.14375
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:
@@ -356,7 +318,7 @@ Window Geometry:
Hide Right Dock: false Hide Right Dock: false
Image: Image:
collapsed: false collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002ddfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000229000000dd00fffffffb0000000a0049006d006100670065010000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000463000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002ddfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000000000002dd000000dd00fffffffb0000000a0049006d006100670065000000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000463000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
Selection: Selection:
collapsed: false collapsed: false
Time: Time:
@@ -367,4 +329,4 @@ Window Geometry:
collapsed: false collapsed: false
Width: 1561 Width: 1561
X: 42 X: 42
Y: 108 Y: 107
launch/demo/demo_find_object.launch
+2 -11
View File
@@ -20,12 +20,12 @@
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/> <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<param name="rgb/image_transport" type="string" value="compressed"/> <param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/> <param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="queue_size" type="int" value="10"/> <param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. --> <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/ScanMatchingSize" type="string" value="1"/> <!-- Do odometry correction with consecutive laser scans --> <param name="RGBD/PoseScanMatching" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM --> <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM --> <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) --> <param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
@@ -50,15 +50,6 @@
<param name="robot_description" command="$(find xacro)/xacro.py '$(find az3_description)/robots/azimut_3_laser.urdf.xacro'" /> <param name="robot_description" command="$(find xacro)/xacro.py '$(find az3_description)/robots/azimut_3_laser.urdf.xacro'" />
--> -->
<!-- Grid map assembler for rviz -->
<node pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler" output="screen">
<remap from="mapData" to="rtabmap/mapData"/>
</node>
<!-- just to re-use azimut3.rviz config below -->
<node name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
<node name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay"/>
<!-- Visualisation --> <!-- Visualisation -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/>
launch/demo/demo_hector_mapping.launch
-5
View File
@@ -67,11 +67,6 @@
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance --> <param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
</node> </node>
<!-- Grid map assembler for rviz -->
<node if="$(arg rviz)" pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler">
<param name="unknown_space_filled" type="bool" value="false"/>
</node>
<!-- Visualisation RTAB-Map --> <!-- 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"> <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_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="true"/>
launch/demo/demo_multi-session_mapping.launch
-5
View File
@@ -70,11 +70,6 @@
<param name="depth/image_transport" type="string" value="compressedDepth"/> <param name="depth/image_transport" type="string" value="compressedDepth"/>
</node> </node>
<!-- Grid map assembler for rviz -->
<node if="$(arg rviz)" pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler">
<param name="unknown_space_filled" type="bool" value="false"/>
</node>
</group> </group>
<!-- Visualisation RVIZ --> <!-- Visualisation RVIZ -->
launch/demo/demo_robot_localization.launch
-5
View File
@@ -47,11 +47,6 @@
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance --> <param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
</node> </node>
<!-- Grid map assembler for rviz -->
<node if="$(arg rviz)" pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler">
<param name="unknown_space_filled" type="bool" value="false"/>
</node>
<!-- Visualisation RTAB-Map --> <!-- 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"> <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_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="true"/>
launch/demo/demo_robot_mapping.launch
-5
View File
@@ -42,11 +42,6 @@
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated --> <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
</node> </node>
<!-- Grid map assembler for rviz -->
<node if="$(arg rviz)" pkg="rtabmap_ros" type="grid_map_assembler" name="grid_map_assembler">
<param name="unknown_space_filled" type="bool" value="false"/>
</node>
<!-- Visualisation RTAB-Map --> <!-- 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"> <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_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="true"/>
src/CoreWrapper.cpp
+555 -30
View File
@@ -26,17 +26,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include "CoreWrapper.h" #include "CoreWrapper.h"
#include <stdio.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <sensor_msgs/Image.h> #include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h> #include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h> #include <cv_bridge/cv_bridge.h>
#include <nav_msgs/Path.h> #include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <std_msgs/Int32MultiArray.h> #include <std_msgs/Int32MultiArray.h>
#include <std_msgs/Bool.h> #include <std_msgs/Bool.h>
#include <rtabmap/core/RtabmapEvent.h> #include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Camera.h> #include <rtabmap/core/Camera.h>
#include <rtabmap/core/Parameters.h> #include <rtabmap/core/Parameters.h>
#include <rtabmap/core/util3d.h> #include <rtabmap/core/util3d.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Memory.h> #include <rtabmap/core/Memory.h>
#include <rtabmap/core/VWDictionary.h> #include <rtabmap/core/VWDictionary.h>
#include <rtabmap/utilite/UEventsManager.h> #include <rtabmap/utilite/UEventsManager.h>
@@ -73,6 +76,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()), databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
waitForTransform_(false), waitForTransform_(false),
useActionForGoal_(false), useActionForGoal_(false),
cloudDecimation_(4),
cloudMaxDepth_(4.0), // meters
cloudVoxelSize_(0.02), // meters
projMaxGroundAngle_(45.0), // degrees
projMinClusterSize_(20),
projMaxHeight_(2.0), // meters
gridCellSize_(0.05), // meters
gridSize_(0), // meters
mapFilterRadius_(0.5),
mapFilterAngle_(30.0), // degrees
mapToOdom_(tf::Transform::getIdentity()), mapToOdom_(tf::Transform::getIdentity()),
depthSync_(0), depthSync_(0),
depthScanSync_(0), depthScanSync_(0),
@@ -126,14 +139,37 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_); pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
// cloud map stuff
pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
//projection map stuff
pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_);
pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_);
pnh.param("proj_max_height", projMaxHeight_, projMaxHeight_);
// common grid map stuff
pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m
pnh.param("grid_size", gridSize_, gridSize_); // m
// common map stuff
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str()); ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str()); ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
ROS_INFO("rtabmap: queue_size = %d", queueSize); ROS_INFO("rtabmap: queue_size = %d", queueSize);
ROS_INFO("rtabmap: tf_delay = %f", tfDelay); ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1); infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
mapData_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1); mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
mapGraph_ = nh.advertise<rtabmap_ros::Graph>("graph", 1); mapGraphPub_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
// mapping topics
cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1);
projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1);
gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
// planning topics // planning topics
goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this); goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
@@ -284,6 +320,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this); setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this); setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this); getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
getGridMapSrv_ = nh.advertiseService("get_grid_map", &CoreWrapper::getGridMapCallback, this);
getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this); publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this); setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
@@ -442,8 +480,7 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
} }
else else
{ {
const Statistics & stats = rtabmap_.getStatistics(); this->publishStats(ros::Time::now());
this->publishStats(stats, ros::Time::now());
} }
} }
else if(!rtabmap_.isIDsGenerated()) else if(!rtabmap_.isIDsGenerated())
@@ -874,6 +911,7 @@ void CoreWrapper::process(
UTimer timer; UTimer timer;
if(rtabmap_.isIDsGenerated() || id > 0) if(rtabmap_.isIDsGenerated() || id > 0)
{ {
double timeRtabmap = 0.0;
cv::Mat imageB; cv::Mat imageB;
if(!depthOrRightImage.empty()) if(!depthOrRightImage.empty())
{ {
@@ -924,18 +962,27 @@ void CoreWrapper::process(
if(!rtabmap_.process(data)) if(!rtabmap_.process(data))
{ {
timeRtabmap = timer.ticks();
ROS_WARN("RTAB-Map could not process the data received! (ROS id = %d)", id); ROS_WARN("RTAB-Map could not process the data received! (ROS id = %d)", id);
} }
else else
{ {
timeRtabmap = timer.ticks();
mapToOdomMutex_.lock(); mapToOdomMutex_.lock();
rtabmap_ros::transformToTF(rtabmap_.getMapCorrection(), mapToOdom_); rtabmap_ros::transformToTF(rtabmap_.getMapCorrection(), mapToOdom_);
odomFrameId_ = odomFrameId; odomFrameId_ = odomFrameId;
mapToOdomMutex_.unlock(); mapToOdomMutex_.unlock();
const Statistics & stats = rtabmap_.getStatistics(); // Publish local graph, info
ros::Time timeNow = ros::Time::now(); ros::Time timeNow = ros::Time::now();
this->publishStats(stats, timeNow); this->publishStats(timeNow);
std::map<int, rtabmap::Transform> filteredPoses;
filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(),
cloudMapPub_.getNumSubscribers() != 0,
projMapPub_.getNumSubscribers() != 0,
gridMapPub_.getNumSubscribers() != 0);
this->publishMaps(filteredPoses, timeNow);
// update goal if planning is enabled // update goal if planning is enabled
if(!currentMetricGoal_.isNull()) if(!currentMetricGoal_.isNull())
@@ -982,6 +1029,12 @@ void CoreWrapper::process(
} }
} }
} }
ROS_INFO("rtabmap: Update rate=%fs, Limit=%fs, RTAB-Map=%fs, Publish=%fs (%d local nodes)",
1.0f/rate_,
rtabmap_.getTimeThreshold()/1000.0f,
timeRtabmap,
timer.ticks(),
rtabmap_.getWMSize()+rtabmap_.getSTMSize());
} }
else if(!rtabmap_.isIDsGenerated()) else if(!rtabmap_.isIDsGenerated())
{ {
@@ -991,11 +1044,6 @@ void CoreWrapper::process(
"when you need to have IDs output of RTAB-map synchronised with the source " "when you need to have IDs output of RTAB-map synchronised with the source "
"image sequence ID."); "image sequence ID.");
} }
ROS_INFO("rtabmap: Update rate=%fs, Limit=%fs, Processing time = %fs (%d local nodes)",
1.0f/rate_,
rtabmap_.getTimeThreshold()/1000.0f,
timer.ticks(),
rtabmap_.getWMSize()+rtabmap_.getSTMSize());
} }
void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> > & poses) void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> > & poses)
@@ -1195,7 +1243,7 @@ bool CoreWrapper::setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Em
return true; return true;
} }
bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& rep) bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res)
{ {
ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...", ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...",
req.global?"true":"false", req.global?"true":"false",
@@ -1237,25 +1285,111 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
mapIds, mapIds,
constraints, constraints,
Transform::getIdentity(), Transform::getIdentity(),
rep.data.graph); res.data.graph);
// add data // add data
rep.data.nodes.resize(signatures.size()); res.data.nodes.resize(signatures.size());
int i=0; int i=0;
for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter) for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
{ {
rtabmap_ros::nodeDataToROS(iter->second, rep.data.nodes[i++]); rtabmap_ros::nodeDataToROS(iter->second, res.data.nodes[i++]);
} }
rep.data.header.stamp = ros::Time::now(); res.data.header.stamp = ros::Time::now();
rep.data.header.frame_id = mapFrameId_; res.data.header.frame_id = mapFrameId_;
return true; return true;
} }
bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
{
std::map<int, rtabmap::Transform> filteredPoses;
filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(), false, true, false);
if(filteredPoses.size() && projMaps_.size())
{
// create the projection map
float xMin=0.0f, yMin=0.0f;
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
filteredPoses,
projMaps_,
gridCellSize_,
xMin, yMin,
gridSize_);
if(!pixels.empty())
{
//init
res.map.info.resolution = gridCellSize_;
res.map.info.origin.position.x = 0.0;
res.map.info.origin.position.y = 0.0;
res.map.info.origin.position.z = 0.0;
res.map.info.origin.orientation.x = 0.0;
res.map.info.origin.orientation.y = 0.0;
res.map.info.origin.orientation.z = 0.0;
res.map.info.origin.orientation.w = 1.0;
res.map.info.width = pixels.cols;
res.map.info.height = pixels.rows;
res.map.info.origin.position.x = xMin;
res.map.info.origin.position.y = yMin;
res.map.data.resize(res.map.info.width * res.map.info.height);
memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
res.map.header.frame_id = mapFrameId_;
res.map.header.stamp = ros::Time::now();
return true;
}
}
return false;
}
bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
{
std::map<int, rtabmap::Transform> filteredPoses;
filteredPoses = this->updateMapCaches(rtabmap_.getLocalOptimizedPoses(), false, false, true);
if(filteredPoses.size() && gridMaps_.size())
{
// create the projection map
float xMin=0.0f, yMin=0.0f;
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
filteredPoses,
gridMaps_,
gridCellSize_,
xMin, yMin,
gridSize_);
if(!pixels.empty())
{
//init
res.map.info.resolution = gridCellSize_;
res.map.info.origin.position.x = 0.0;
res.map.info.origin.position.y = 0.0;
res.map.info.origin.position.z = 0.0;
res.map.info.origin.orientation.x = 0.0;
res.map.info.origin.orientation.y = 0.0;
res.map.info.origin.orientation.z = 0.0;
res.map.info.origin.orientation.w = 1.0;
res.map.info.width = pixels.cols;
res.map.info.height = pixels.rows;
res.map.info.origin.position.x = xMin;
res.map.info.origin.position.y = yMin;
res.map.data.resize(res.map.info.width * res.map.info.height);
memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
res.map.header.frame_id = mapFrameId_;
res.map.header.stamp = ros::Time::now();
return true;
}
}
return false;
}
bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtabmap_ros::PublishMap::Response& res) bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtabmap_ros::PublishMap::Response& res)
{ {
if(mapData_.getNumSubscribers() || mapGraph_.getNumSubscribers()) if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
{ {
ROS_INFO("rtabmap: Publishing map..."); ROS_INFO("rtabmap: Publishing map...");
@@ -1264,7 +1398,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
std::multimap<int, Link> constraints; std::multimap<int, Link> constraints;
std::map<int, int> mapIds; std::map<int, int> mapIds;
if(mapData_.getNumSubscribers() == 0 || req.graphOnly) if(mapDataPub_.getNumSubscribers() == 0 || req.graphOnly)
{ {
rtabmap_.getGraph( rtabmap_.getGraph(
poses, poses,
@@ -1291,7 +1425,8 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
} }
rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph); rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph);
graphMsg->header.stamp = ros::Time::now(); ros::Time now = ros::Time::now();
graphMsg->header.stamp = now;
graphMsg->header.frame_id = mapFrameId_; graphMsg->header.frame_id = mapFrameId_;
rtabmap_ros::mapGraphToROS(poses, rtabmap_ros::mapGraphToROS(poses,
@@ -1300,7 +1435,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
Transform::getIdentity(), Transform::getIdentity(),
*graphMsg); *graphMsg);
if(mapData_.getNumSubscribers()) if(mapDataPub_.getNumSubscribers())
{ {
//RGB-D SLAM data //RGB-D SLAM data
rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData); rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
@@ -1315,13 +1450,15 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
rtabmap_ros::nodeDataToROS(iter->second, msg->nodes[i++]); rtabmap_ros::nodeDataToROS(iter->second, msg->nodes[i++]);
} }
mapData_.publish(msg); mapDataPub_.publish(msg);
} }
if(mapGraph_.getNumSubscribers()) if(mapGraphPub_.getNumSubscribers())
{ {
mapGraph_.publish(graphMsg); mapGraphPub_.publish(graphMsg);
} }
publishMaps(poses, now);
} }
else else
{ {
@@ -1338,8 +1475,10 @@ bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ro
return !currentMetricGoal_.isNull(); return !currentMetricGoal_.isNull();
} }
void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp) void CoreWrapper::publishStats(const ros::Time & stamp)
{ {
const rtabmap::Statistics & stats = rtabmap_.getStatistics();
if(infoPub_.getNumSubscribers()) if(infoPub_.getNumSubscribers())
{ {
//ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId()); //ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId());
@@ -1351,7 +1490,7 @@ void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp
infoPub_.publish(msg); infoPub_.publish(msg);
} }
if(mapData_.getNumSubscribers() || mapGraph_.getNumSubscribers()) if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
{ {
if(stats.poses().size() == 0 || stats.poses().size() == stats.getMapIds().size()) if(stats.poses().size() == 0 || stats.poses().size() == stats.getMapIds().size())
{ {
@@ -1366,7 +1505,7 @@ void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp
stats.mapCorrection(), stats.mapCorrection(),
*graphMsg); *graphMsg);
if(mapData_.getNumSubscribers()) if(mapDataPub_.getNumSubscribers())
{ {
//RGB-D SLAM data //RGB-D SLAM data
rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData); rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
@@ -1376,12 +1515,12 @@ void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp
msg->nodes.resize(1); msg->nodes.resize(1);
rtabmap_ros::nodeDataToROS(stats.getSignature(), msg->nodes[0]); rtabmap_ros::nodeDataToROS(stats.getSignature(), msg->nodes[0]);
mapData_.publish(msg); mapDataPub_.publish(msg);
} }
if(mapGraph_.getNumSubscribers()) if(mapGraphPub_.getNumSubscribers())
{ {
mapGraph_.publish(graphMsg); mapGraphPub_.publish(graphMsg);
} }
} }
else else
@@ -1391,6 +1530,392 @@ void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp
} }
} }
std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
const std::map<int, rtabmap::Transform> & poses,
bool updateCloud,
bool updateProj,
bool updateGrid)
{
if(!rtabmap_.getMemory())
{
ROS_FATAL("Memory not initialized!?");
return std::map<int, rtabmap::Transform>();
}
std::map<int, rtabmap::Transform> filteredPoses;
// update cache
if(updateCloud || updateProj || updateGrid)
{
// filter nodes
if(mapFilterRadius_ > 0.0)
{
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
}
else
{
filteredPoses = poses;
}
for(std::map<int, Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
{
if(!iter->second.isNull())
{
Signature data;
bool rgbDepthRequired = updateCloud && !uContains(clouds_, iter->first);
bool depthRequired = updateProj && !uContains(projMaps_, iter->first);
bool scanRequired = updateGrid && !uContains(gridMaps_, iter->first);
if(rgbDepthRequired ||
depthRequired ||
scanRequired)
{
data = rtabmap_.getMemory()->getSignatureDataConst(iter->first);
}
if(data.id() > 0)
{
Transform localTransform = data.getLocalTransform();
if(!localTransform.isNull())
{
// Which data should we decompress?
cv::Mat image, depth, scan;
data.uncompressDataConst(rgbDepthRequired?&image:0, rgbDepthRequired||depthRequired?&depth:0, scanRequired?&scan:0);
if(!depth.empty() &&
depth.type() == CV_8UC1 &&
image.empty() &&
!rgbDepthRequired)
{
// Stereo detected, we should uncompress left image too
data.uncompressDataConst(&image, 0, 0);
}
float fx = data.getDepthFx();
float fy = data.getDepthFy();
float cx = data.getDepthCx();
float cy = data.getDepthCy();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
if(rgbDepthRequired)
{
if(!image.empty() &&
!depth.empty() &&
fx > 0.0f && fy > 0.0f &&
cx >= 0.0f && cy >= 0.0f)
{
if(depth.type() == CV_8UC1)
{
cloudRGB = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloudDecimation_);
}
else
{
cloudRGB = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
}
if(cloudRGB->size() && cloudMaxDepth_ > 0)
{
cloudRGB = util3d::passThrough<pcl::PointXYZRGB>(cloudRGB, "z", 0, cloudMaxDepth_);
}
if(cloudRGB->size() && cloudVoxelSize_ > 0)
{
cloudRGB = util3d::voxelize<pcl::PointXYZRGB>(cloudRGB, cloudVoxelSize_);
}
if(cloudRGB->size())
{
cloudRGB = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudRGB, localTransform);
}
}
else
{
ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first);
}
}
else if(depthRequired)
{
if( !depth.empty() &&
fx > 0.0f && fy > 0.0f &&
cx >= 0.0f && cy >= 0.0f)
{
if(depth.type() == CV_8UC1)
{
if(!image.empty())
{
cv::Mat leftMono;
if(image.channels() == 3)
{
cv::cvtColor(image, leftMono, CV_BGR2GRAY);
}
else
{
leftMono = image;
}
cloudXYZ = rtabmap::util3d::cloudFromDisparity(
util3d::disparityFromStereoImages(leftMono, depth),
cx, cy,
fx, fy,
cloudDecimation_);
}
}
else
{
cloudXYZ = util3d::cloudFromDepth(depth, cx, cy, fx, fy, cloudDecimation_);
}
if(cloudXYZ.get())
{
if(cloudXYZ->size() && cloudMaxDepth_ > 0)
{
cloudXYZ = util3d::passThrough<pcl::PointXYZ>(cloudXYZ, "z", 0, cloudMaxDepth_);
}
if(cloudXYZ->size() && gridCellSize_ > 0)
{
// use gridCellSize since this cloud is only for the projection map
cloudXYZ = util3d::voxelize<pcl::PointXYZ>(cloudXYZ, gridCellSize_);
}
if(cloudXYZ->size())
{
cloudXYZ = util3d::transformPointCloud<pcl::PointXYZ>(cloudXYZ, localTransform);
}
}
else
{
ROS_ERROR("Left stereo image was empty! (node=%d)", iter->first);
}
}
else
{
ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first);
}
}
if(cloudRGB.get())
{
clouds_.insert(std::make_pair(iter->first, cloudRGB));
}
if(depthRequired)
{
cv::Mat ground, obstacles;
if(cloudRGB.get())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloudRGB;
if(cloudClipped->size() && projMaxHeight_ > 0)
{
cloudClipped = util3d::passThrough<pcl::PointXYZRGB>(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
}
if(cloudClipped->size())
{
cloudClipped = util3d::voxelize<pcl::PointXYZRGB>(cloudClipped, gridCellSize_);
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
}
}
else if(cloudXYZ.get())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ;
if(cloudClipped->size() && projMaxHeight_ > 0)
{
cloudClipped = util3d::passThrough<pcl::PointXYZ>(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
}
if(cloudClipped->size())
{
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
}
}
projMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
if(scanRequired)
{
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_);
gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
}
else
{
ROS_ERROR("Local transform detected for node %d", iter->first);
}
}
}
else
{
ROS_ERROR("Pose null for node %d", iter->first);
}
}
// cleanup not used nodes
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=clouds_.begin();
iter!=clouds_.end();)
{
if(!uContains(poses, iter->first))
{
clouds_.erase(iter++);
}
else
{
++iter;
}
}
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=projMaps_.begin();
iter!=projMaps_.end();)
{
if(!uContains(poses, iter->first))
{
projMaps_.erase(iter++);
}
else
{
++iter;
}
}
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=gridMaps_.begin();
iter!=gridMaps_.end();)
{
if(!uContains(poses, iter->first))
{
gridMaps_.erase(iter++);
}
else
{
++iter;
}
}
}
// clear memory if no one subscribed
if(cloudMapPub_.getNumSubscribers() == 0)
{
clouds_.clear();
}
if(projMapPub_.getNumSubscribers() == 0)
{
projMaps_.clear();
}
if(gridMapPub_.getNumSubscribers() == 0)
{
gridMaps_.clear();
}
return filteredPoses;
}
void CoreWrapper::publishMaps(
const std::map<int, rtabmap::Transform> & poses,
const ros::Time & stamp)
{
// publish maps
if(cloudMapPub_.getNumSubscribers())
{
// generate the assembled cloud!
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first);
if(jter != clouds_.end())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud<pcl::PointXYZRGB>(jter->second, iter->second);
*assembledCloud+=*transformed;
}
}
if(assembledCloud->size())
{
if(cloudVoxelSize_ > 0)
{
assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud, cloudVoxelSize_);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*assembledCloud, *cloudMsg);
cloudMsg->header.stamp = stamp;
cloudMsg->header.frame_id = mapFrameId_;
cloudMapPub_.publish(cloudMsg);
}
}
if(projMapPub_.getNumSubscribers())
{
// create the projection map
float xMin=0.0f, yMin=0.0f;
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
poses,
projMaps_,
gridCellSize_,
xMin, yMin,
gridSize_);
if(!pixels.empty())
{
//init
nav_msgs::OccupancyGrid map;
map.info.resolution = gridCellSize_;
map.info.origin.position.x = 0.0;
map.info.origin.position.y = 0.0;
map.info.origin.position.z = 0.0;
map.info.origin.orientation.x = 0.0;
map.info.origin.orientation.y = 0.0;
map.info.origin.orientation.z = 0.0;
map.info.origin.orientation.w = 1.0;
map.info.width = pixels.cols;
map.info.height = pixels.rows;
map.info.origin.position.x = xMin;
map.info.origin.position.y = yMin;
map.data.resize(map.info.width * map.info.height);
memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
map.header.frame_id = mapFrameId_;
map.header.stamp = stamp;
projMapPub_.publish(map);
}
}
if(gridMapPub_.getNumSubscribers())
{
// create the grid map
float xMin=0.0f, yMin=0.0f;
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
poses,
gridMaps_,
gridCellSize_,
xMin, yMin,
gridSize_);
if(!pixels.empty())
{
//init
nav_msgs::OccupancyGrid map;
map.info.resolution = gridCellSize_;
map.info.origin.position.x = 0.0;
map.info.origin.position.y = 0.0;
map.info.origin.position.z = 0.0;
map.info.origin.orientation.x = 0.0;
map.info.origin.orientation.y = 0.0;
map.info.origin.orientation.z = 0.0;
map.info.origin.orientation.w = 1.0;
map.info.width = pixels.cols;
map.info.height = pixels.rows;
map.info.origin.position.x = xMin;
map.info.origin.position.y = yMin;
map.data.resize(map.info.width * map.info.height);
memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
map.header.frame_id = mapFrameId_;
map.header.stamp = stamp;
gridMapPub_.publish(map);
}
}
}
void CoreWrapper::publishCurrentGoal(const ros::Time & stamp) void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
{ {
if(!currentMetricGoal_.isNull()) if(!currentMetricGoal_.isNull())
src/CoreWrapper.h
+33 -4
View File
@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/CameraInfo.h> #include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/LaserScan.h> #include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h> #include <nav_msgs/Odometry.h>
#include <nav_msgs/GetMap.h>
#include <rtabmap/core/Statistics.h> #include <rtabmap/core/Statistics.h>
#include <rtabmap/core/Parameters.h> #include <rtabmap/core/Parameters.h>
@@ -63,6 +64,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/image_transport.h> #include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h> #include <image_transport/subscriber_filter.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <actionlib/client/simple_action_client.h> #include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h> #include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseActionGoal.h> #include <move_base_msgs/MoveBaseActionGoal.h>
@@ -129,7 +133,9 @@ private:
bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& rep); bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&); bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res); bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
@@ -138,7 +144,9 @@ private:
void publishLoop(double tfDelay); void publishLoop(double tfDelay);
void publishStats(const rtabmap::Statistics & stats, const ros::Time & stamp); void publishStats(const ros::Time & stamp);
std::map<int, rtabmap::Transform> updateMapCaches(const std::map<int, rtabmap::Transform> & poses, bool updateCloud, bool updateProj, bool updateGrid);
void publishMaps(const std::map<int, rtabmap::Transform> & poses, const ros::Time & stamp);
void publishCurrentGoal(const ros::Time & stamp); void publishCurrentGoal(const ros::Time & stamp);
void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result); void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
void goalActiveCb(); void goalActiveCb();
@@ -160,12 +168,31 @@ private:
bool waitForTransform_; bool waitForTransform_;
bool useActionForGoal_; bool useActionForGoal_;
// mapping stuff
int cloudDecimation_;
double cloudMaxDepth_;
double cloudVoxelSize_;
double projMaxGroundAngle_;
int projMinClusterSize_;
double projMaxHeight_;
double gridCellSize_;
double gridSize_;
double mapFilterRadius_;
double mapFilterAngle_;
tf::Transform mapToOdom_; tf::Transform mapToOdom_;
boost::mutex mapToOdomMutex_; boost::mutex mapToOdomMutex_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
ros::Publisher infoPub_; ros::Publisher infoPub_;
ros::Publisher mapData_; ros::Publisher mapDataPub_;
ros::Publisher mapGraph_; ros::Publisher mapGraphPub_;
ros::Publisher cloudMapPub_;
ros::Publisher projMapPub_;
ros::Publisher gridMapPub_;
//Planning stuff //Planning stuff
ros::Subscriber goalSub_; ros::Subscriber goalSub_;
@@ -243,6 +270,8 @@ private:
ros::ServiceServer setModeLocalizationSrv_; ros::ServiceServer setModeLocalizationSrv_;
ros::ServiceServer setModeMappingSrv_; ros::ServiceServer setModeMappingSrv_;
ros::ServiceServer getMapDataSrv_; ros::ServiceServer getMapDataSrv_;
ros::ServiceServer getProjMapSrv_;
ros::ServiceServer getGridMapSrv_;
ros::ServiceServer publishMapDataSrv_; ros::ServiceServer publishMapDataSrv_;
ros::ServiceServer setGoalSrv_; ros::ServiceServer setGoalSrv_;
src/GridMapAssemblerNode.cpp
+23 -8
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Compression.h> #include <rtabmap/core/Compression.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h> #include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <nav_msgs/OccupancyGrid.h> #include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetMap.h> #include <nav_msgs/GetMap.h>
#include <std_srvs/Empty.h> #include <std_srvs/Empty.h>
@@ -48,14 +49,12 @@ public:
GridMapAssembler() : GridMapAssembler() :
gridCellSize_(0.05), // meters gridCellSize_(0.05), // meters
mapSize_(0), // meters mapSize_(0), // meters
gridUnknownSpaceFilled_(false),
filterRadius_(0.5), filterRadius_(0.5),
filterAngle_(30.0) // degrees filterAngle_(30.0) // degrees
{ {
ros::NodeHandle pnh("~"); ros::NodeHandle pnh("~");
pnh.param("cell_size", gridCellSize_, gridCellSize_); // m pnh.param("cell_size", gridCellSize_, gridCellSize_); // m
pnh.param("map_size", mapSize_, mapSize_); // m pnh.param("map_size", mapSize_, mapSize_); // m
pnh.param("unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_);
pnh.param("filter_radius", filterRadius_, filterRadius_); pnh.param("filter_radius", filterRadius_, filterRadius_);
pnh.param("filter_angle", filterAngle_, filterAngle_); pnh.param("filter_angle", filterAngle_, filterAngle_);
@@ -78,12 +77,22 @@ public:
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg) void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
{ {
UTimer timer;
for(unsigned int i=0; i<msg->nodes.size(); ++i) for(unsigned int i=0; i<msg->nodes.size(); ++i)
{ {
if(!uContains(scans_, msg->nodes[i].id) && msg->nodes[i].laserScan.size()) if(!uContains(gridMaps_, msg->nodes[i].id) && msg->nodes[i].laserScan.size())
{ {
cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan); cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan);
scans_.insert(std::make_pair(msg->nodes[i].id, util3d::laserScanToPointCloud(laserScan))); if(!laserScan.empty())
{
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(laserScan, ground, obstacles, gridCellSize_);
if(!ground.empty() || !obstacles.empty())
{
gridMaps_.insert(std::make_pair(msg->nodes[i].id, std::make_pair(ground, obstacles)));
}
}
} }
} }
@@ -102,7 +111,13 @@ public:
{ {
// create the map // create the map
float xMin=0.0f, yMin=0.0f; float xMin=0.0f, yMin=0.0f;
cv::Mat pixels = util3d::create2DMap(poses, scans_, gridCellSize_, gridUnknownSpaceFilled_, xMin, yMin, mapSize_); //cv::Mat pixels = util3d::create2DMap(poses, scans_, gridCellSize_, gridUnknownSpaceFilled_, xMin, yMin, mapSize_);
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
poses,
gridMaps_,
gridCellSize_,
xMin, yMin,
mapSize_);
if(!pixels.empty()) if(!pixels.empty())
{ {
@@ -128,6 +143,7 @@ public:
map_.header.stamp = ros::Time::now(); map_.header.stamp = ros::Time::now();
gridMap_.publish(map_); gridMap_.publish(map_);
ROS_INFO("Grid Map published [%d,%d] (%fs)", pixels.cols, pixels.rows, timer.ticks());
} }
} }
} }
@@ -145,7 +161,7 @@ public:
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{ {
ROS_INFO("grid_map_assembler: reset!"); ROS_INFO("grid_map_assembler: reset!");
scans_.clear(); gridMaps_.clear();
map_ = nav_msgs::OccupancyGrid(); map_ = nav_msgs::OccupancyGrid();
return true; return true;
} }
@@ -153,7 +169,6 @@ public:
private: private:
double gridCellSize_; double gridCellSize_;
double mapSize_; double mapSize_;
bool gridUnknownSpaceFilled_;
double filterRadius_; double filterRadius_;
double filterAngle_; double filterAngle_;
@@ -164,7 +179,7 @@ private:
ros::ServiceServer getMapService_; ros::ServiceServer getMapService_;
ros::ServiceServer resetService_; ros::ServiceServer resetService_;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_; std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; //<ground,obstacles>
nav_msgs::OccupancyGrid map_; nav_msgs::OccupancyGrid map_;
}; };
src/MapAssemblerNode.cpp
+19 -9
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Graph.h> #include <rtabmap/core/Graph.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h> #include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl_ros/transforms.h> #include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <nav_msgs/OccupancyGrid.h> #include <nav_msgs/OccupancyGrid.h>
@@ -57,7 +58,6 @@ public:
gridCellSize_(0.05), gridCellSize_(0.05),
groundMaxAngle_(M_PI_4), groundMaxAngle_(M_PI_4),
clusterMinSize_(20), clusterMinSize_(20),
emptyCellFillingRadius_(1),
maxHeight_(0), maxHeight_(0),
occupancyMapSize_(0.0) occupancyMapSize_(0.0)
{ {
@@ -77,12 +77,10 @@ public:
pnh.param("occupancy_cell_size", gridCellSize_, gridCellSize_); pnh.param("occupancy_cell_size", gridCellSize_, gridCellSize_);
pnh.param("occupancy_ground_max_angle", groundMaxAngle_, groundMaxAngle_); pnh.param("occupancy_ground_max_angle", groundMaxAngle_, groundMaxAngle_);
pnh.param("occupancy_cluster_min_size", clusterMinSize_, clusterMinSize_); pnh.param("occupancy_cluster_min_size", clusterMinSize_, clusterMinSize_);
pnh.param("occupancy_empty_filling_radius", emptyCellFillingRadius_, emptyCellFillingRadius_);
pnh.param("occupancy_max_height", maxHeight_, maxHeight_); pnh.param("occupancy_max_height", maxHeight_, maxHeight_);
pnh.param("occupancy_map_size", occupancyMapSize_, occupancyMapSize_); pnh.param("occupancy_map_size", occupancyMapSize_, occupancyMapSize_);
UASSERT(gridCellSize_ > 0); UASSERT(gridCellSize_ > 0);
UASSERT(emptyCellFillingRadius_ >= 0);
UASSERT(maxHeight_ >= 0); UASSERT(maxHeight_ >= 0);
UASSERT(occupancyMapSize_ >=0.0); UASSERT(occupancyMapSize_ >=0.0);
@@ -106,6 +104,7 @@ public:
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg) void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
{ {
UTimer timer;
for(unsigned int i=0; i<msg->nodes.size(); ++i) for(unsigned int i=0; i<msg->nodes.size(); ++i)
{ {
int id = msg->nodes[i].id; int id = msg->nodes[i].id;
@@ -168,15 +167,22 @@ public:
if(computeOccupancyGrid_) if(computeOccupancyGrid_)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloud;
if(maxHeight_ > 0) if(cloudClipped->size() && maxHeight_ > 0)
{ {
cloudClipped = util3d::passThrough<pcl::PointXYZRGB>(cloudClipped, "z", std::numeric_limits<int>::min(), maxHeight_); cloudClipped = util3d::passThrough<pcl::PointXYZRGB>(cloudClipped, "z", std::numeric_limits<int>::min(), maxHeight_);
} }
cv::Mat ground, obstacles; if(cloudClipped->size())
if(util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_))
{ {
occupancyLocalMaps_.insert(std::make_pair(id, std::make_pair(ground, obstacles))); cloudClipped = util3d::voxelize<pcl::PointXYZRGB>(cloudClipped, gridCellSize_);
cv::Mat ground, obstacles;
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_);
if(!ground.empty() || !obstacles.empty())
{
occupancyLocalMaps_.insert(std::make_pair(id, std::make_pair(ground, obstacles)));
}
} }
} }
} }
} }
@@ -276,7 +282,11 @@ public:
{ {
// create the map // create the map
float xMin=0.0f, yMin=0.0f; float xMin=0.0f, yMin=0.0f;
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(poses, occupancyLocalMaps_, gridCellSize_, xMin, yMin, emptyCellFillingRadius_, occupancyMapSize_); cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
poses,
occupancyLocalMaps_,
gridCellSize_, xMin, yMin,
occupancyMapSize_);
if(!pixels.empty()) if(!pixels.empty())
{ {
@@ -305,6 +315,7 @@ public:
occupancyMapPub_.publish(map); occupancyMapPub_.publish(map);
} }
} }
ROS_INFO("Processing data %fs", timer.ticks());
} }
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -332,7 +343,6 @@ private:
double gridCellSize_; double gridCellSize_;
double groundMaxAngle_; double groundMaxAngle_;
int clusterMinSize_; int clusterMinSize_;
int emptyCellFillingRadius_;
double maxHeight_; double maxHeight_;
double occupancyMapSize_; double occupancyMapSize_;