Added input stamps double-verification in some nodelets (data_throttle, stereo_throttle, rgbd_sync, stereo_sync, pointcloud_to_depthimage)

rtabmapviz: added "max_odom_update_rate" parameters
Odom: moved IMU callback from stereo_odometry nodelet to OdometryROS, added "wait_imu_to_init" parameter to initialize odom with IMU first
Fixed fake camera local transform on scan-only callbacks (rtabmap, rtabmapviz)

src/nodelets/data_throttle.cpp

@@ -140,38 +140,10 @@ private:
 
 		last_update_ = ros::Time::now();
 
-		if(imagePub_.getNumSubscribers())
-		{
-			if(decimation_ > 1)
-			{
-				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
-				cv_bridge::CvImage out;
-				out.header = imagePtr->header;
-				out.encoding = imagePtr->encoding;
-				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
-				imagePub_.publish(out.toImageMsg());
-			}
-			else
-			{
-				imagePub_.publish(image);
-			}
-		}
-		if(imageDepthPub_.getNumSubscribers())
-		{
-			if(decimation_ > 1)
-			{
-				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
-				cv_bridge::CvImage out;
-				out.header = imagePtr->header;
-				out.encoding = imagePtr->encoding;
-				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
-				imageDepthPub_.publish(out.toImageMsg());
-			}
-			else
-			{
-				imageDepthPub_.publish(imageDepth);
-			}
-		}
+		double rgbStamp = image->header.stamp.toSec();
+		double depthStamp = imageDepth->header.stamp.toSec();
+		double infoStamp = camInfo->header.stamp.toSec();
+
 		if(infoPub_.getNumSubscribers())
 		{
 			if(decimation_ > 1)
@@ -196,6 +168,52 @@ private:
 				infoPub_.publish(camInfo);
 			}
 		}
+		if(imagePub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imagePub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imagePub_.publish(image);
+			}
+		}
+
+		if(imageDepthPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imageDepthPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageDepthPub_.publish(imageDepth);
+			}
+		}
+
+		if( rgbStamp != image->header.stamp.toSec() ||
+			depthStamp != imageDepth->header.stamp.toSec() ||
+			infoStamp != camInfo->header.stamp.toSec())
+		{
+			NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+					"sure the node publishing the topics doesn't override the same data after publishing them. A "
+					"solution is to use this node within another nodelet manager. Stamps: "
+					"rgb=%f->%f depth=%f->%f info=%f->%f",
+					rgbStamp, image->header.stamp.toSec(),
+					depthStamp, imageDepth->header.stamp.toSec(),
+					infoStamp, camInfo->header.stamp.toSec());
+		}
 	}
 
 	image_transport::Publisher imagePub_;

src/nodelets/pointcloud_to_depthimage.cpp

@@ -144,6 +144,9 @@ private:
 	{
 		if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0)
 		{
+			double cloudStamp = pointCloud2Msg->header.stamp.toSec();
+			double infoStamp = cameraInfoMsg->header.stamp.toSec();
+
 			rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity();
 			if(!fixedFrameId_.empty())
 			{
@@ -221,6 +224,17 @@ private:
 				depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
 				depthImage16Pub_.publish(depthImage.toImageMsg());
 			}
+
+			if( cloudStamp != pointCloud2Msg->header.stamp.toSec() ||
+				infoStamp != cameraInfoMsg->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"cloud=%f->%f info=%f->%f",
+						cloudStamp, pointCloud2Msg->header.stamp.toSec(),
+						infoStamp, cameraInfoMsg->header.stamp.toSec());
+			}
 		}
 	}
 

src/nodelets/rgbd_sync.cpp

@@ -161,6 +161,10 @@ private:
 		callbackCalled_ = true;
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
 		{
+			double rgbStamp = image->header.stamp.toSec();
+			double depthStamp = depth->header.stamp.toSec();
+			double infoStamp = cameraInfo->header.stamp.toSec();
+
 			rtabmap_ros::RGBDImage msg;
 			msg.header.frame_id = cameraInfo->header.frame_id;
 			msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
@@ -204,6 +208,19 @@ private:
 				}
 				rgbdImagePub_.publish(msg);
 			}
+
+			if( rgbStamp != image->header.stamp.toSec() ||
+				depthStamp != depth->header.stamp.toSec() ||
+				infoStamp != cameraInfo->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"rgb=%f->%f depth=%f->%f info=%f->%f",
+						rgbStamp, image->header.stamp.toSec(),
+						depthStamp, depth->header.stamp.toSec(),
+						infoStamp, cameraInfo->header.stamp.toSec());
+			}
 		}
 	}
 

src/nodelets/stereo_odometry.cpp

@@ -38,7 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
-#include <sensor_msgs/Imu.h>
 
 #include <image_geometry/stereo_camera_model.h>
 
@@ -143,14 +142,6 @@ private:
 					cameraInfoRight_.getTopic().c_str());
 		}
 
-		int odomStrategy = 0;
-		Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy);
-		if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF)
-		{
-			imuSub_ = nh.subscribe("imu", queueSize_*5, &StereoOdometry::callbackIMU, this);
-			NODELET_INFO("VIO approach selected, subscribing to IMU topic %s", imuSub_.getTopic().c_str());
-		}
-
 		this->startWarningThread(subscribedTopicsMsg, approxSync);
 	}
 
@@ -329,36 +320,6 @@ private:
 		}
 	}
 
-	void callbackIMU(
-			const sensor_msgs::ImuConstPtr& msg)
-	{
-		if(!this->isPaused())
-		{
-			double stamp = msg->header.stamp.toSec();
-			rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
-			if(this->frameId().compare(msg->header.frame_id) != 0)
-			{
-				localTransform = getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp);
-			}
-			if(localTransform.isNull())
-			{
-				ROS_ERROR("Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF not available at time %f",
-						msg->header.frame_id.c_str(), this->frameId().c_str(), stamp);
-				return;
-			}
-
-			IMU imu(
-					cv::Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z),
-					cv::Mat(3,3,CV_64FC1,(void*)msg->angular_velocity_covariance.data()).clone(),
-					cv::Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z),
-					cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
-					localTransform);
-
-			SensorData data(imu, 0, stamp);
-			this->processData(data, msg->header.stamp);
-		}
-	}
-
 protected:
 	virtual void flushCallbacks()
 	{

src/nodelets/stereo_sync.cpp

@@ -161,6 +161,11 @@ private:
 		callbackCalled_ = true;
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
 		{
+			double leftStamp = imageLeft->header.stamp.toSec();
+			double rightStamp = imageRight->header.stamp.toSec();
+			double leftInfoStamp = cameraInfoLeft->header.stamp.toSec();
+			double rightInfoStamp = cameraInfoRight->header.stamp.toSec();
+
 			rtabmap_ros::RGBDImage msg;
 			msg.header.frame_id = cameraInfoLeft->header.frame_id;
 			msg.header.stamp = imageLeft->header.stamp>imageRight->header.stamp?imageLeft->header.stamp:imageRight->header.stamp;
@@ -186,6 +191,21 @@ private:
 				msg.depth = *imageRight;
 				rgbdImagePub_.publish(msg);
 			}
+
+			if( leftStamp != imageLeft->header.stamp.toSec() ||
+				rightStamp != imageRight->header.stamp.toSec() ||
+				leftInfoStamp != cameraInfoLeft->header.stamp.toSec() ||
+				rightInfoStamp != cameraInfoRight->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"left%f->%f right=%f->%f info_left=%f->%f info_right=%f->%f",
+						leftStamp, imageLeft->header.stamp.toSec(),
+						rightStamp, imageRight->header.stamp.toSec(),
+						leftInfoStamp, cameraInfoLeft->header.stamp.toSec(),
+						rightInfoStamp, cameraInfoRight->header.stamp.toSec());
+			}
 		}
 	}
 

src/nodelets/stereo_throttle.cpp

@@ -138,38 +138,11 @@ private:
 
 		last_update_ = ros::Time::now();
 
-		if(imageLeftPub_.getNumSubscribers())
-		{
-			if(decimation_ > 1)
-			{
-				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
-				cv_bridge::CvImage out;
-				out.header = imagePtr->header;
-				out.encoding = imagePtr->encoding;
-				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
-				imageLeftPub_.publish(out.toImageMsg());
-			}
-			else
-			{
-				imageLeftPub_.publish(imageLeft);
-			}
-		}
-		if(imageRightPub_.getNumSubscribers())
-		{
-			if(decimation_ > 1)
-			{
-				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
-				cv_bridge::CvImage out;
-				out.header = imagePtr->header;
-				out.encoding = imagePtr->encoding;
-				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
-				imageRightPub_.publish(out.toImageMsg());
-			}
-			else
-			{
-				imageRightPub_.publish(imageRight);
-			}
-		}
+		double leftStamp = imageLeft->header.stamp.toSec();
+		double rightStamp = imageRight->header.stamp.toSec();
+		double leftInfoStamp = camInfoLeft->header.stamp.toSec();
+		double rightInfoStamp = camInfoRight->header.stamp.toSec();
+
 		if(infoLeftPub_.getNumSubscribers())
 		{
 			if(decimation_ > 1)
@@ -220,6 +193,55 @@ private:
 				infoRightPub_.publish(camInfoRight);
 			}
 		}
+
+
+		if(imageLeftPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imageLeftPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageLeftPub_.publish(imageLeft);
+			}
+		}
+		if(imageRightPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imageRightPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageRightPub_.publish(imageRight);
+			}
+		}
+
+		if( leftStamp != imageLeft->header.stamp.toSec() ||
+			rightStamp != imageRight->header.stamp.toSec() ||
+			leftInfoStamp != camInfoLeft->header.stamp.toSec() ||
+			rightInfoStamp != camInfoRight->header.stamp.toSec())
+		{
+			NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+					"sure the node publishing the topics doesn't override the same data after publishing them. A "
+					"solution is to use this node within another nodelet manager. Stamps: "
+					"left%f->%f right=%f->%f info_left=%f->%f info_right=%f->%f",
+					leftStamp, imageLeft->header.stamp.toSec(),
+					rightStamp, imageRight->header.stamp.toSec(),
+					leftInfoStamp, camInfoLeft->header.stamp.toSec(),
+					rightInfoStamp, camInfoRight->header.stamp.toSec());
+		}
 	}
 
 	image_transport::Publisher imageLeftPub_;