Updated demo_hector_mapping.launch with max_range, pm and p2n options. Odometry: added postProcessData() function (used by icp_odometry to republish filtered scan after odometry processing)

include/rtabmap_ros/OdometryROS.h

@@ -57,7 +57,7 @@ public:
 	OdometryROS(bool stereoParams, bool visParams, bool icpParams);
 	virtual ~OdometryROS();
 
-	void processData(const rtabmap::SensorData & data, const ros::Time & stamp, const std::string & sensorFrameId);
+	void processData(rtabmap::SensorData & data, const std_msgs::Header & header);
 
 	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&);
@@ -80,6 +80,7 @@ protected:
 
 	virtual void flushCallbacks() = 0;
 	tf::TransformListener & tfListener() {return tfListener_;}
+	virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {}
 
 private:
 	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
@@ -143,7 +144,7 @@ private:
 	bool waitIMUToinit_;
 	bool imuProcessed_;
 	std::map<double, rtabmap::IMU> imus_;
-	std::pair<rtabmap::SensorData, std::pair<ros::Time, std::string> > bufferedData_;
+	std::pair<rtabmap::SensorData, std_msgs::Header > bufferedData_;
 };
 
 }

launch/demo/demo_hector_mapping.launch

@@ -1,4 +1,4 @@
-
+<!-- -->
 <launch>
 
   <!-- HECTOR MAPPING VERSION: use this with ROS bag demo_mapping_no_odom.bag generated  -->
@@ -22,8 +22,14 @@
   <!-- Example with camera or not -->
   <arg name="camera" default="true" />
   
-  <!-- Example with camera or not -->
+  <!-- Limit lidar range if > 0 (has effect only when hector:=false) -->
+  <arg name="max_range" default="0" />
   
+  <!-- Point to Plane ICP? (has effect only when hector:=false) -->
+  <arg name="p2n" default="true" />
+  
+  <!-- Use libpointmatcher for ICP? (has effect only when hector:=false) -->
+  <arg name="pm" default="true" />
 
   <param name="use_sim_time" type="bool" value="True"/>
   
@@ -67,13 +73,18 @@
      <param if="$(arg odom_guess)" name="odom_frame_id"   type="string" value="icp_odom"/>
      <param if="$(arg odom_guess)" name="guess_frame_id"  type="string" value="odom"/>
      
-     <param name="Icp/PointToPlane"  type="string" value="true"/>
      <param name="Icp/VoxelSize"     type="string" value="0.05"/>
+     <param name="Icp/RangeMax"      type="string" value="$(arg max_range)"/>
      <param name="Icp/Epsilon"       type="string" value="0.001"/>
-     <param name="Icp/PointToPlaneK"  type="string" value="5"/>
+     <param unless="$(arg odom_guess)" name="Icp/MaxTranslation" type="string" value="0"/> <!-- can be set to reject large ICP jumps -->
-     <param name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
+     <param if="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="true"/>
+     <param if="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="5"/>
+     <param if="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
+     <param unless="$(arg p2n)" name="Icp/PointToPlane"  type="string" value="false"/>
+     <param unless="$(arg p2n)" name="Icp/PointToPlaneK"  type="string" value="0"/>
+     <param unless="$(arg p2n)" name="Icp/PointToPlaneRadius"  type="string" value="0"/>
      <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
-     <param name="Icp/PM"             type="string" value="true"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
+     <param name="Icp/PM"             type="string" value="$(arg pm)"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
      <param name="Icp/PMOutlierRatio" type="string" value="0.85"/>
      <param name="Odom/Strategy"        type="string" value="0"/>
      <param name="Odom/GuessMotion"     type="string" value="true"/>
@@ -116,6 +127,8 @@
       <param name="RGBD/ProximityBySpace"    type="string" value="true"/>
       <param name="Icp/CorrespondenceRatio"  type="string" value="0.2"/>
       <param name="Icp/VoxelSize"            type="string" value="0.05"/>
+      <param name="Icp/RangeMax"             type="string" value="$(arg max_range)"/>
+      <param name="Grid/RangeMax"            type="string" value="$(arg max_range)"/>
     </node>
     
     <!-- Visualisation RTAB-Map -->

src/OdometryROS.cpp

@@ -450,7 +450,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 		{
 			SensorData data = bufferedData_.first;
 			bufferedData_.first = SensorData();
-			processData(data, bufferedData_.second.first, bufferedData_.second.second);
+			processData(data, bufferedData_.second);
 		}
 
 		if(imus_.size() > 1000)
@@ -460,7 +460,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 	}
 }
 
-void OdometryROS::processData(const SensorData & data, const ros::Time & stamp, const std::string & sensorFrameId)
+void OdometryROS::processData(SensorData & data, const std_msgs::Header & header)
 {
 	if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty())
 	{
@@ -468,7 +468,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 		return;
 	}
 
-	if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first<stamp.toSec()))
+	if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec()))
 	{
 		//NODELET_WARN("No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", stamp.toSec());
 
@@ -481,12 +481,11 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 					bufferedData_.first.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
 		}
 		bufferedData_.first = data;
-		bufferedData_.second.first = stamp;
+		bufferedData_.second = header;
-		bufferedData_.second.second = sensorFrameId;
 		return;
 	}
 	// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
-	std::map<double, rtabmap::IMU>::iterator iterEnd = imus_.lower_bound(stamp.toSec());
+	std::map<double, rtabmap::IMU>::iterator iterEnd = imus_.lower_bound(header.stamp.toSec());
 	if(iterEnd!= imus_.end())
 	{
 		++iterEnd;
@@ -505,15 +504,15 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 	Transform groundTruth;
 	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 	{
-		if(previousStamp_>0.0 && previousStamp_ >= stamp.toSec())
+		if(previousStamp_>0.0 && previousStamp_ >= header.stamp.toSec())
 		{
 			NODELET_WARN("Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). New stamp should be always greater than previous stamp. This new data is ignored. This message will appear only once.",
-					previousStamp_, stamp.toSec());
+					previousStamp_, header.stamp.toSec());
 			return;
 		}
 		else if(maxUpdateRate_ > 0 &&
 				previousStamp_ > 0 &&
-				(stamp.toSec()-previousStamp_+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_)
+				(header.stamp.toSec()-previousStamp_+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_)
 		{
 			// throttling
 			return;
@@ -521,16 +520,16 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 		else if(maxUpdateRate_ == 0 &&
 				expectedUpdateRate_ > 0 &&
 			    previousStamp_ > 0 &&
-			    (stamp.toSec()-previousStamp_) < 1.0/expectedUpdateRate_)
+			    (header.stamp.toSec()-previousStamp_) < 1.0/expectedUpdateRate_)
 		{
 			NODELET_WARN("Odometry: Aborting odometry update, higher frame rate detected (%f Hz) than the expected one (%f Hz). (stamps: previous=%fs new=%fs)",
-					1.0/(stamp.toSec()-previousStamp_), expectedUpdateRate_, previousStamp_, stamp.toSec());
+					1.0/(header.stamp.toSec()-previousStamp_), expectedUpdateRate_, previousStamp_, header.stamp.toSec());
 			return;
 		}
 
 		if(!groundTruthFrameId_.empty())
 		{
-			groundTruth = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
+			groundTruth = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, header.stamp);
 
 			if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 			{
@@ -560,7 +559,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 	Transform guessCurrentPose;
 	if(!guessFrameId_.empty())
 	{
-		guessCurrentPose = this->getTransform(guessFrameId_, frameId_, stamp);
+		guessCurrentPose = this->getTransform(guessFrameId_, frameId_, header.stamp);
 
 		Transform previousPose = guessPreviousPose_;
 		if(guessPreviousPose_.isNull())
@@ -589,7 +588,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 				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_) &&
-				   (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && stamp.toSec()-previousStamp_ < guessMinTime_)))
+				   (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && header.stamp.toSec()-previousStamp_ < guessMinTime_)))
 				{
 					// Ignore odometry update, we didn't move enough
 					if(publishTf_)
@@ -597,7 +596,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 						geometry_msgs::TransformStamped correctionMsg;
 						correctionMsg.child_frame_id = guessFrameId_;
 						correctionMsg.header.frame_id = odomFrameId_;
-						correctionMsg.header.stamp = stamp;
+						correctionMsg.header.stamp = header.stamp;
 						Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
 						rtabmap_ros::transformToGeometryMsg(correction, correctionMsg.transform);
 						tfBroadcaster_.sendTransform(correctionMsg);
@@ -618,12 +617,11 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 	// process data
 	ros::WallTime time = ros::WallTime::now();
 	rtabmap::OdometryInfo info;
-	SensorData dataCpy = data;
 	if(!groundTruth.isNull())
 	{
-		dataCpy.setGroundTruth(groundTruth);
+		data.setGroundTruth(groundTruth);
 	}
-	rtabmap::Transform pose = odometry_->process(dataCpy, guess_, &info);
+	rtabmap::Transform pose = odometry_->process(data, guess_, &info);
 	if(!pose.isNull())
 	{
 		guess_.setNull();
@@ -635,7 +633,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 		geometry_msgs::TransformStamped poseMsg;
 		poseMsg.child_frame_id = frameId_;
 		poseMsg.header.frame_id = odomFrameId_;
-		poseMsg.header.stamp = stamp;
+		poseMsg.header.stamp = header.stamp;
 		rtabmap_ros::transformToGeometryMsg(pose, poseMsg.transform);
 
 		if(publishTf_)
@@ -646,7 +644,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 				geometry_msgs::TransformStamped correctionMsg;
 				correctionMsg.child_frame_id = guessFrameId_;
 				correctionMsg.header.frame_id = odomFrameId_;
-				correctionMsg.header.stamp = stamp;
+				correctionMsg.header.stamp = header.stamp;
 				Transform correction = pose * guessCurrentPose.inverse();
 				rtabmap_ros::transformToGeometryMsg(correction, correctionMsg.transform);
 				tfBroadcaster_.sendTransform(correctionMsg);
@@ -661,7 +659,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 		{
 			//next, we'll publish the odometry message over ROS
 			nav_msgs::Odometry odom;
-			odom.header.stamp = stamp; // use corresponding time stamp to image
+			odom.header.stamp = header.stamp; // use corresponding time stamp to image
 			odom.header.frame_id = odomFrameId_;
 			odom.child_frame_id = frameId_;
 
@@ -725,7 +723,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 			}
 			sensor_msgs::PointCloud2 cloudMsg;
 			pcl::toROSMsg(cloud, cloudMsg);
-			cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
+			cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 			cloudMsg.header.frame_id = odomFrameId_;
 			odomLocalMap_.publish(cloudMsg);
 		}
@@ -748,7 +746,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 
 					sensor_msgs::PointCloud2 cloudMsg;
 					pcl::toROSMsg(cloud, cloudMsg);
-					cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
+					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_.publish(cloudMsg);
 				}
@@ -768,7 +766,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 					}
 					sensor_msgs::PointCloud2 cloudMsg;
 					pcl::toROSMsg(cloud, cloudMsg);
-					cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
+					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_.publish(cloudMsg);
 				}
@@ -799,7 +797,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 				pcl::toROSMsg(*cloud, cloudMsg);
 			}
 
-			cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
+			cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 			cloudMsg.header.frame_id = odomFrameId_;
 			odomLocalScanMap_.publish(cloudMsg);
 		}
@@ -814,7 +812,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 
 		//send null pose to notify that odometry is lost
 		nav_msgs::Odometry odom;
-		odom.header.stamp = stamp; // use corresponding time stamp to image
+		odom.header.stamp = header.stamp; // use corresponding time stamp to image
 		odom.header.frame_id = odomFrameId_;
 		odom.child_frame_id = frameId_;
 		odom.pose.covariance.at(0) = BAD_COVARIANCE;  // xx
@@ -842,7 +840,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 		if(resetCurrentCount_ == 0)
 		{
 			// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
-			Transform tfPose = this->getTransform(odomFrameId_, frameId_, stamp);
+			Transform tfPose = this->getTransform(odomFrameId_, frameId_, header.stamp);
 			if(tfPose.isNull())
 			{
 				NODELET_WARN( "Odometry automatically reset to latest computed pose!");
@@ -862,19 +860,18 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 	{
 		rtabmap_ros::OdomInfo infoMsg;
 		odomInfoToROS(info, infoMsg);
-		infoMsg.header.stamp = stamp; // use corresponding time stamp to image
+		infoMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 		infoMsg.header.frame_id = odomFrameId_;
 		odomInfoPub_.publish(infoMsg);
 	}
 
 	if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers())
 	{
-		if(!sensorFrameId.empty())
+		if(!header.frame_id.empty())
 		{
 			rtabmap_ros::RGBDImage msg;
-			rtabmap_ros::rgbdImageToROS(dataCpy, msg, sensorFrameId);
+			rtabmap_ros::rgbdImageToROS(data, msg, header.frame_id);
-			msg.header.stamp = stamp; // use corresponding time stamp to image
+			msg.header = header; // use corresponding time stamp to image
-			msg.header.frame_id = sensorFrameId;
 			odomRgbdImagePub_.publish(msg);
 		}
 		else
@@ -883,6 +880,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 		}
 	}
 
+	postProcessData(data, header);
+
 	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 	{
 		if(visParams_)
@@ -900,7 +899,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp,
 		{
 			NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
 		}
-		previousStamp_ = stamp.toSec();
+		previousStamp_ = header.stamp.toSec();
 	}
 }
 

src/nodelets/icp_odometry.cpp

@@ -414,21 +414,6 @@ private:
 					pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
 					scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
 				}
-
-				if(filtered_scan_pub_.getNumSubscribers())
-				{
-					sensor_msgs::PointCloud2 msg;
-					if(hasIntensity)
-					{
-						pcl::toROSMsg(*pclScanINormal, msg);
-					}
-					else
-					{
-						pcl::toROSMsg(*pclScanNormal, msg);
-					}
-					msg.header = scanMsg->header;
-					filtered_scan_pub_.publish(msg);
-				}
 			}
 			else
 			{
@@ -440,28 +425,18 @@ private:
 				{
 					scan = util3d::laserScan2dFromPointCloud(*pclScan);
 				}
-
-				if(filtered_scan_pub_.getNumSubscribers())
-				{
-					sensor_msgs::PointCloud2 msg;
-					if(hasIntensity)
-					{
-						pcl::toROSMsg(*pclScanI, msg);
-					}
-					else
-					{
-						pcl::toROSMsg(*pclScan, msg);
-					}
-					msg.header = scanMsg->header;
-					filtered_scan_pub_.publish(msg);
-				}
 			}
 		}
 
+		if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
+		{
+			scan = util3d::rangeFiltering(scan, scanRangeMin_, scanRangeMax_);
+		}
+
 		rtabmap::SensorData data(
 				LaserScan(scan,
 						maxLaserScans,
-						scanMsg->range_max,
+						scanRangeMax_>0&&scanRangeMax_<scanMsg->range_max?scanRangeMax_:scanMsg->range_max,
 						localScanTransform),
 				cv::Mat(),
 				cv::Mat(),
@@ -469,7 +444,7 @@ private:
 				0,
 				rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
 
-		this->processData(data, scanMsg->header.stamp, "");
+		this->processData(data, scanMsg->header);
 	}
 
 	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
@@ -583,13 +558,6 @@ private:
 				}
 			}
 			scan = util3d::laserScanFromPointCloud(*pclScan);
-			if(filtered_scan_pub_.getNumSubscribers())
-			{
-				sensor_msgs::PointCloud2 msg;
-				pcl::toROSMsg(*pclScan, msg);
-				msg.header = cloudMsg.header;
-				filtered_scan_pub_.publish(msg);
-			}
 		}
 		else if(hasNormals)
 		{
@@ -608,13 +576,6 @@ private:
 				}
 			}
 			scan = util3d::laserScanFromPointCloud(*pclScan);
-			if(filtered_scan_pub_.getNumSubscribers())
-			{
-				sensor_msgs::PointCloud2 msg;
-				pcl::toROSMsg(*pclScan, msg);
-				msg.header = cloudMsg.header;
-				filtered_scan_pub_.publish(msg);
-			}
 		}
 		else if(hasIntensity)
 		{
@@ -653,26 +614,10 @@ private:
 					pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
 					pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
 					scan = util3d::laserScanFromPointCloud(*pclScanNormal);
-
-					if(filtered_scan_pub_.getNumSubscribers())
-					{
-						sensor_msgs::PointCloud2 msg;
-						pcl::toROSMsg(*pclScanNormal, msg);
-						msg.header = cloudMsg.header;
-						filtered_scan_pub_.publish(msg);
-					}
 				}
 				else
 				{
 					scan = util3d::laserScanFromPointCloud(*pclScan);
-
-					if(filtered_scan_pub_.getNumSubscribers())
-					{
-						sensor_msgs::PointCloud2 msg;
-						pcl::toROSMsg(*pclScan, msg);
-						msg.header = cloudMsg.header;
-						filtered_scan_pub_.publish(msg);
-					}
 				}
 			}
 		}
@@ -713,26 +658,10 @@ private:
 					pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
 					pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
 					scan = util3d::laserScanFromPointCloud(*pclScanNormal);
-
-					if(filtered_scan_pub_.getNumSubscribers())
-					{
-						sensor_msgs::PointCloud2 msg;
-						pcl::toROSMsg(*pclScanNormal, msg);
-						msg.header = cloudMsg.header;
-						filtered_scan_pub_.publish(msg);
-					}
 				}
 				else
 				{
 					scan = util3d::laserScanFromPointCloud(*pclScan);
-
-					if(filtered_scan_pub_.getNumSubscribers())
-					{
-						sensor_msgs::PointCloud2 msg;
-						pcl::toROSMsg(*pclScan, msg);
-						msg.header = cloudMsg.header;
-						filtered_scan_pub_.publish(msg);
-					}
 				}
 			}
 		}
@@ -758,7 +687,7 @@ private:
 				0,
 				rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));
 
-		this->processData(data, cloudMsg.header.stamp, cloudMsg.header.frame_id);
+		this->processData(data, cloudMsg.header);
 	}
 
 protected:
@@ -767,6 +696,17 @@ protected:
 		// flush callbacks
 	}
 
+	void postProcessData(const SensorData & data, const std_msgs::Header & header) const
+	{
+		if(filtered_scan_pub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 msg;
+			pcl_conversions::fromPCL(*rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw()), msg);
+			msg.header = header;
+			filtered_scan_pub_.publish(msg);
+		}
+	}
+
 private:
 	ros::Subscriber scan_sub_;
 	ros::Subscriber cloud_sub_;

src/nodelets/rgbd_odometry.cpp

@@ -447,7 +447,10 @@ private:
 				0,
 				rtabmap_ros::timestampFromROS(higherStamp));
 
-		this->processData(data, higherStamp, rgbImages.size()==1?rgbImages[0]->header.frame_id:"");
+		std_msgs::Header header;
+		header.stamp = higherStamp;
+		header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
+		this->processData(data, header);
 	}
 
 	void callback(

src/nodelets/rgbdicp_odometry.cpp

@@ -418,7 +418,10 @@ private:
 						0,
 						rtabmap_ros::timestampFromROS(stamp));
 
-				this->processData(data, stamp, image->header.frame_id);
+				std_msgs::Header header;
+				header.stamp = stamp;
+				header.frame_id = image->header.frame_id;
+				this->processData(data, header);
 			}
 		}
 	}

src/nodelets/stereo_odometry.cpp

@@ -280,7 +280,10 @@ private:
 						0,
 						rtabmap_ros::timestampFromROS(stamp));
 
-				this->processData(data, stamp, imageRectLeft->header.frame_id);
+				std_msgs::Header header;
+				header.stamp = stamp;
+				header.frame_id = imageRectLeft->header.frame_id;
+				this->processData(data, header);
 			}
 			else
 			{
@@ -422,7 +425,10 @@ private:
 						0,
 						rtabmap_ros::timestampFromROS(stamp));
 
-				this->processData(data, stamp, image->header.frame_id);
+				std_msgs::Header header;
+				header.stamp = stamp;
+				header.frame_id = image->header.frame_id;
+				this->processData(data, header);
 			}
 			else
 			{