point_cloud_aggregator: added fixed_frame_id to adjust clouds when the robot is moving (accordingly to odometry). odom: added guess_min_time parameter (used to force odom to publish odom msg at this rate in case the guess is not changing).

src/OdometryROS.cpp

@@ -67,6 +67,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	guessFrameId_(""),
 	guessMinTranslation_(0.0),
 	guessMinRotation_(0.0),
+	guessMinTime_(0.0),
 	publishTf_(true),
 	waitForTransform_(true),
 	waitForTransformDuration_(0.1), // 100 ms
@@ -147,6 +148,7 @@ void OdometryROS::onInit()
 	pnh.param("guess_frame_id", guessFrameId_, guessFrameId_); // odometry guess frame
 	pnh.param("guess_min_translation", guessMinTranslation_, guessMinTranslation_);
 	pnh.param("guess_min_rotation", guessMinRotation_, guessMinRotation_);
+	pnh.param("guess_min_time", guessMinTime_, guessMinTime_);
 
 	pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_);
 
@@ -172,6 +174,7 @@ void OdometryROS::onInit()
 	NODELET_INFO("Odometry: guess_frame_id         = %s", guessFrameId_.c_str());
 	NODELET_INFO("Odometry: guess_min_translation  = %f", guessMinTranslation_);
 	NODELET_INFO("Odometry: guess_min_rotation     = %f", guessMinRotation_);
+	NODELET_INFO("Odometry: guess_min_time         = %f", guessMinTime_);
 	NODELET_INFO("Odometry: expected_update_rate   = %f Hz", expectedUpdateRate_);
 	NODELET_INFO("Odometry: wait_imu_to_init       = %s", waitIMUToinit_?"true":"false");
 
@@ -556,7 +559,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 				float x,y,z,roll,pitch,yaw;
 				guess_.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 				if((guessMinTranslation_ <= 0.0 || uMax3(fabs(x), fabs(y), fabs(z)) < guessMinTranslation_) &&
-				   (guessMinRotation_ <= 0.0 || uMax3(fabs(roll), fabs(pitch), fabs(yaw)) < guessMinRotation_))
+				   (guessMinRotation_ <= 0.0 || uMax3(fabs(roll), fabs(pitch), fabs(yaw)) < guessMinRotation_) &&
+				   (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && stamp.toSec()-previousStamp_ < guessMinTime_)))
 				{
 					// Ignore odometry update, we didn't move enough
 					if(publishTf_)

src/nodelets/point_cloud_aggregator.cpp

@@ -6,6 +6,7 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
+#include <pcl/io/pcd_io.h>
 
 #include <pcl_ros/transforms.h>
 
@@ -68,6 +69,7 @@ private:
 		bool approx=true;
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
 		pnh.param("approx_sync", approx, approx);
 		pnh.param("count", count, count);
 
@@ -198,17 +200,51 @@ private:
 
 			for(unsigned int i=1; i<cloudMsgs.size(); ++i)
 			{
+				rtabmap::Transform cloudDisplacement;
+				bool notsync = false;
+				if(!fixedFrameId_.empty() &&
+				   cloudMsgs[0]->header.stamp != cloudMsgs[i]->header.stamp)
+				{
+					// approx sync
+					cloudDisplacement = rtabmap_ros::getTransform(
+							frameId, //sourceTargetFrame
+							fixedFrameId_, //fixedFrame
+							cloudMsgs[i]->header.stamp, //stampSource
+							cloudMsgs[0]->header.stamp, //stampTarget
+							tfListener_,
+							0.1);
+					notsync = true;
+				}
+
 				pcl::PCLPointCloud2 cloud2;
 				if(frameId.compare(cloudMsgs[i]->header.frame_id) != 0)
 				{
 					sensor_msgs::PointCloud2 tmp;
 					pcl_ros::transformPointCloud(frameId, *cloudMsgs[i], tmp, tfListener_);
-					pcl_conversions::toPCL(tmp, cloud2);
+					if(!cloudDisplacement.isNull())
+					{
+						sensor_msgs::PointCloud2 tmp2;
+						pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
+						pcl_conversions::toPCL(tmp2, cloud2);
+					}
+					else
+					{
+						pcl_conversions::toPCL(tmp, cloud2);
+					}
+
 				}
 				else
 				{
-					pcl_conversions::toPCL(*cloudMsgs[i], cloud2);
-					frameId = cloudMsgs[i]->header.frame_id;
+					if(!cloudDisplacement.isNull())
+					{
+						sensor_msgs::PointCloud2 tmp;
+						pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
+						pcl_conversions::toPCL(tmp, cloud2);
+					}
+					else
+					{
+						pcl_conversions::toPCL(*cloudMsgs[i], cloud2);
+					}
 				}
 
 				pcl::PCLPointCloud2 tmp_output;
@@ -266,6 +302,7 @@ private:
 	ros::Publisher cloudPub_;
 
 	std::string frameId_;
+	std::string fixedFrameId_;
 	tf::TransformListener tfListener_;
 };