CoreWrapper: forward odom twist to rtabmap library

include/rtabmap_ros/CoreWrapper.h

@@ -195,6 +195,7 @@ private:
 			const ros::Time & stamp,
 			rtabmap::SensorData & data,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
+			const std::vector<float> & odomVelocity = std::vector<float>(),
 			const std::string & odomFrameId = "",
 			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
 			const rtabmap::OdometryInfo & odomInfo = rtabmap::OdometryInfo(),
@@ -260,6 +261,7 @@ private:
 	bool paused_;
 	rtabmap::Transform lastPose_;
 	ros::Time lastPoseStamp_;
+	std::vector<float> lastPoseVelocity_;
 	bool lastPoseIntermediate_;
 	cv::Mat covariance_;
 	rtabmap::Transform currentMetricGoal_;

src/nodelets/point_cloud_assembler.cpp

@@ -42,7 +42,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/point_cloud2_iterator.h>
 
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/exact_time.h>
@@ -51,7 +50,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
-#include <rtabmap/core/Version.h>
+
+//ADC
+#include <sensor_msgs/Range.h>
 
 namespace rtabmap_ros
 {
@@ -77,17 +78,16 @@ public:
 		skipClouds_(0),
 		cloudsSkipped_(0),
 		circularBuffer_(false),
-		linearUpdate_(0),
-		angularUpdate_(0),
 		waitForTransformDuration_(0.1),
 		rangeMin_(0),
 		rangeMax_(0),
 		voxelSize_(0),
 		noiseRadius_(0),
 		noiseMinNeighbors_(5),
-		removeZ_(false),
 		fixedFrameId_("odom"),
-		frameId_("")
+		frameId_(""),
+		//ADC
+		use_lidar_topics_(false)
 	{}
 
 	virtual ~PointCloudAssembler()
@@ -119,16 +119,20 @@ private:
 		pnh.param("assembling_time", assemblingTime_, assemblingTime_);
 		pnh.param("skip_clouds", skipClouds_, skipClouds_);
 		pnh.param("circular_buffer", circularBuffer_, circularBuffer_);
-		pnh.param("linear_update", linearUpdate_, linearUpdate_);
-		pnh.param("angular_update", angularUpdate_, angularUpdate_);
 		pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
 		pnh.param("range_min", rangeMin_, rangeMin_);
 		pnh.param("range_max", rangeMax_, rangeMax_);
 		pnh.param("voxel_size", voxelSize_, voxelSize_);
 		pnh.param("noise_radius", noiseRadius_, noiseRadius_);
 		pnh.param("noise_min_neighbors", noiseMinNeighbors_, noiseMinNeighbors_);
-		pnh.param("remove_z", removeZ_, removeZ_);
 		pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
+		ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);
+		//ADC
+		pnh.param("use_lidar_topics", use_lidar_topics_, use_lidar_topics_);
+	    std::vector<std::string> lidar_topics;
+  		pnh.param("lidar_topics", lidar_topics, lidar_topics);
+		lidarSub_.resize(lidar_topics.size());
+
 
 		ROS_INFO("%s: queue_size=%d", getName().c_str(), queueSize);
 		ROS_INFO("%s: fixed_frame_id=%s", getName().c_str(), fixedFrameId_.c_str());
@@ -137,31 +141,40 @@ private:
 		ROS_INFO("%s: assembling_time=%fs", getName().c_str(), assemblingTime_);
 		ROS_INFO("%s: skip_clouds=%d", getName().c_str(), skipClouds_);
 		ROS_INFO("%s: circular_buffer=%s", getName().c_str(), circularBuffer_?"true":"false");
-		ROS_INFO("%s: linear_update=%f m", getName().c_str(), linearUpdate_);
-		ROS_INFO("%s: angular_update=%f rad", getName().c_str(), angularUpdate_);
 		ROS_INFO("%s: wait_for_transform_duration=%f", getName().c_str(), waitForTransformDuration_);
 		ROS_INFO("%s: range_min=%f", getName().c_str(), rangeMin_);
 		ROS_INFO("%s: range_max=%f", getName().c_str(), rangeMax_);
 		ROS_INFO("%s: voxel_size=%fm", getName().c_str(), voxelSize_);
 		ROS_INFO("%s: noise_radius=%fm", getName().c_str(), noiseRadius_);
 		ROS_INFO("%s: noise_min_neighbors=%d", getName().c_str(), noiseMinNeighbors_);
-		ROS_INFO("%s: remove_z=%s", getName().c_str(), removeZ_?"true":"false");
+		//ADC
-
+		ROS_INFO("%s: use_lidar_topics=%d", getName().c_str(), use_lidar_topics_);
-		if(maxClouds_==0 && assemblingTime_ ==0.0)
-		{
-			ROS_ERROR("point_cloud_assembler: max_cloud or assembling_time parameters should be set!");
-			exit(-1);
-		}
 
 		cloudsSkipped_ = skipClouds_;
 
 		std::string subscribedTopicsMsg;
 		if(!fixedFrameId_.empty())
 		{
-			cloudSub_ = nh.subscribe("cloud", queueSize, &PointCloudAssembler::callbackCloud, this);
+			if(!use_lidar_topics_)
-			subscribedTopicsMsg = uFormat("\n%s subscribed to %s",
+			{
-								getName().c_str(),
+				ROS_INFO("ADC: fixedFrameId != empty");
-								cloudSub_.getTopic().c_str());
+				cloudSub_ = nh.subscribe("cloud", queueSize, &PointCloudAssembler::callbackCloud, this);
+				subscribedTopicsMsg = uFormat("\n%s subscribed to %s",
+									getName().c_str(),
+									cloudSub_.getTopic().c_str());
+			}
+			else
+			{
+				if (lidar_topics.size() == 0)
+				{
+					ROS_WARN("Missing lidar topics!");
+				}
+				for (int i = 0; i < lidar_topics.size(); ++i)
+				{
+					ROS_INFO("Subscribing to lidar_topics `%s`", lidar_topics[i].c_str());
+					lidarSub_[i] = nh.subscribe(lidar_topics[i], queueSize, &PointCloudAssembler::callbackLidar, this);  
+				}
+			}
 		}
 		else if(subscribeOdomInfo)
 		{
@@ -241,50 +254,29 @@ private:
 		}
 	}
 
-	sensor_msgs::PointCloud2 removeField(const sensor_msgs::PointCloud2 & input, const std::string & field)
+	void callbackLidar(const sensor_msgs::RangeConstPtr & lidarMsg)
 	{
-		sensor_msgs::PointCloud2 output;
+		if(lidarMsg->range > 0.1)
-		int offset = 0;
-		std::vector<int> inputFieldIndex;
-		for(size_t i=0; i<input.fields.size(); ++i)
 		{
-			if(input.fields[i].name.compare(field) == 0)
+			callbackCalled_ = true;
-			{
+
-				continue;
+			//transform lidar to PC
-			}
+			pcl::PointCloud<pcl::PointXYZ>* pcl_cloud = new pcl::PointCloud<pcl::PointXYZ>;
-			else
+			pcl_cloud->push_back (pcl::PointXYZ (lidarMsg->range, 0, 0));
-			{
+
-				sensor_msgs::PointField outputField = input.fields[i];
+			//const sensor_msgs::PointCloudConstPtr pcl_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-				outputField.offset = offset;
+			//pcl::PointCloud<pcl::PointXYZ>::Ptr assembledGround_(pcl);
-				offset += outputField.count * sizeOfPointField(outputField.datatype);
+			sensor_msgs::PointCloud2::Ptr pc2(new sensor_msgs::PointCloud2);
-				output.fields.push_back(outputField);
+
-				inputFieldIndex.push_back(i);
+			pcl::toROSMsg(*pcl_cloud, *pc2);
-			}
+			pc2->header = lidarMsg->header;
+			callbackCloud(pc2);
 		}
-		output.header = input.header;
+
-		output.height = input.height;
-		output.width = input.width;
-		output.is_bigendian = input.is_bigendian;
-		output.is_dense = input.is_dense;
-		output.point_step = offset;
-		output.row_step = output.width * output.point_step;
-		output.data.resize(output.height*output.row_step);
-		int total = output.height*output.width;
-		for(int i=0; i<total; ++i)
-		{
-			// for each point, copy fields
-			int oi = i*output.point_step;
-			int pi = i*input.point_step;
-			for(size_t j=0;j<output.fields.size(); ++j)
-			{
-				memcpy(&output.data[oi + output.fields[j].offset],
-					   &input.data[pi + input.fields[inputFieldIndex[j]].offset],
-					   output.fields[j].count * sizeOfPointField(output.fields[j].datatype));
-			}
-		}
-		return output;
 	}
 
+
+
 	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
 	{
 		if(cloudPub_.getNumSubscribers())
@@ -295,36 +287,20 @@ private:
 			if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
 			{
 				cloudsSkipped_ = 0;
-
+				rtabmap::Transform t = rtabmap_ros::getTransform(
-				rtabmap::Transform pose = rtabmap_ros::getTransform(
 						fixedFrameId_, //fromFrame
 						cloudMsg->header.frame_id, //toFrame
 						cloudMsg->header.stamp,
-						tfListener_,
+						*tfListener_,
 						waitForTransformDuration_);
 
-				if(pose.isNull())
+				if(t.isNull())
 				{
-					ROS_ERROR("Cloud not transform all clouds! Resetting...");
+					ROS_WARN("Cloud not use transform! Ignoring...");
-					clouds_.clear();
+					//clouds_.clear();    //comment out to prevent PC reset
 					return;
 				}
 
-				bool isMoving = true;
-				if(!previousPose_.isNull() && (linearUpdate_>0 || angularUpdate_>0))
-				{
-					rtabmap::Transform delta = previousPose_.inverse()*pose;
-					float roll, pitch, yaw;
-					delta.getEulerAngles(roll, pitch, yaw);
-					isMoving = fabs(delta.x()) > linearUpdate_ ||
-									fabs(delta.y()) > linearUpdate_ ||
-									fabs(delta.z()) > linearUpdate_ ||
-									(angularUpdate_>0.0f && (
-										fabs(roll) > angularUpdate_ ||
-										fabs(pitch) > angularUpdate_ ||
-										fabs(yaw) > angularUpdate_));
-				}
-
 				pcl::PCLPointCloud2::Ptr newCloud(new pcl::PCLPointCloud2);
 				if(rangeMin_ > 0.0 || rangeMax_ > 0.0 || voxelSize_ > 0.0f)
 				{
@@ -336,13 +312,13 @@ private:
 #else
 					pcl::uint64_t stamp = newCloud->header.stamp;
 #endif
-					newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, pose);
+					newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, t);
 					newCloud->header.stamp = stamp;
 				}
 				else
 				{
 					sensor_msgs::PointCloud2 output;
-					pcl_ros::transformPointCloud(pose.toEigen4f(), *cloudMsg, output);
+					pcl_ros::transformPointCloud(t.toEigen4f(), *cloudMsg, output);
 					pcl_conversions::toPCL(output, *newCloud);
 				}
 
@@ -392,52 +368,12 @@ private:
 					sensor_msgs::PointCloud2 rosCloud;
 					if(voxelSize_>0.0)
 					{
-						// estimate if there would be an overflow
+						pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
-						int x_idx=-1, y_idx=-1, z_idx=-1;
+						filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
-						for (std::size_t d = 0; d < assembled->fields.size (); ++d)
+						filter.setInputCloud(assembled);
-						{
+						pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
-							if (assembled->fields[d].name.compare("x")==0)
+						filter.filter(*output);
-								x_idx = d;
+						assembled = output;
-							if (assembled->fields[d].name.compare("y")==0)
-								y_idx = d;
-							if (assembled->fields[d].name.compare("z")==0)
-								z_idx = d;
-						}
-						bool overflow = false;
-						if(x_idx>=0 && y_idx>=0 && z_idx>=0) {
-							Eigen::Vector4f min_p, max_p;
-							pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p);
-							float inverseVoxelSize = 1.0f/voxelSize_;
-							std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
-							std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
-							std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
-
-							if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
-							{
-								overflow = true;
-							}
-						}
-						if(overflow)
-						{
-							rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled);
-							scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_);
-#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
-							std::uint64_t stamp = assembled->header.stamp;
-#else
-							pcl::uint64_t stamp = assembled->header.stamp;
-#endif
-							assembled = rtabmap::util3d::laserScanToPointCloud2(scan);
-							assembled->header.stamp = stamp;
-						}
-						else
-						{
-							pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
-							filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
-							filter.setInputCloud(assembled);
-							pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
-							filter.filter(*output);
-							assembled = output;
-						}
 					}
 					if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
 					{
@@ -451,7 +387,6 @@ private:
 					}
 
 					pcl_conversions::moveFromPCL(*assembled, rosCloud);
-					rtabmap::Transform t = pose;
 					if(!frameId_.empty())
 					{
 						// transform in target frame_id instead of sensor frame
@@ -459,57 +394,41 @@ private:
 								fixedFrameId_, //fromFrame
 								frameId_, //toFrame
 								cloudMsg->header.stamp,
-								tfListener_,
+								*tfListener_,
 								waitForTransformDuration_);
 						if(t.isNull())
 						{
-							ROS_ERROR("Cloud not transform back assembled clouds in target frame \"%s\"! Resetting...", frameId_.c_str());
+							ROS_ERROR("Cloud not transform back assembled clouds in target frame \"%s\"! Ignoring...", frameId_.c_str());
-							clouds_.clear();
+							//clouds_.clear();   // don;t clear the cloud
 							return;
 						}
 					}
 					pcl_ros::transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
 
-					if(removeZ_)
-					{
-						rosCloud = removeField(rosCloud, "z");
-					}
 
 					rosCloud.header = cloudMsg->header;
 					if(!frameId_.empty())
 					{
 						rosCloud.header.frame_id = frameId_;
 					}
-					cloudPub_.publish(rosCloud);
+					ros::Duration rate_sleep = ros::Duration(0.1);
+					if((rosCloud.header.stamp - cloudPub_timestamp) > rate_sleep )
+					{
+						cloudPub_timestamp = rosCloud.header.stamp;
+						cloudPub_.publish(rosCloud);
+					}
 					if(circularBuffer_)
 					{
-						if(!isMoving)
+						if(reachedMaxSize)
 						{
-							clouds_.pop_back();
+							clouds_.pop_front();
-						}
-						else
-						{
-							previousPose_ = pose;
-							if(reachedMaxSize)
-							{
-								clouds_.pop_front();
-							}
 						}
 					}
 					else
 					{
 						clouds_.clear();
-						previousPose_.setNull();
 					}
 				}
-				else if(!isMoving)
-				{
-					clouds_.pop_back();
-				}
-				else
-				{
-					previousPose_ = pose;
-				}
 			}
 			else
 			{
@@ -554,8 +473,6 @@ private:
 	int skipClouds_;
 	int cloudsSkipped_;
 	bool circularBuffer_;
-	double linearUpdate_;
-	double angularUpdate_;
 	double assemblingTime_;
 	double waitForTransformDuration_;
 	double rangeMin_;
@@ -563,13 +480,19 @@ private:
 	double voxelSize_;
 	double noiseRadius_;
 	int noiseMinNeighbors_;
-	bool removeZ_;
 	std::string fixedFrameId_;
 	std::string frameId_;
-	tf::TransformListener tfListener_;
+	//tf::TransformListener tfListener_;
-	rtabmap::Transform previousPose_;
+	ros::Duration not_time = ros::Duration(1000);
+	tf::TransformListener* tfListener_  = new tf::TransformListener(not_time, true);
 
 	std::list<pcl::PCLPointCloud2::Ptr> clouds_;
+
+	//ADC
+	bool use_lidar_topics_;
+	std::vector<ros::Subscriber> lidarSub_;
+	sensor_msgs::PointCloud2 LidarCloudMsg;
+	ros::Time cloudPub_timestamp = ros::Time(0.0);
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAssembler, nodelet::Nodelet);