Merge branch 'master' of github.com:introlab/rtabmap_ros into 0.11.0

Conflicts:
	src/CoreWrapper.cpp
	src/CoreWrapper.h

src/CoreWrapper.cpp

@@ -611,8 +611,8 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 		lastPoseIntermediate_ = false;
 		lastPose_ = odom;
 		lastPoseStamp_ = odomMsg->header.stamp;
-		double transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
-		double rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+		float transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+		float rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
 		if(uIsFinite(rotVariance) && rotVariance > rotVariance_)
 		{
 			rotVariance_ = rotVariance;
@@ -799,6 +799,7 @@ void CoreWrapper::commonDepthCallback(
 		Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
 		if(localTransform.isNull())
 		{
+			ROS_ERROR("TF of received depth image %d at time %fs is not set, aborting rtabmap update.", i, depthMsgs[i]->header.stamp.toSec());
 			return;
 		}
 		// sync with odometry stamp
@@ -809,9 +810,13 @@ void CoreWrapper::commonDepthCallback(
 				Transform sensorT = getTransform(odomFrameId, frameId_, depthMsgs[i]->header.stamp);
 				if(sensorT.isNull())
 				{
-					return;
+					ROS_WARN("Could not get odometry value for depth image %d stamp (%fs). Latest odometry "
+							 "stamp is %fs. The depth image pose will not be synchronized with odometry.", i, depthMsgs[i]->header.stamp.toSec(), lastPoseStamp_.toSec());
+				}
+				else
+				{
+					localTransform = odomT.inverse() * sensorT * localTransform;
 				}
-				localTransform = odomT.inverse() * sensorT * localTransform;
 			}
 		}
 
@@ -896,8 +901,11 @@ void CoreWrapper::commonDepthCallback(
 	if(scan2dMsg.get() != 0)
 	{
 		// make sure the frame of the laser is updated too
-		if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
+		if(getTransform(frameId_,
+				scan2dMsg->header.frame_id,
+				scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull())
 		{
+			ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec());
 			return;
 		}
 
@@ -916,10 +924,14 @@ void CoreWrapper::commonDepthCallback(
 				Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp);
 				if(sensorT.isNull())
 				{
-					return;
+					ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
+							"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scanMsg->header.stamp.toSec(), lastPoseStamp_.toSec());
+				}
+				else
+				{
+					Transform t = odomT.inverse() * sensorT;
+					pclScan = util3d::transformPointCloud(pclScan, t);
 				}
-				Transform t = odomT.inverse() * sensorT;
-				pclScan = util3d::transformPointCloud(pclScan, t);
 
 			}
 		}
@@ -1020,8 +1032,11 @@ void CoreWrapper::commonStereoCallback(
 	if(scan2dMsg.get() != 0)
 	{
 		// make sure the frame of the laser is updated too
-		if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull())
+		if(getTransform(frameId_,
+				scan2dMsg->header.frame_id,
+				scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull())
 		{
+			ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec());
 			return;
 		}
 
@@ -1325,8 +1340,8 @@ void CoreWrapper::process(
 		const SensorData & data,
 		const Transform & odom,
 		const std::string & odomFrameId,
-		double odomRotationalVariance,
-		double odomTransitionalVariance)
+		float odomRotationalVariance,
+		float odomTransitionalVariance)
 {
 	UTimer timer;
 	if(rtabmap_.isIDsGenerated() || data.id() > 0)
@@ -1795,7 +1810,7 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 	rtabmap_ros::mapDataToROS(poses,
 		constraints,
 		signatures,
-		Transform::getIdentity(),
+		mapToOdom_,
 		res.data);
 
 	res.data.header.stamp = ros::Time::now();
@@ -1938,7 +1953,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 			rtabmap_ros::mapDataToROS(poses,
 				constraints,
 				signatures,
-				Transform::getIdentity(),
+				mapToOdom_,
 				*msg);
 
 			mapDataPub_.publish(msg);
@@ -1952,7 +1967,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 			rtabmap_ros::mapGraphToROS(poses,
 				constraints,
-				Transform::getIdentity(),
+				mapToOdom_,
 				*msg);
 
 			mapGraphPub_.publish(msg);

src/CoreWrapper.h

@@ -211,8 +211,8 @@ private:
 			const rtabmap::SensorData & data,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
-			double odomRotationalVariance = 1.0,
-			double odomTransitionalVariance = 1.0);
+			float odomRotationalVariance = 1.0,
+			float odomTransitionalVariance = 1.0);
 
 	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -258,8 +258,8 @@ private:
 	rtabmap::Transform lastPose_;
 	ros::Time lastPoseStamp_;
 	bool lastPoseIntermediate_;
-	double rotVariance_;
-	double transVariance_;
+	float rotVariance_;
+	float transVariance_;
 	rtabmap::Transform currentMetricGoal_;
 	bool latestNodeWasReached_;
 	rtabmap::ParametersMap parameters_;

src/MapsManager.cpp

@@ -34,6 +34,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 		cloudMaxDepth_(4.0), // meters
 		cloudVoxelSize_(0.05), // meters
 		cloudFloorCullingHeight_(0.0),
+		cloudCeilingCullingHeight_(0.0),
 		cloudOutputVoxelized_(false),
 		cloudFrustumCulling_(false),
 		cloudNoiseFilteringRadius_(0.0),
@@ -60,6 +61,14 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 	pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
 	pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
 	pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_);
+	pnh.param("cloud_ceiling_culling_height", cloudCeilingCullingHeight_, cloudCeilingCullingHeight_);
+	if(cloudFloorCullingHeight_ > 0 &&
+	   cloudCeilingCullingHeight_ > 0 &&
+	   cloudCeilingCullingHeight_ < cloudFloorCullingHeight_)
+	{
+		ROS_WARN("\"cloud_floor_culling_height\" should be lower than \"cloud_ceiling_culling_height\", setting \"cloud_ceiling_culling_height\" to 0 (disabled).");
+		cloudCeilingCullingHeight_ = 0;
+	}
 	pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
 	pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_);
 	pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_);
@@ -512,9 +521,11 @@ void MapsManager::publishMaps(
 				}
 			}
 
-			if(assembledCloud->size() && cloudFloorCullingHeight_ > 0.0)
+			if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0))
 			{
-				assembledCloud = util3d::passThrough(assembledCloud, "z", cloudFloorCullingHeight_, 99999.0f);
+				assembledCloud = util3d::passThrough(assembledCloud, "z",
+						cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0,
+						cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0);
 			}
 
 			if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_)

src/MapsManager.h

@@ -70,6 +70,7 @@ private:
 	double cloudMaxDepth_;
 	double cloudVoxelSize_;
 	double cloudFloorCullingHeight_;
+	double cloudCeilingCullingHeight_;
 	bool cloudOutputVoxelized_;
 	bool cloudFrustumCulling_;
 	double cloudNoiseFilteringRadius_;

src/OdometryROS.cpp

@@ -366,6 +366,21 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			odom.pose.covariance.at(28) = info.variance; // pp
 			odom.pose.covariance.at(35) = info.variance; // yawyaw
 
+			//set velocity
+			if(previousStamp_.isValid())
+			{
+				float dt = 1.0f/(stamp - previousStamp_).toSec();
+				float x,y,z,roll,pitch,yaw;
+				odometry_->previousTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				odom.twist.twist.linear.x = x*dt;
+				odom.twist.twist.linear.y = y*dt;
+				odom.twist.twist.linear.z = z*dt;
+				odom.twist.twist.angular.x = roll*dt;
+				odom.twist.twist.angular.y = pitch*dt;
+				odom.twist.twist.angular.z = yaw*dt;
+			}
+			previousStamp_ = stamp;
+
 			//publish the message
 			odomPub_.publish(odom);
 		}
@@ -465,6 +480,7 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	ROS_INFO("visual_odometry: reset odom!");
 	odometry_->reset();
+	previousStamp_ = ros::Time();
 	return true;
 }
 
@@ -473,6 +489,7 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
 	Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
 	ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
 	odometry_->reset(pose);
+	previousStamp_ = ros::Time();
 	return true;
 }
 

src/OdometryROS.h

@@ -101,6 +101,7 @@ private:
 	tf::TransformListener tfListener_;
 
 	bool paused_;
+	ros::Time previousStamp_;
 };
 
 }

src/rviz/MapCloudDisplay.cpp

@@ -156,6 +156,13 @@ MapCloudDisplay::MapCloudDisplay()
 	cloud_filter_floor_height_->setMin( 0.0f );
 	cloud_filter_floor_height_->setMax( 999.0f );
 
+	cloud_filter_ceiling_height_ = new rviz::FloatProperty( "Filter ceiling (m)", 0.0f,
+										 "Filter the ceiling at the specified height set here "
+										 "(only appropriate for 2D mapping).",
+										 this, SLOT( updateCloudParameters() ), this );
+	cloud_filter_ceiling_height_->setMin( 0.0f );
+	cloud_filter_ceiling_height_->setMax( 999.0f );
+
 	node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.2f,
 										 "(Disabled=0) Only keep one node in the specified radius.",
 										 this, SLOT( updateCloudParameters() ), this );
@@ -276,9 +283,11 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 
 					if(cloud->size())
 					{
-						if(cloud_filter_floor_height_->getFloat() > 0.0f)
+						if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
 						{
-							cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
+							cloud = rtabmap::util3d::passThrough(cloud, "z",
+									cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
+									cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
 						}
 
 						sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);

src/rviz/MapCloudDisplay.h

@@ -108,6 +108,7 @@ public:
 	rviz::FloatProperty* cloud_max_depth_;
 	rviz::FloatProperty* cloud_voxel_size_;
 	rviz::FloatProperty* cloud_filter_floor_height_;
+	rviz::FloatProperty* cloud_filter_ceiling_height_;
 	rviz::FloatProperty* node_filtering_radius_;
 	rviz::FloatProperty* node_filtering_angle_;
 	rviz::BoolProperty* download_map_;