Added lidar_deskewing node and nodelet. Added "deskewing" option for icp_odometry. Updated velodyne and ouster examples with deskewing option. Added velodyne+T265 deskewing example.

src/LidarDeskewingNode.cpp

@@ -0,0 +1,47 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "lidar_deskewing");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_ros/lidar_deskewing", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/MsgConversion.cpp

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Compression.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <eigen_conversions/eigen_msg.h>
 #include <tf_conversions/tf_eigen.h>
@@ -2487,4 +2488,419 @@ bool convertScan3dMsg(
 	return true;
 }
 
+bool deskew_impl(
+		const sensor_msgs::PointCloud2 & input,
+		sensor_msgs::PointCloud2 & output,
+		const std::string & fixedFrameId,
+		tf::TransformListener * listener,
+		double waitForTransform,
+		bool slerp,
+		const rtabmap::Transform & velocity,
+		double previousStamp)
+{
+	if(listener != 0)
+	{
+		if(input.header.frame_id.empty())
+		{
+			ROS_ERROR("Input cloud has empty frame_id!");
+			return false;
+		}
+
+		if(fixedFrameId.empty())
+		{
+			ROS_ERROR("fixedFrameId parameter should be set!");
+			return false;
+		}
+	}
+	else
+	{
+		if(!slerp)
+		{
+			ROS_ERROR("slerp should be true when constant velocity model is used!");
+			return false;
+		}
+
+		if(previousStamp <= 0.0)
+		{
+			ROS_ERROR("previousStamp should be >0 when constant velocity model is used!");
+			return false;
+		}
+
+		if(velocity.isNull())
+		{
+			ROS_ERROR("velocity should be valid when constant velocity model is used!");
+			return false;
+		}
+	}
+
+	int offsetTime = -1;
+	int offsetX = -1;
+	int offsetY = -1;
+	int offsetZ = -1;
+	int timeDatatype = 6;
+	for(size_t i=0; i<input.fields.size(); ++i)
+	{
+		if(input.fields[i].name.compare("t") == 0)
+		{
+			if(offsetTime != -1)
+			{
+				ROS_WARN("The input cloud should have only one of these fields: t, time or stamps. Overriding with %s.", input.fields[i].name.c_str());
+			}
+			offsetTime = input.fields[i].offset;
+			timeDatatype = input.fields[i].datatype;
+		}
+		else if(input.fields[i].name.compare("time") == 0)
+		{
+			if(offsetTime != -1)
+			{
+				ROS_WARN("The input cloud should have only one of these fields: t, time or stamps. Overriding with %s.", input.fields[i].name.c_str());
+			}
+			offsetTime = input.fields[i].offset;
+			timeDatatype = input.fields[i].datatype;
+		}
+		else if(input.fields[i].name.compare("stamps") == 0)
+		{
+			if(offsetTime != -1)
+			{
+				ROS_WARN("The input cloud should have only one of these fields: t, time or stamps. Overriding with %s.", input.fields[i].name.c_str());
+			}
+			offsetTime = input.fields[i].offset;
+			timeDatatype = input.fields[i].datatype;
+		}
+		else if(input.fields[i].name.compare("x") == 0)
+		{
+			ROS_ASSERT(input.fields[i].datatype==7);
+			offsetX = input.fields[i].offset;
+		}
+		else if(input.fields[i].name.compare("y") == 0)
+		{
+			ROS_ASSERT(input.fields[i].datatype==7);
+			offsetY = input.fields[i].offset;
+		}
+		else if(input.fields[i].name.compare("z") == 0)
+		{
+			ROS_ASSERT(input.fields[i].datatype==7);
+			offsetZ = input.fields[i].offset;
+		}
+	}
+
+	if(offsetTime < 0)
+	{
+		ROS_ERROR("Input cloud doesn't have \"t\" or \"time\" field!");
+		return false;
+	}
+	if(offsetX < 0)
+	{
+		ROS_ERROR("Input cloud doesn't have \"x\" field!");
+		return false;
+	}
+	if(offsetY < 0)
+	{
+		ROS_ERROR("Input cloud doesn't have \"y\" field!");
+		return false;
+	}
+	if(offsetZ < 0)
+	{
+		ROS_ERROR("Input cloud doesn't have \"z\" field!");
+		return false;
+	}
+	if(input.height == 0)
+	{
+		ROS_ERROR("Input cloud height is zero!");
+		return false;
+	}
+	if(input.width == 0)
+	{
+		ROS_ERROR("Input cloud width is zero!");
+		return false;
+	}
+
+	bool timeOnColumns = input.width > input.height;
+
+	// Get latest timestamp
+	ros::Time firstStamp;
+	ros::Time lastStamp;
+	if(timeDatatype == 6) // UINT32
+	{
+		unsigned int nsec = *((const unsigned int*)(&input.data[0]+offsetTime));
+		firstStamp = input.header.stamp+ros::Duration(0, nsec);
+		nsec = *((const unsigned int*)(&input.data[timeOnColumns?(input.width-1)*input.point_step:(input.height-1)*input.row_step]+offsetTime));
+		lastStamp = input.header.stamp+ros::Duration(0, nsec);
+	}
+	else if(timeDatatype == 7) // FLOAT32
+	{
+		float sec = *((const float*)(&input.data[0]+offsetTime));
+		firstStamp = input.header.stamp+ros::Duration().fromSec(sec);
+		sec = *((const float*)(&input.data[timeOnColumns?(input.width-1)*input.point_step:(input.height-1)*input.row_step]+offsetTime));
+		lastStamp = input.header.stamp+ros::Duration().fromSec(sec);
+	}
+	else
+	{
+		ROS_ERROR("Not supported time datatype %d!", timeDatatype);
+		return false;
+	}
+
+	if(lastStamp <= firstStamp)
+	{
+		ROS_ERROR("First and last stamps in the scan are the same!");
+		return false;
+	}
+
+	std::string errorMsg;
+	if(listener != 0 &&
+	   waitForTransform>0.0 &&
+	   !listener->waitForTransform(
+			input.header.frame_id,
+			firstStamp,
+			input.header.frame_id,
+			lastStamp,
+			fixedFrameId,
+			ros::Duration(waitForTransform),
+			ros::Duration(0.01),
+			&errorMsg))
+	{
+		ROS_ERROR("Could not estimate motion of %s accordingly to fixed frame %s between stamps %f and %f! (%s)",
+				input.header.frame_id.c_str(),
+				fixedFrameId.c_str(),
+				firstStamp.toSec(),
+				lastStamp.toSec(),
+				errorMsg.c_str());
+		return false;
+	}
+
+	rtabmap::Transform firstPose;
+	rtabmap::Transform lastPose;
+	double scanTime = 0;
+	if(slerp)
+	{
+		if(listener != 0)
+		{
+			firstPose = rtabmap_ros::getTransform(
+					input.header.frame_id,
+					fixedFrameId,
+					firstStamp,
+					input.header.stamp,
+					*listener,
+					0);
+			lastPose = rtabmap_ros::getTransform(
+					input.header.frame_id,
+					fixedFrameId,
+					lastStamp,
+					input.header.stamp,
+					*listener,
+					0);
+		}
+		else
+		{
+			float vx,vy,vz, vroll,vpitch,vyaw;
+			velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+
+			// We need three poses:
+			//  1- The pose of base frame in odom frame at first stamp
+			//  2- The pose of base frame in odom frame at msg stamp
+			//  3- The pose of base frame in odom frame at last stamp
+			UASSERT(firstStamp.toSec() >= previousStamp);
+			UASSERT(lastStamp.toSec() > previousStamp);
+			double dt1 = firstStamp.toSec() - previousStamp;
+			double dt2 = input.header.stamp.toSec() - previousStamp;
+			double dt3 = lastStamp.toSec() - previousStamp;
+
+			rtabmap::Transform p1(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
+			rtabmap::Transform p2(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
+			rtabmap::Transform p3(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3);
+
+			// First and last poses are relative to stamp of the msg
+			firstPose = p2.inverse() * p1;
+			lastPose = p2.inverse() * p3;
+		}
+
+		if(firstPose.isNull())
+		{
+			ROS_ERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
+					input.header.frame_id.c_str(),
+					fixedFrameId.empty()?"velocity":fixedFrameId.c_str(),
+					firstStamp.toSec(),
+					input.header.stamp.toSec());
+			return false;
+		}
+		if(lastPose.isNull())
+		{
+			ROS_ERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
+					input.header.frame_id.c_str(),
+					fixedFrameId.empty()?"velocity":fixedFrameId.c_str(),
+					lastStamp.toSec(),
+					input.header.stamp.toSec());
+			return false;
+		}
+		scanTime = lastStamp.toSec() - firstStamp.toSec();
+	}
+	//else tf will be used to get more accurate transforms
+
+	output = input;
+	ros::Time stamp;
+	UTimer processingTime;
+	if(timeOnColumns)
+	{
+		// ouster point cloud:
+		// t1     t2    ...
+		// ring1  ring1 ...
+		// ring2  ring2 ...
+		// ring3  ring4 ...
+		// ring4  ring3 ...
+		for(size_t u=0; u<output.width; ++u)
+		{
+			if(timeDatatype == 6) // UINT32
+			{
+				unsigned int nsec = *((const unsigned int*)(&output.data[u*output.point_step]+offsetTime));
+				stamp = input.header.stamp+ros::Duration(0, nsec);
+			}
+			else
+			{
+				float sec = *((const float*)(&output.data[u*output.point_step]+offsetTime));
+				stamp = input.header.stamp+ros::Duration().fromSec(sec);
+			}
+
+			rtabmap::Transform transform;
+			if(slerp)
+			{
+				transform = firstPose.interpolate((stamp-firstStamp).toSec() / scanTime, lastPose);
+			}
+			else
+			{
+				transform = rtabmap_ros::getTransform(
+						output.header.frame_id,
+						fixedFrameId,
+						stamp,
+						output.header.stamp,
+						*listener,
+						0);
+				if(transform.isNull())
+				{
+					ROS_ERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
+							output.header.frame_id.c_str(),
+							fixedFrameId.c_str(),
+							stamp.toSec(),
+							output.header.stamp.toSec());
+					return false;
+				}
+			}
+
+			for(size_t v=0; v<input.height; ++v)
+			{
+				unsigned char * dataPtr = &output.data[v*output.row_step + u*output.point_step];
+				float & x = *((float*)(dataPtr+offsetX));
+				float & y = *((float*)(dataPtr+offsetY));
+				float & z = *((float*)(dataPtr+offsetZ));
+				pcl::PointXYZ pt(x,y,z);
+				pt = rtabmap::util3d::transformPoint(pt, transform);
+				x = pt.x;
+				y = pt.y;
+				z = pt.z;
+
+				// set delta stamp to zero so that on downstream they know the cloud is deskewed
+				if(timeDatatype == 6) // UINT32
+				{
+					*((unsigned int*)(dataPtr+offsetTime)) = 0;
+				}
+				else
+				{
+					*((float*)(dataPtr+offsetTime)) = 0;
+				}
+			}
+		}
+	}
+	else // time on rows
+	{
+		// velodyne point cloud:
+		// t1     ring1 ring2 ring3 ring4
+		// t2     ring1 ring2 ring3 ring4
+		// t3     ring1 ring2 ring3 ring4
+		// t4     ring1 ring2 ring3 ring4
+		// ...    ...   ...   ...   ...
+		for(size_t v=0; v<output.height; ++v)
+		{
+			if(timeDatatype == 6) // UINT32
+			{
+				unsigned int nsec = *((const unsigned int*)(&output.data[v*output.row_step]+offsetTime));
+				stamp = input.header.stamp+ros::Duration(0, nsec);
+			}
+			else
+			{
+				float sec = *((const float*)(&output.data[v*output.row_step]+offsetTime));
+				stamp = input.header.stamp+ros::Duration().fromSec(sec);
+			}
+
+			rtabmap::Transform transform;
+			if(slerp)
+			{
+				transform = firstPose.interpolate((stamp-firstStamp).toSec() / scanTime, lastPose);
+			}
+			else
+			{
+				transform = rtabmap_ros::getTransform(
+						output.header.frame_id,
+						fixedFrameId,
+						stamp,
+						output.header.stamp,
+						*listener,
+						0);
+				if(transform.isNull())
+				{
+					ROS_ERROR("Could not get transform of %s accordingly to %s between stamps %f and %f!",
+							output.header.frame_id.c_str(),
+							fixedFrameId.c_str(),
+							stamp.toSec(),
+							output.header.stamp.toSec());
+					return false;
+				}
+			}
+
+			for(size_t u=0; u<input.width; ++u)
+			{
+				unsigned char * dataPtr = &output.data[v*output.row_step + u*output.point_step];
+				float & x = *((float*)(dataPtr+offsetX));
+				float & y = *((float*)(dataPtr+offsetY));
+				float & z = *((float*)(dataPtr+offsetZ));
+				pcl::PointXYZ pt(x,y,z);
+				pt = rtabmap::util3d::transformPoint(pt, transform);
+				x = pt.x;
+				y = pt.y;
+				z = pt.z;
+
+				// set delta stamp to zero so that on downstream they know the cloud is deskewed
+				if(timeDatatype == 6) // UINT32
+				{
+					*((unsigned int*)(dataPtr+offsetTime)) = 0;
+				}
+				else
+				{
+					*((float*)(dataPtr+offsetTime)) = 0;
+				}
+			}
+		}
+	}
+	ROS_DEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
+	return true;
+}
+
+bool deskew(
+		const sensor_msgs::PointCloud2 & input,
+		sensor_msgs::PointCloud2 & output,
+		const std::string & fixedFrameId,
+		tf::TransformListener & listener,
+		double waitForTransform,
+		bool slerp)
+{
+	return deskew_impl(input, output, fixedFrameId, &listener, waitForTransform, slerp, rtabmap::Transform(), 0);
+}
+
+bool deskew(
+		const sensor_msgs::PointCloud2 & input,
+		sensor_msgs::PointCloud2 & output,
+		double previousStamp,
+		const rtabmap::Transform & velocity)
+{
+	return deskew_impl(input, output, "", 0, 0, true, velocity, previousStamp);
+}
+
 }

src/OdometryROS.cpp

@@ -423,6 +423,15 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
 	return transform;
 }
 
+rtabmap::Transform OdometryROS::velocityGuess() const
+{
+	if(odometry_)
+	{
+		return odometry_->getVelocityGuess();
+	}
+	return rtabmap::Transform();
+}
+
 void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 {
 	if(!this->isPaused())

src/nodelets/icp_odometry.cpp

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
 
 #include "rtabmap_ros/MsgConversion.h"
 #include "rtabmap_ros/PluginInterface.h"
@@ -68,6 +69,8 @@ public:
 		scanNormalK_(0),
 		scanNormalRadius_(0.0),
 		scanNormalGroundUp_(0.0),
+		deskewing_(false),
+		deskewingSlerp_(false),
 		plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface"),
 		scanReceived_(false),
 		cloudReceived_(false)
@@ -96,6 +99,8 @@ private:
 		pnh.param("scan_normal_k",   scanNormalK_, scanNormalK_);
 		pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
 		pnh.param("scan_normal_ground_up", scanNormalGroundUp_, scanNormalGroundUp_);
+		pnh.param("deskewing",  deskewing_, deskewing_);
+		pnh.param("deskewing_slerp",  deskewingSlerp_, deskewingSlerp_);
 
 		if (pnh.hasParam("plugins"))
 		{
@@ -142,6 +147,8 @@ private:
 		NODELET_INFO("IcpOdometry: scan_normal_k          = %d", scanNormalK_);
 		NODELET_INFO("IcpOdometry: scan_normal_radius     = %f m", scanNormalRadius_);
 		NODELET_INFO("IcpOdometry: scan_normal_ground_up  = %f", scanNormalGroundUp_);
+		NODELET_INFO("IcpOdometry: deskewing              = %s", deskewing_?"true":"false");
+		NODELET_INFO("IcpOdometry: deskewing_slerp        = %s", deskewingSlerp_?"true":"false");
 
 		scan_sub_ = nh.subscribe("scan", queueSize, &ICPOdometry::callbackScan, this);
 		cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this);
@@ -328,7 +335,7 @@ private:
 		// make sure the frame of the laser is updated too
 		Transform localScanTransform = getTransform(this->frameId(),
 				scanMsg->header.frame_id,
-				scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment));
+				scanMsg->header.stamp);
 		if(localScanTransform.isNull())
 		{
 			ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
@@ -338,7 +345,35 @@ private:
 		//transform in frameId_ frame
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
-		projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
+
+		if(deskewing_ && !guessFrameId().empty())
+		{
+			projection.transformLaserScanToPointCloud(deskewing_&&!guessFrameId().empty()?guessFrameId():scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
+
+			sensor_msgs::PointCloud2 scanOutDeskewed;
+			if(!pcl_ros::transformPointCloud(scanMsg->header.frame_id, scanOut, scanOutDeskewed, this->tfListener()))
+			{
+				ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
+						guessFrameId().c_str(), scanMsg->header.frame_id.c_str(), scanMsg->header.stamp.toSec());
+				return;
+			}
+			scanOut = scanOutDeskewed;
+		}
+		else
+		{
+			projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
+
+			if(previousStamp() > 0 && !velocityGuess().isNull())
+			{
+				// deskew with constant velocity model
+				sensor_msgs::PointCloud2 scanOutDeskewed;
+				if(!deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+				{
+					ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
+					return;
+				}
+			}
+		}
 
 		bool hasIntensity = false;
 		for(unsigned int i=0; i<scanOut.fields.size(); ++i)
@@ -531,6 +566,28 @@ private:
 			cloudMsg = *pointCloudMsg;
 		}
 
+		if(deskewing_)
+		{
+			if(!guessFrameId().empty())
+			{
+				// deskew with TF
+				if(!deskew(*pointCloudMsg, cloudMsg, guessFrameId(), tfListener(), waitForTransformDuration(), deskewingSlerp_))
+				{
+					ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
+					return;
+				}
+			}
+			else if(previousStamp() > 0 && !velocityGuess().isNull())
+			{
+				// deskew with constant velocity model
+				if(!deskew(*pointCloudMsg, cloudMsg, previousStamp(), velocityGuess()))
+				{
+					ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
+					return;
+				}
+			}
+		}
+
 		LaserScan scan;
 		bool hasNormals = false;
 		bool hasIntensity = false;
@@ -760,6 +817,8 @@ private:
 	int scanNormalK_;
 	double scanNormalRadius_;
 	double scanNormalGroundUp_;
+	bool deskewing_;
+	bool deskewingSlerp_;
 	std::vector<boost::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
 	pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
 	bool scanReceived_ = false;

src/nodelets/lidar_deskewing.cpp

@@ -0,0 +1,108 @@
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <laser_geometry/laser_geometry.h>
+
+#include <pcl_ros/transforms.h>
+
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap_ros/MsgConversion.h>
+
+namespace rtabmap_ros
+{
+
+class LidarDeskewing : public nodelet::Nodelet
+{
+public:
+	LidarDeskewing() :
+		waitForTransformDuration_(0.01),
+		slerp_(false),
+		tfListener_(0)
+	{
+	}
+
+	virtual ~LidarDeskewing()
+	{
+	}
+
+private:
+	virtual void onInit()
+	{
+		tfListener_ = new tf::TransformListener();
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
+		pnh.param("wait_for_transform", waitForTransformDuration_, waitForTransformDuration_);
+		pnh.param("slerp", slerp_, slerp_);
+
+		NODELET_INFO("fixed_frame_id:  %s", fixedFrameId_.c_str());
+		NODELET_INFO("wait_for_transform:  %fs", waitForTransformDuration_);
+		NODELET_INFO("slerp:  %s", slerp_?"true":"false");
+
+		if(fixedFrameId_.empty())
+		{
+			NODELET_FATAL("fixed_frame_id parameter cannot be empty!");
+		}
+
+		pubScan_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("input_scan") + "/deskewed", 1);
+		pubCloud_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("input_cloud") + "/deskewed", 1);
+		subScan_ = nh.subscribe("input_scan", 1, &LidarDeskewing::callbackScan, this);
+		subCloud_ = nh.subscribe("input_cloud", 1, &LidarDeskewing::callbackCloud, this);
+	}
+
+	void callbackScan(const sensor_msgs::LaserScanConstPtr & msg)
+	{
+		sensor_msgs::PointCloud2 scanOut;
+		laser_geometry::LaserProjection projection;
+		projection.transformLaserScanToPointCloud(fixedFrameId_, *msg, scanOut, *tfListener_);
+
+		sensor_msgs::PointCloud2 scanOutDeskewed;
+		if(!pcl_ros::transformPointCloud(msg->header.frame_id, scanOut, scanOutDeskewed, *tfListener_))
+		{
+			ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
+					fixedFrameId_.c_str(), msg->header.frame_id.c_str(), msg->header.stamp.toSec());
+			return;
+		}
+		pubScan_.publish(scanOutDeskewed);
+	}
+
+	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & msg)
+	{
+		sensor_msgs::PointCloud2 msgDeskewed;
+		if(deskew(*msg, msgDeskewed, fixedFrameId_, *tfListener_, waitForTransformDuration_, slerp_))
+		{
+			pubCloud_.publish(msgDeskewed);
+		}
+		else
+		{
+			// Just republish the msg to not breakdown downstream
+			// A warning should be already shown (see deskew() source code)
+			ROS_WARN("deskewing failed! returning possible skewed cloud!");
+			pubCloud_.publish(msg);
+		}
+	}
+
+private:
+	ros::Publisher pubScan_;
+	ros::Publisher pubCloud_;
+	ros::Subscriber subScan_;
+	ros::Subscriber subCloud_;
+	std::string fixedFrameId_;
+	double waitForTransformDuration_;
+	bool slerp_;
+	tf::TransformListener * tfListener_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::LidarDeskewing, nodelet::Nodelet);
+}
+
+