point_cloud_assembler: optimized time to assemble final cloud, added circular_buffer option.

CMakeLists.txt

@@ -386,6 +386,10 @@ add_executable(rtabmap_pointcloud_to_depthimage src/PointCloudToDepthImageNode.c
 target_link_libraries(rtabmap_pointcloud_to_depthimage ${Libraries})
 set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "pointcloud_to_depthimage")
 
+add_executable(rtabmap_point_cloud_assembler src/PointCloudAssemblerNode.cpp)
+target_link_libraries(rtabmap_point_cloud_assembler ${Libraries})
+set_target_properties(rtabmap_point_cloud_assembler PROPERTIES OUTPUT_NAME "point_cloud_assembler")
+
 # If rviz is found, add plugins
 IF(rviz_FOUND)
 
@@ -530,6 +534,7 @@ install(TARGETS
    rtabmap_data_player
    rtabmap_odom_msg_to_tf
    rtabmap_pointcloud_to_depthimage
+   rtabmap_point_cloud_assembler
    rtabmap_camera
    rtabmap_rgbd_sync
    rtabmap_rgbd_relay

src/PointCloudAssemblerNode.cpp

@@ -0,0 +1,44 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "point_cloud_assembler");
+
+	nodelet::Loader nodelet;
+	nodelet::V_string nargv;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_ros/point_cloud_assembler", remap, nargv);
+	ros::spin();
+	return 0;
+}
+
+

src/nodelets/point_cloud_assembler.cpp

@@ -73,6 +73,7 @@ public:
 		assemblingTime_(0),
 		skipClouds_(0),
 		cloudsSkipped_(0),
+		circularBuffer_(false),
 		waitForTransformDuration_(0.1),
 		rangeMin_(0),
 		rangeMax_(0),
@@ -107,6 +108,7 @@ private:
 		pnh.param("max_clouds", maxClouds_, maxClouds_);
 		pnh.param("assembling_time", assemblingTime_, assemblingTime_);
 		pnh.param("skip_clouds", skipClouds_, skipClouds_);
+		pnh.param("circular_buffer", circularBuffer_, circularBuffer_);
 		pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
 		pnh.param("range_min", rangeMin_, rangeMin_);
 		pnh.param("range_max", rangeMax_, rangeMax_);
@@ -210,56 +212,59 @@ private:
 			{
 				cloudsSkipped_ = 0;
 
-				sensor_msgs::PointCloud2Ptr cpy(new sensor_msgs::PointCloud2);
+				rtabmap::Transform t = rtabmap_ros::getTransform(
-				*cpy = *cloudMsg;
+						fixedFrameId_, //fromFrame
-				clouds_.push_back(cpy);
+						cloudMsg->header.frame_id, //toFrame
+						cloudMsg->header.stamp,
+						tfListener_,
+						waitForTransformDuration_);
 
-				if(  (int)clouds_.size() >= maxClouds_ && maxClouds_ > 0
+				if(t.isNull())
-					||
+				{
-					 (double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ && assemblingTime_ > 0.0 )
+					ROS_ERROR("Cloud not transform all clouds! Resetting...");
+					clouds_.clear();
+					return;
+				}
+
+				pcl::PCLPointCloud2::Ptr newCloud(new pcl::PCLPointCloud2);
+				if(rangeMin_ > 0.0 || rangeMax_ > 0.0 || voxelSize_ > 0.0f)
+				{
+					pcl_conversions::toPCL(*cloudMsg, *newCloud);
+					rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*newCloud);
+					scan = rtabmap::util3d::commonFiltering(scan, 1, rangeMin_, rangeMax_, voxelSize_);
+					pcl::uint64_t stamp = newCloud->header.stamp;
+					newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, t);
+					newCloud->header.stamp = stamp;
+				}
+				else
+				{
+					sensor_msgs::PointCloud2 output;
+					pcl_ros::transformPointCloud(t.toEigen4f(), *cloudMsg, output);
+					pcl_conversions::toPCL(output, *newCloud);
+				}
+
+				clouds_.push_back(newCloud);
+
+				bool reachedMaxSize =
+						((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
+						||
+						((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<pcl::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
+
+				if( circularBuffer_ || reachedMaxSize )
 				{
 					pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
-					pcl_conversions::toPCL(*clouds_.back(), *assembled);
+					for(std::list<pcl::PCLPointCloud2::Ptr>::iterator iter=clouds_.begin(); iter!=clouds_.end(); ++iter)
-
-					for(size_t i=0; i<clouds_.size()-1; ++i)
 					{
-						rtabmap::Transform t = rtabmap_ros::getTransform(
+						if(assembled->data.empty())
-								clouds_[i]->header.frame_id, //sourceTargetFrame
-								fixedFrameId_, //fixedFrame
-								clouds_[i]->header.stamp, //stampSource
-								clouds_.back()->header.stamp, //stampTarget
-								tfListener_,
-								waitForTransformDuration_);
-
-						if(t.isNull())
 						{
-							ROS_ERROR("Cloud not transform all clouds! Resetting...");
+							*assembled = *(*iter);
-							clouds_.clear();
-							return;
-						}
-
-						pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
-						if(rangeMin_ > 0.0 || rangeMax_ > 0.0)
-						{
-							pcl::PCLPointCloud2 output2;
-							pcl_conversions::toPCL(*clouds_[i], output2);
-							rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(output2);
-							if(rangeMin_ > 0.0 || rangeMax_ > 0.0)
-							{
-								scan = rtabmap::util3d::rangeFiltering(scan, rangeMin_, rangeMax_);
-							}
-							pcl::concatenatePointCloud(*assembled, *rtabmap::util3d::laserScanToPointCloud2(scan, t), *assembledTmp);
 						}
 						else
 						{
-							sensor_msgs::PointCloud2 output;
+							pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
-							pcl_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output);
+							pcl::concatenatePointCloud(*assembled, *(*iter), *assembledTmp);
-							pcl::PCLPointCloud2 output2;
+							assembled = assembledTmp;
-							pcl_conversions::toPCL(output, output2);
-							pcl::concatenatePointCloud(*assembled, output2, *assembledTmp);
 						}
-
-						assembled = assembledTmp;
 					}
 
 					sensor_msgs::PointCloud2 rosCloud;
@@ -270,15 +275,25 @@ private:
 						filter.setInputCloud(assembled);
 						pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
 						filter.filter(*output);
-						pcl_conversions::moveFromPCL(*output, rosCloud);
+						assembled = output;
+					}
+
+					pcl_conversions::moveFromPCL(*assembled, rosCloud);
+					pcl_ros::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
+
+					rosCloud.header = cloudMsg->header;
+					cloudPub_.publish(rosCloud);
+					if(circularBuffer_)
+					{
+						if(reachedMaxSize)
+						{
+							clouds_.pop_front();
+						}
 					}
 					else
 					{
-						pcl_conversions::moveFromPCL(*assembled, rosCloud);
+						clouds_.clear();
 					}
-					rosCloud.header = cloudMsg->header;
-					cloudPub_.publish(rosCloud);
-					clouds_.clear();
 				}
 			}
 			else
@@ -323,6 +338,7 @@ private:
 	int maxClouds_;
 	int skipClouds_;
 	int cloudsSkipped_;
+	bool circularBuffer_;
 	double assemblingTime_;
 	double waitForTransformDuration_;
 	double rangeMin_;
@@ -331,7 +347,7 @@ private:
 	std::string fixedFrameId_;
 	tf::TransformListener tfListener_;
 
-	std::vector<sensor_msgs::PointCloud2Ptr> clouds_;
+	std::list<pcl::PCLPointCloud2::Ptr> clouds_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAssembler, nodelet::Nodelet);