From 44bbaa2cefd3e976648baa8cbd8ba7df4b90b5af Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 16 Oct 2022 21:36:11 -0700 Subject: [PATCH] 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. --- CMakeLists.txt | 5 + include/rtabmap_ros/MsgConversion.h | 14 + include/rtabmap_ros/OdometryROS.h | 4 + launch/demo/demo_hector_mapping.launch | 1 + launch/rtabmap.launch | 21 +- launch/tests/test_ouster.launch | 9 +- launch/tests/test_ouster_gen2.launch | 35 +- launch/tests/test_velodyne.launch | 40 +- .../tests/test_velodyne_t265_deskewing.launch | 184 ++++++++ nodelet_plugins.xml | 8 + src/LidarDeskewingNode.cpp | 47 ++ src/MsgConversion.cpp | 416 ++++++++++++++++++ src/OdometryROS.cpp | 9 + src/nodelets/icp_odometry.cpp | 63 ++- src/nodelets/lidar_deskewing.cpp | 108 +++++ 15 files changed, 930 insertions(+), 34 deletions(-) create mode 100644 launch/tests/test_velodyne_t265_deskewing.launch create mode 100644 src/LidarDeskewingNode.cpp create mode 100644 src/nodelets/lidar_deskewing.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f67488e3..4d74966b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -263,6 +263,7 @@ SET(rtabmap_plugins_lib_src src/nodelets/undistort_depth.cpp src/nodelets/imu_to_tf.cpp src/nodelets/rgbdx_sync.cpp + src/nodelets/lidar_deskewing.cpp ) IF(${cv_bridge_VERSION_MAJOR} GREATER 1 OR ${cv_bridge_VERSION_MINOR} GREATER 10) @@ -386,6 +387,10 @@ add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp) target_link_libraries(rtabmap_imu_to_tf ${Libraries}) set_target_properties(rtabmap_imu_to_tf PROPERTIES OUTPUT_NAME "imu_to_tf") +add_executable(rtabmap_lidar_deskewing src/LidarDeskewingNode.cpp) +target_link_libraries(rtabmap_lidar_deskewing ${Libraries}) +set_target_properties(rtabmap_lidar_deskewing PROPERTIES OUTPUT_NAME "lidar_deskewing") + IF(NOT WIN32) add_executable(rtabmap_wifi_signal_pub src/WifiSignalPubNode.cpp) target_link_libraries(rtabmap_wifi_signal_pub rtabmap_ros) diff --git a/include/rtabmap_ros/MsgConversion.h b/include/rtabmap_ros/MsgConversion.h index 6de25b860..5d55f765e 100644 --- a/include/rtabmap_ros/MsgConversion.h +++ b/include/rtabmap_ros/MsgConversion.h @@ -264,6 +264,20 @@ bool convertScan3dMsg( int maxPoints = 0, float maxRange = 0.0f); +bool deskew( + const sensor_msgs::PointCloud2 & input, + sensor_msgs::PointCloud2 & output, + const std::string & fixedFrameId, + tf::TransformListener & listener, + double waitForTransform, + bool slerp = false); + +bool deskew( + const sensor_msgs::PointCloud2 & input, + sensor_msgs::PointCloud2 & output, + double previousStamp, + const rtabmap::Transform & velocity); + } #endif /* MSGCONVERSION_H_ */ diff --git a/include/rtabmap_ros/OdometryROS.h b/include/rtabmap_ros/OdometryROS.h index bbae32aad..f505d1cf9 100644 --- a/include/rtabmap_ros/OdometryROS.h +++ b/include/rtabmap_ros/OdometryROS.h @@ -70,6 +70,7 @@ class OdometryROS : public nodelet::Nodelet const std::string & frameId() const {return frameId_;} const std::string & odomFrameId() const {return odomFrameId_;} + const std::string & guessFrameId() const {return guessFrameId_;} const rtabmap::ParametersMap & parameters() const {return parameters_;} bool isPaused() const {return paused_;} rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; @@ -80,6 +81,9 @@ class OdometryROS : public nodelet::Nodelet virtual void flushCallbacks() = 0; tf::TransformListener & tfListener() {return tfListener_;} + double waitForTransformDuration() const {return waitForTransform_?waitForTransformDuration_:0.0;} + rtabmap::Transform velocityGuess() const; + double previousStamp() const {return previousStamp_;} virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {} private: diff --git a/launch/demo/demo_hector_mapping.launch b/launch/demo/demo_hector_mapping.launch index edf0c8456..529fc97ba 100644 --- a/launch/demo/demo_hector_mapping.launch +++ b/launch/demo/demo_hector_mapping.launch @@ -69,6 +69,7 @@ + diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch index 28ba85053..4c6709c4e 100644 --- a/launch/rtabmap.launch +++ b/launch/rtabmap.launch @@ -96,8 +96,10 @@ + + - + @@ -312,6 +314,17 @@ + + + + + + + + + + + @@ -430,10 +443,10 @@ - + - - + + diff --git a/launch/tests/test_ouster.launch b/launch/tests/test_ouster.launch index 821e77f13..3daee6376 100644 --- a/launch/tests/test_ouster.launch +++ b/launch/tests/test_ouster.launch @@ -12,14 +12,15 @@ coming from the first cloud sent by os1_cloud_node, which may be poorly synchronized with IMU data. --> + + - - + + - + - diff --git a/launch/tests/test_ouster_gen2.launch b/launch/tests/test_ouster_gen2.launch index e5650408d..8b160f6ff 100644 --- a/launch/tests/test_ouster_gen2.launch +++ b/launch/tests/test_ouster_gen2.launch @@ -40,11 +40,14 @@ + + - + + @@ -56,13 +59,12 @@ - - - + + @@ -70,15 +72,26 @@ - + + + + + + + + + + + + - - + + @@ -116,8 +129,8 @@ - - + + @@ -158,7 +171,7 @@ - + @@ -170,7 +183,7 @@ - + diff --git a/launch/tests/test_velodyne.launch b/launch/tests/test_velodyne.launch index a7ad67fa2..8a61c446d 100644 --- a/launch/tests/test_velodyne.launch +++ b/launch/tests/test_velodyne.launch @@ -14,7 +14,9 @@ - + + + @@ -24,7 +26,7 @@ - + @@ -34,7 +36,7 @@ - + @@ -46,9 +48,18 @@ - - - + + + + + + + + + + + + @@ -58,12 +69,14 @@ + + - + @@ -92,8 +105,9 @@ - - + + + @@ -105,7 +119,7 @@ - + @@ -127,7 +141,7 @@ - + @@ -151,12 +165,12 @@ - + + - diff --git a/launch/tests/test_velodyne_t265_deskewing.launch b/launch/tests/test_velodyne_t265_deskewing.launch new file mode 100644 index 000000000..8c7dcda1c --- /dev/null +++ b/launch/tests/test_velodyne_t265_deskewing.launch @@ -0,0 +1,184 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 6875fd85a..d3e43bf3b 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -175,6 +175,14 @@ This is my nodelet. + + + + This is my nodelet. + + diff --git a/src/LidarDeskewingNode.cpp b/src/LidarDeskewingNode.cpp new file mode 100644 index 000000000..1811fc1c5 --- /dev/null +++ b/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 #include #include +#include #include #include #include @@ -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.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; ugetVelocityGuess(); + } + return rtabmap::Transform(); +} + void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) { if(!this->isPaused()) diff --git a/src/nodelets/icp_odometry.cpp b/src/nodelets/icp_odometry.cpp index 1738b3780..c0ad4aa7e 100644 --- a/src/nodelets/icp_odometry.cpp +++ b/src/nodelets/icp_odometry.cpp @@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include "rtabmap_ros/MsgConversion.h" #include "rtabmap_ros/PluginInterface.h" @@ -68,6 +69,8 @@ class ICPOdometry : public rtabmap_ros::OdometryROS 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 @@ class ICPOdometry : public rtabmap_ros::OdometryROS 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 @@ class ICPOdometry : public rtabmap_ros::OdometryROS 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 @@ class ICPOdometry : public rtabmap_ros::OdometryROS // 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 @@ class ICPOdometry : public rtabmap_ros::OdometryROS //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 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 @@ class ICPOdometry : public rtabmap_ros::OdometryROS int scanNormalK_; double scanNormalRadius_; double scanNormalGroundUp_; + bool deskewing_; + bool deskewingSlerp_; std::vector > plugins_; pluginlib::ClassLoader plugin_loader_; bool scanReceived_ = false; diff --git a/src/nodelets/lidar_deskewing.cpp b/src/nodelets/lidar_deskewing.cpp new file mode 100644 index 000000000..d15c45ebb --- /dev/null +++ b/src/nodelets/lidar_deskewing.cpp @@ -0,0 +1,108 @@ + +#include +#include +#include + +#include + +#include +#include + +#include + +#include + +#include +#include +#include + +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(nh.resolveName("input_scan") + "/deskewed", 1); + pubCloud_ = nh.advertise(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); +} + +