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

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
+49 -87
View File
@@ -7,7 +7,7 @@ Panels:
- /Global Options1
- /TF1/Frames1
Splitter Ratio: 0.434783
Tree Height: 410
Tree Height: 590
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -82,24 +82,36 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
az3_base_link:
Value: true
az3_odom:
Value: true
base_footprint:
Value: true
base_laser_link:
Value: true
base_link:
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:
Value: true
object_4:
Value: true
object_5:
Value: true
object_6:
Value: true
object_7:
Value: true
object_8:
Value: true
odom:
Value: true
stereo_camera:
Value: true
stereo_camera_base:
Value: true
wheelLB_linkWheel_link:
Value: true
wheelLB_wheel_link:
@@ -128,8 +140,21 @@ Visualization Manager:
base_link:
base_laser_link:
{}
stereo_camera_base:
stereo_camera:
camera_link:
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_wheel_link:
@@ -148,8 +173,8 @@ Visualization Manager:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 3.66965
Min Value: -0.144665
Max Value: -0.574923
Min Value: -5.11672
Value: true
Axis: X
Channel Name: intensity
@@ -185,85 +210,30 @@ Visualization Manager:
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Enabled: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
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
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Value: false
Visual Enabled: true
- Class: rviz/Image
Enabled: true
Image Topic: /camera/data_throttled_image_relay
Enabled: false
Image Topic: /camera/data_throttled_image
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Value: true
Transport Hint: compressed
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
@@ -303,14 +273,6 @@ Visualization Manager:
Name: Info
Topic: /rtabmap/info
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
Global Options:
Background Color: 48; 48; 48
@@ -331,7 +293,7 @@ Visualization Manager:
Views:
Current:
Class: rtabmap_ros/OrbitOriented
Distance: 7.83194
Distance: 7.89956
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
@@ -343,10 +305,10 @@ Visualization Manager:
Z: -0.0896359
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.699796
Pitch: 0.819797
Target Frame: base_link
Value: OrbitOriented (rtabmap)
Yaw: 3.19875
Yaw: 3.14375
Saved: ~
Window Geometry:
Displays:
@@ -356,7 +318,7 @@ Window Geometry:
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002ddfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000229000000dd00fffffffb0000000a0049006d006100670065010000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000463000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002ddfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000000000002dd000000dd00fffffffb0000000a0049006d006100670065000000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000463000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -367,4 +329,4 @@ Window Geometry:
collapsed: false
Width: 1561
X: 42
Y: 108
Y: 107
launch/demo/demo_find_object.launch
+1 -10
View File
@@ -25,7 +25,7 @@
<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. -->
<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/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) -->
@@ -50,15 +50,6 @@
<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 -->
<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 -->
</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 -->
<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"/>
launch/demo/demo_multi-session_mapping.launch
-5
View File
@@ -70,11 +70,6 @@
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</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>
<!-- 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 -->
</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 -->
<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"/>
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 -->
</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 -->
<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"/>
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 <stdio.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/Bool.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/utilite/UEventsManager.h>
@@ -73,6 +76,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
waitForTransform_(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()),
depthSync_(0),
depthScanSync_(0),
@@ -126,14 +139,37 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
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: map_frame_id = %s", mapFrameId_.c_str());
ROS_INFO("rtabmap: queue_size = %d", queueSize);
ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
mapData_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
mapGraph_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 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
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);
setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, 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);
setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
@@ -442,8 +480,7 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
}
else
{
const Statistics & stats = rtabmap_.getStatistics();
this->publishStats(stats, ros::Time::now());
this->publishStats(ros::Time::now());
}
}
else if(!rtabmap_.isIDsGenerated())
@@ -874,6 +911,7 @@ void CoreWrapper::process(
UTimer timer;
if(rtabmap_.isIDsGenerated() || id > 0)
{
double timeRtabmap = 0.0;
cv::Mat imageB;
if(!depthOrRightImage.empty())
{
@@ -924,18 +962,27 @@ void CoreWrapper::process(
if(!rtabmap_.process(data))
{
timeRtabmap = timer.ticks();
ROS_WARN("RTAB-Map could not process the data received! (ROS id = %d)", id);
}
else
{
timeRtabmap = timer.ticks();
mapToOdomMutex_.lock();
rtabmap_ros::transformToTF(rtabmap_.getMapCorrection(), mapToOdom_);
odomFrameId_ = odomFrameId;
mapToOdomMutex_.unlock();
const Statistics & stats = rtabmap_.getStatistics();
// Publish local graph, info
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
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())
{
@@ -991,11 +1044,6 @@ void CoreWrapper::process(
"when you need to have IDs output of RTAB-map synchronised with the source "
"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)
@@ -1195,7 +1243,7 @@ bool CoreWrapper::setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Em
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)...",
req.global?"true":"false",
@@ -1237,25 +1285,111 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
mapIds,
constraints,
Transform::getIdentity(),
rep.data.graph);
res.data.graph);
// add data
rep.data.nodes.resize(signatures.size());
res.data.nodes.resize(signatures.size());
int i=0;
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();
rep.data.header.frame_id = mapFrameId_;
res.data.header.stamp = ros::Time::now();
res.data.header.frame_id = mapFrameId_;
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)
{
if(mapData_.getNumSubscribers() || mapGraph_.getNumSubscribers())
if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
{
ROS_INFO("rtabmap: Publishing map...");
@@ -1264,7 +1398,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
if(mapData_.getNumSubscribers() == 0 || req.graphOnly)
if(mapDataPub_.getNumSubscribers() == 0 || req.graphOnly)
{
rtabmap_.getGraph(
poses,
@@ -1291,7 +1425,8 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
}
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_;
rtabmap_ros::mapGraphToROS(poses,
@@ -1300,7 +1435,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
Transform::getIdentity(),
*graphMsg);
if(mapData_.getNumSubscribers())
if(mapDataPub_.getNumSubscribers())
{
//RGB-D SLAM data
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++]);
}
mapData_.publish(msg);
mapDataPub_.publish(msg);
}
if(mapGraph_.getNumSubscribers())
if(mapGraphPub_.getNumSubscribers())
{
mapGraph_.publish(graphMsg);
mapGraphPub_.publish(graphMsg);
}
publishMaps(poses, now);
}
else
{
@@ -1338,8 +1475,10 @@ bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ro
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())
{
//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);
}
if(mapData_.getNumSubscribers() || mapGraph_.getNumSubscribers())
if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
{
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(),
*graphMsg);
if(mapData_.getNumSubscribers())
if(mapDataPub_.getNumSubscribers())
{
//RGB-D SLAM data
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);
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
@@ -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)
{
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/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/GetMap.h>
#include <rtabmap/core/Statistics.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/subscriber_filter.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseActionGoal.h>
@@ -129,7 +133,9 @@ private:
bool triggerNewMapCallback(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 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 setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
@@ -138,7 +144,9 @@ private:
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 goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
void goalActiveCb();
@@ -160,12 +168,31 @@ private:
bool waitForTransform_;
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_;
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 mapData_;
ros::Publisher mapGraph_;
ros::Publisher mapDataPub_;
ros::Publisher mapGraphPub_;
ros::Publisher cloudMapPub_;
ros::Publisher projMapPub_;
ros::Publisher gridMapPub_;
//Planning stuff
ros::Subscriber goalSub_;
@@ -243,6 +270,8 @@ private:
ros::ServiceServer setModeLocalizationSrv_;
ros::ServiceServer setModeMappingSrv_;
ros::ServiceServer getMapDataSrv_;
ros::ServiceServer getProjMapSrv_;
ros::ServiceServer getGridMapSrv_;
ros::ServiceServer publishMapDataSrv_;
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/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetMap.h>
#include <std_srvs/Empty.h>
@@ -48,14 +49,12 @@ public:
GridMapAssembler() :
gridCellSize_(0.05), // meters
mapSize_(0), // meters
gridUnknownSpaceFilled_(false),
filterRadius_(0.5),
filterAngle_(30.0) // degrees
{
ros::NodeHandle pnh("~");
pnh.param("cell_size", gridCellSize_, gridCellSize_); // m
pnh.param("map_size", mapSize_, mapSize_); // m
pnh.param("unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_);
pnh.param("filter_radius", filterRadius_, filterRadius_);
pnh.param("filter_angle", filterAngle_, filterAngle_);
@@ -78,12 +77,22 @@ public:
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
{
UTimer timer;
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);
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
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())
{
@@ -128,6 +143,7 @@ public:
map_.header.stamp = ros::Time::now();
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&)
{
ROS_INFO("grid_map_assembler: reset!");
scans_.clear();
gridMaps_.clear();
map_ = nav_msgs::OccupancyGrid();
return true;
}
@@ -153,7 +169,6 @@ public:
private:
double gridCellSize_;
double mapSize_;
bool gridUnknownSpaceFilled_;
double filterRadius_;
double filterAngle_;
@@ -164,7 +179,7 @@ private:
ros::ServiceServer getMapService_;
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_;
};
src/MapAssemblerNode.cpp
+17 -7
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Graph.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <nav_msgs/OccupancyGrid.h>
@@ -57,7 +58,6 @@ public:
gridCellSize_(0.05),
groundMaxAngle_(M_PI_4),
clusterMinSize_(20),
emptyCellFillingRadius_(1),
maxHeight_(0),
occupancyMapSize_(0.0)
{
@@ -77,12 +77,10 @@ public:
pnh.param("occupancy_cell_size", gridCellSize_, gridCellSize_);
pnh.param("occupancy_ground_max_angle", groundMaxAngle_, groundMaxAngle_);
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_map_size", occupancyMapSize_, occupancyMapSize_);
UASSERT(gridCellSize_ > 0);
UASSERT(emptyCellFillingRadius_ >= 0);
UASSERT(maxHeight_ >= 0);
UASSERT(occupancyMapSize_ >=0.0);
@@ -106,6 +104,7 @@ public:
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
{
UTimer timer;
for(unsigned int i=0; i<msg->nodes.size(); ++i)
{
int id = msg->nodes[i].id;
@@ -168,16 +167,23 @@ public:
if(computeOccupancyGrid_)
{
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_);
}
if(cloudClipped->size())
{
cloudClipped = util3d::voxelize<pcl::PointXYZRGB>(cloudClipped, gridCellSize_);
cv::Mat ground, obstacles;
if(util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_))
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
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())
{
@@ -305,6 +315,7 @@ public:
occupancyMapPub_.publish(map);
}
}
ROS_INFO("Processing data %fs", timer.ticks());
}
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -332,7 +343,6 @@ private:
double gridCellSize_;
double groundMaxAngle_;
int clusterMinSize_;
int emptyCellFillingRadius_;
double maxHeight_;
double occupancyMapSize_;