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

rtabmap: added local grid output topics for convenience. rviz/MapCloud: added cloud_from_scan option. point_cloud_assembler: added option to skip clouds.

Browse Source
This commit is contained in:
matlabbe
2019-05-02 17:03:04 -04:00
parent e3abe460ee
commit b670186610
5 changed files with 145 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+32
View File
@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <geometry_msgs/PoseArray.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/io.h>
#include <visualization_msgs/MarkerArray.h>
@@ -217,6 +218,9 @@ void CoreWrapper::onInit()
landmarksPub_ = nh.advertise<geometry_msgs::PoseArray>("landmarks", 1);
labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
mapPathPub_ = nh.advertise<nav_msgs::Path>("mapPath", 1);
localGridObstacle_ = nh.advertise<sensor_msgs::PointCloud2>("local_grid_obstacle", 1);
localGridEmpty_ = nh.advertise<sensor_msgs::PointCloud2>("local_grid_empty", 1);
localGridGround_ = nh.advertise<sensor_msgs::PointCloud2>("local_grid_ground", 1);
localizationPosePub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("localization_pose", 1);
initialPoseSub_ = nh.subscribe("initialpose", 1, &CoreWrapper::initialPoseCallback, this);
@@ -3257,6 +3261,34 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
mapGraphPub_.publish(msg);
}
if(localGridObstacle_.getNumSubscribers() && !stats.getLastSignatureData().sensorData().gridObstacleCellsRaw().empty())
{
pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridObstacleCellsRaw()));
sensor_msgs::PointCloud2 msg;
pcl_conversions::moveFromPCL(*cloud, msg);
msg.header.stamp = stamp;
msg.header.frame_id = frameId_;
localGridObstacle_.publish(msg);
}
if(localGridEmpty_.getNumSubscribers() && !stats.getLastSignatureData().sensorData().gridEmptyCellsRaw().empty())
{
pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridEmptyCellsRaw()));
sensor_msgs::PointCloud2 msg;
pcl_conversions::moveFromPCL(*cloud, msg);
msg.header.stamp = stamp;
msg.header.frame_id = frameId_;
localGridEmpty_.publish(msg);
}
if(localGridGround_.getNumSubscribers() && !stats.getLastSignatureData().sensorData().gridGroundCellsRaw().empty())
{
pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridGroundCellsRaw()));
sensor_msgs::PointCloud2 msg;
pcl_conversions::moveFromPCL(*cloud, msg);
msg.header.stamp = stamp;
msg.header.frame_id = frameId_;
localGridGround_.publish(msg);
}
bool pubLabels = labelsPub_.getNumSubscribers();
visualization_msgs::MarkerArray markers;
if((landmarksPub_.getNumSubscribers() || pubLabels) && !stats.poses().empty() && stats.poses().begin()->first < 0)