From 2d0b043428c5e9a54ed11972f39fa1b91c6b060f Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Tue, 22 Dec 2020 14:54:01 +0100 Subject: [PATCH 1/6] [ROS][refactor] Introduce common lidar_conversions/Utilities.h --- .../lidar_conversions/src/Utilities.h | 116 ++++++++++++++++++ .../src/VelodyneToLidarNode.cxx | 30 ++--- 2 files changed, 124 insertions(+), 22 deletions(-) create mode 100644 ros_wrapping/lidar_conversions/src/Utilities.h diff --git a/ros_wrapping/lidar_conversions/src/Utilities.h b/ros_wrapping/lidar_conversions/src/Utilities.h new file mode 100644 index 00000000..c69c6fc4 --- /dev/null +++ b/ros_wrapping/lidar_conversions/src/Utilities.h @@ -0,0 +1,116 @@ +//============================================================================== +// Copyright 2019-2020 Kitware, Inc., Kitware SAS +// Author: Sanchez Julia (Kitware SAS) +// Creation date: 2020-12-10 +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +//============================================================================== + +#pragma once + +#include +#include + +namespace lidar_conversions +{ +namespace Utils +{ + +//------------------------------------------------------------------------------ +/*! + * @brief Copy pointcloud metadata to an other cloud + * @param[in] from The pointcloud to copy info from + * @param[out] to The pointcloud to copy info to + */ +template +inline void CopyPointCloudMetadata(const pcl::PointCloud& from, pcl::PointCloud& to) +{ + to.header = from.header; + to.is_dense = from.is_dense; + to.sensor_orientation_ = from.sensor_orientation_; + to.sensor_origin_ = from.sensor_origin_; +} + +//------------------------------------------------------------------------------ +/*! + * @brief Check if a PCL point is valid + * @param point A PCL point, with possible NaN as coordinates. + * @return true if all point coordinates are valid, false otherwise. + */ +template +inline bool IsFinite(const PointT& point) +{ + return std::isfinite(point.x) && std::isfinite(point.y) && std::isfinite(point.z); +} + +//------------------------------------------------------------------------------ +/*! + * @struct Helper to estimate point-wise within frame advancement for a spinning + * lidar sensor using azimuth angle. + */ +struct SpinningFrameAdvancementEstimator +{ + /*! + * @brief Reset the estimator. This should be called when a new frame is received. + */ + void Reset() + { + this->PreviousAdvancementPerRing.clear(); + } + + /*! + * @brief Estimate point advancement within current frame using azimuth angle. + * @param point the point to estimate its relative advancement. It MUST have a 'laser_id' field. + * @return relative advancement in range [0;~1]. + * + * This computation is based on azimuth angle of each measured point. + * The first point will return a 0 advancement. + * Advancement will increase clock-wise, reaching 1 when azimuth angle reaches + * initial azimuth value. It may be greater than 1 if the frame spins more + * than 360 degrees. + * + * NOTE: this estimation uses 2 priors: + * - real azimuth is always strictly inceasing within each laser ring, + * - real azimuth angle of any first point of a laser ring should be greater than initial azimuth. + */ + template + double operator()(const PointT& point) + { + // Compute normalized azimuth angle (in range [0-1]) + double pointAdvancement = (M_PI - std::atan2(point.y, point.x)) / (2 * M_PI); + + // If this is the first point of the frame + if (this->PreviousAdvancementPerRing.empty()) + this->InitAdvancement = pointAdvancement; + + // Get normalized angle (in [0-1]), with angle 0 being first point direction + auto wrapMax = [](double x, double max) { return std::fmod(max + std::fmod(x, max), max); }; + double frameAdvancement = wrapMax(pointAdvancement - this->InitAdvancement, 1.); + + // If we detect overflow, correct it + // If current laser_id is not in map, the following line will insert it, + // associating it to value 0.0. + if (frameAdvancement < this->PreviousAdvancementPerRing[point.laser_id]) + frameAdvancement += 1.; + this->PreviousAdvancementPerRing[point.laser_id] = frameAdvancement; + + return frameAdvancement; + } + +private: + double InitAdvancement; + std::map PreviousAdvancementPerRing; +}; + +} // end of namespace Utils +} // end of namespace lidar_conversions diff --git a/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx b/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx index dd2d4b4a..6eb4d33e 100644 --- a/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx +++ b/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx @@ -17,6 +17,7 @@ //============================================================================== #include "VelodyneToLidarNode.h" +#include "Utilities.h" #include #define BOLD_GREEN(s) "\033[1;32m" << s << "\033[0m" @@ -55,10 +56,7 @@ void VelodyneToLidarNode::Callback(const CloudV& cloudV) cloudS.reserve(cloudV.size()); // Copy pointcloud metadata - cloudS.header = cloudV.header; - cloudS.is_dense = cloudV.is_dense; - cloudS.sensor_orientation_ = cloudV.sensor_orientation_; - cloudS.sensor_origin_ = cloudV.sensor_origin_; + Utils::CopyPointCloudMetadata(cloudV, cloudS); // Check if time field looks properly set // If first and last points have same timestamps, this is not normal @@ -66,11 +64,8 @@ void VelodyneToLidarNode::Callback(const CloudV& cloudV) if (!isTimeValid) ROS_WARN_STREAM("Invalid 'time' field, it will be built from azimuth advancement."); - // Helpers to estimate frameAdvancement in case time field is invalid - auto wrapMax = [](double x, double max) { return std::fmod(max + std::fmod(x, max), max); }; - auto advancement = [](const PointV& velodynePoint) { return (M_PI - std::atan2(velodynePoint.y, velodynePoint.x)) / (2 * M_PI); }; - const double initAdvancement = advancement(cloudV.front()); - std::map previousAdvancementPerRing; + // Helper to estimate frameAdvancement in case time field is invalid + Utils::SpinningFrameAdvancementEstimator frameAdvancementEstimator; // Build SLAM pointcloud for (const PointV& velodynePoint : cloudV) @@ -83,25 +78,16 @@ void VelodyneToLidarNode::Callback(const CloudV& cloudV) slamPoint.laser_id = velodynePoint.ring; slamPoint.device_id = 0; - // Use time field is available + // Use time field if available // time is the offset to add to header.stamp to get point-wise timestamp if (isTimeValid) slamPoint.time = velodynePoint.time; // Try to build approximate timestamp from azimuth angle - // time is 0 for first point, and should match LiDAR period for last point for a complete scan. + // time is 0 for first point, and should match LiDAR period for last point + // for a 360 degrees scan. else - { - // Get normalized angle (in [0-1]), with angle 0 being first point direction - double frameAdvancement = advancement(velodynePoint); - frameAdvancement = wrapMax(frameAdvancement - initAdvancement, 1.); - // If we detect overflow, correct it - // If current laser_id (ring) is not in map, the following line will insert it, associating it to value 0.0. - if (frameAdvancement < previousAdvancementPerRing[velodynePoint.ring]) - frameAdvancement += 1; - previousAdvancementPerRing[velodynePoint.ring] = frameAdvancement; - slamPoint.time = frameAdvancement / this->LidarFreq; - } + slamPoint.time = frameAdvancementEstimator(slamPoint) / this->LidarFreq; cloudS.push_back(slamPoint); } -- GitLab From 5686dd0055dc74469519ca7be0de3454ee41cd68 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Tue, 22 Dec 2020 19:39:33 +0100 Subject: [PATCH 2/6] [ROS][feat] Add conversion node to use RSLidar16 data --- ros_wrapping/lidar_conversions/CMakeLists.txt | 5 + ros_wrapping/lidar_conversions/package.xml | 1 + .../src/RobosenseToLidarNode.cxx | 129 ++++++++++++++++++ .../src/RobosenseToLidarNode.h | 71 ++++++++++ 4 files changed, 206 insertions(+) create mode 100644 ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx create mode 100644 ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h diff --git a/ros_wrapping/lidar_conversions/CMakeLists.txt b/ros_wrapping/lidar_conversions/CMakeLists.txt index c9745c27..995541f8 100644 --- a/ros_wrapping/lidar_conversions/CMakeLists.txt +++ b/ros_wrapping/lidar_conversions/CMakeLists.txt @@ -32,5 +32,10 @@ include_directories( "../../slam_lib/include/" # this package needs LidarPoint definition which is a header file in slam library ) +# Velodyne Lidar add_executable(velodyne_conversion_node src/VelodyneToLidarNode.cxx) target_link_libraries(velodyne_conversion_node ${catkin_LIBRARIES}) + +# Robosense RSLidar +add_executable(robosense_conversion_node src/RobosenseToLidarNode.cxx) +target_link_libraries(robosense_conversion_node ${catkin_LIBRARIES}) diff --git a/ros_wrapping/lidar_conversions/package.xml b/ros_wrapping/lidar_conversions/package.xml index 25a618c1..88530637 100644 --- a/ros_wrapping/lidar_conversions/package.xml +++ b/ros_wrapping/lidar_conversions/package.xml @@ -6,6 +6,7 @@ Point type conversion from sensor driver to LidarPoint used in SLAM process. The ROS Velodyne driver can be found at https://github.com/ros-drivers/velodyne. + The ROS RSLidar driver can be found at https://github.com/RoboSense-LiDAR/ros_rslidar. diff --git a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx new file mode 100644 index 00000000..3ae0c577 --- /dev/null +++ b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx @@ -0,0 +1,129 @@ +//============================================================================== +// Copyright 2019-2020 Kitware, Inc., Kitware SAS +// Author: Sanchez Julia (Kitware SAS) +// Cadart Nicolas (Kitware SAS) +// Creation date: 2020-12-22 +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +//============================================================================== + +#include "RobosenseToLidarNode.h" +#include "Utilities.h" +#include + +#define BOLD_GREEN(s) "\033[1;32m" << s << "\033[0m" + +namespace lidar_conversions +{ +namespace +{ + // Mapping between RSLidar laser id and vertical laser id + // TODO add laser ID mappings for RS32, RSBPEARL and RSBPEARL_MINI ? + const std::array LASER_ID_MAPPING_RS16 = {0, 1, 2, 3, 4, 5, 6, 7, 15, 14, 13, 12, 11, 10, 9, 8}; +} + +RobosenseToLidarNode::RobosenseToLidarNode(ros::NodeHandle& nh, ros::NodeHandle& priv_nh) + : Nh(nh) + , PrivNh(priv_nh) +{ + // Get LiDAR frequency + this->PrivNh.param("lidar_frequency", this->LidarFreq, 10.0); + + // Init ROS publisher + this->Talker = nh.advertise("lidar_points", 1); + + // Init ROS subscriber + this->Listener = nh.subscribe("rslidar_points", 1, &RobosenseToLidarNode::Callback, this); + + ROS_INFO_STREAM(BOLD_GREEN("RSLidar data converter is ready !")); +} + +//------------------------------------------------------------------------------ +void RobosenseToLidarNode::Callback(const CloudRS& cloudRS) +{ + // If input cloud is empty, ignore it + if (cloudRS.empty()) + { + ROS_ERROR_STREAM("Input RSLidar pointcloud is empty : frame ignored."); + return; + } + + // Init SLAM pointcloud + CloudS cloudS; + cloudS.reserve(cloudRS.size()); + + // Copy pointcloud metadata + Utils::CopyPointCloudMetadata(cloudRS, cloudS); + cloudS.is_dense = true; + + // Helper to estimate frame advancement + Utils::SpinningFrameAdvancementEstimator frameAdvancementEstimator; + + // Build SLAM pointcloud + for (unsigned int i = 0; i < cloudRS.size(); ++i) + { + const PointRS& rsPoint = cloudRS[i]; + + // Check that input point does not have NaNs + if (!Utils::IsFinite(rsPoint)) + continue; + + // In case of dual returns mode, check that the second return is not identical to the first + // CHECK this operation for other sensors than RS16 + if (!cloudS.empty() && std::equal(rsPoint.data, rsPoint.data + 3, cloudS.back().data)) + continue; + + // Copy coordinates and intensity + PointS slamPoint; + slamPoint.x = rsPoint.x; + slamPoint.y = rsPoint.y; + slamPoint.z = rsPoint.z; + slamPoint.intensity = rsPoint.intensity; + + // Compute laser ID + // If we are using RS16 sensor, we need to correct the laser number + // CHECK this operation for other sensors than RS16 + uint16_t laser_id = i / cloudRS.width; + slamPoint.laser_id = (cloudRS.height == 16) ? LASER_ID_MAPPING_RS16[laser_id] : laser_id; + + // Build approximate timestamp from azimuth angle + // time is 0 for first point, and should match LiDAR period for last point + // for a 360 degrees scan. + slamPoint.time = frameAdvancementEstimator(slamPoint) / this->LidarFreq; + + cloudS.push_back(slamPoint); + } + + // Publish pointcloud only if non empty + if (!cloudS.empty()) + this->Talker.publish(cloudS); +} + +} // end of namespace lidar_conversions + +//------------------------------------------------------------------------------ +/*! + * @brief Main node entry point. + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "rslidar_conversion"); + ros::NodeHandle n; + ros::NodeHandle priv_nh("~"); + + lidar_conversions::RobosenseToLidarNode rs2s(n, priv_nh); + + ros::spin(); + + return 0; +} diff --git a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h new file mode 100644 index 00000000..ba57f529 --- /dev/null +++ b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h @@ -0,0 +1,71 @@ +//============================================================================== +// Copyright 2019-2020 Kitware, Inc., Kitware SAS +// Author: Sanchez Julia (Kitware SAS) +// Cadart Nicolas (Kitware SAS) +// Creation date: 2020-12-22 +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +//============================================================================== + +#pragma once + +#include +#include +#include +#include + +namespace lidar_conversions +{ + +/** + * @class RobosenseToLidarNode aims at converting pointclouds published by RSLidar + * ROS driver to the expected SLAM pointcloud format. + * + * The ROS RSLidar driver can be found here : + * https://github.com/RoboSense-LiDAR/ros_rslidar + */ +class RobosenseToLidarNode +{ +public: + using PointRS = pcl::PointXYZI; + using CloudRS = pcl::PointCloud; ///< Pointcloud published by rslidar driver + using PointS = LidarSlam::LidarPoint; + using CloudS = pcl::PointCloud; ///< Pointcloud needed by SLAM + + //---------------------------------------------------------------------------- + /*! + * @brief Constructor. + * @param nh Public ROS node handle, used to init publisher/subscriber. + * @param priv_nh Private ROS node handle, used to access parameters. + */ + RobosenseToLidarNode(ros::NodeHandle& nh, ros::NodeHandle& priv_nh); + + //---------------------------------------------------------------------------- + /*! + * @brief New lidar frame callback, converting and publishing RSLidar PointCloud as SLAM LidarPoint. + * @param cloud New Lidar Frame, published by rslidar_pointcloud/cloud_node. + */ + void Callback(const CloudRS& cloud); + +private: + + //---------------------------------------------------------------------------- + + // ROS node handles, subscriber and publisher + ros::NodeHandle &Nh, &PrivNh; + ros::Subscriber Listener; + ros::Publisher Talker; + double LidarFreq; +}; + +} // end of namespace lidar_conversions -- GitLab From 29719af6c7f155c9e519b22d72aaa6d8d313279c Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Wed, 23 Dec 2020 10:05:24 +0100 Subject: [PATCH 3/6] [ROS][fix] Improve approximate point-wise timestamps computation --- .../src/RobosenseToLidarNode.cxx | 10 ++++++---- .../lidar_conversions/src/VelodyneToLidarNode.cxx | 15 +++++++++++---- .../lidar_conversions/src/VelodyneToLidarNode.h | 1 + ros_wrapping/lidar_slam/launch/slam.launch | 10 +++++++--- 4 files changed, 25 insertions(+), 11 deletions(-) diff --git a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx index 3ae0c577..d4afd872 100644 --- a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx +++ b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx @@ -96,10 +96,12 @@ void RobosenseToLidarNode::Callback(const CloudRS& cloudRS) uint16_t laser_id = i / cloudRS.width; slamPoint.laser_id = (cloudRS.height == 16) ? LASER_ID_MAPPING_RS16[laser_id] : laser_id; - // Build approximate timestamp from azimuth angle - // time is 0 for first point, and should match LiDAR period for last point - // for a 360 degrees scan. - slamPoint.time = frameAdvancementEstimator(slamPoint) / this->LidarFreq; + // Build approximate point-wise timestamp from azimuth angle + // 'frame advancement' is 0 for first point, and should match 1 for last point + // for a 360 degrees scan at ideal spinning frequency. + // 'time' is the offset to add to 'header.stamp' (timestamp of the last RSLidar packet) + // to get approximate point-wise timestamp. + slamPoint.time = (frameAdvancementEstimator(slamPoint) - 1) / this->LidarFreq; cloudS.push_back(slamPoint); } diff --git a/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx b/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx index 6eb4d33e..164af3d4 100644 --- a/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx +++ b/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx @@ -31,6 +31,7 @@ VelodyneToLidarNode::VelodyneToLidarNode(ros::NodeHandle& nh, ros::NodeHandle& p { // Get LiDAR frequency this->PrivNh.param("lidar_frequency", this->LidarFreq, 10.0); + this->PrivNh.param("timestamp_first_packet", this->TimestampFirstPacket, this->TimestampFirstPacket); // Init ROS publisher this->Talker = nh.advertise("lidar_points", 1); @@ -83,11 +84,17 @@ void VelodyneToLidarNode::Callback(const CloudV& cloudV) if (isTimeValid) slamPoint.time = velodynePoint.time; - // Try to build approximate timestamp from azimuth angle - // time is 0 for first point, and should match LiDAR period for last point - // for a 360 degrees scan. + // Build approximate point-wise timestamp from azimuth angle + // 'frameAdvancement' is 0 for first point, and should match 1 for last point + // for a 360 degrees scan at ideal spinning frequency. + // 'time' is the offset to add to 'header.stamp' to get approximate point-wise timestamp. + // By default, 'header.stamp' is the timestamp of the last Veloydne packet, + // but user can choose the first packet timestamp using parameter 'timestamp_first_packet'. else - slamPoint.time = frameAdvancementEstimator(slamPoint) / this->LidarFreq; + { + double frameAdvancement = frameAdvancementEstimator(slamPoint); + slamPoint.time = (this->TimestampFirstPacket ? frameAdvancement : frameAdvancement - 1) / this->LidarFreq; + } cloudS.push_back(slamPoint); } diff --git a/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.h b/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.h index aba606eb..99d78b7f 100644 --- a/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.h +++ b/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.h @@ -65,6 +65,7 @@ private: ros::Subscriber Listener; ros::Publisher Talker; double LidarFreq; + bool TimestampFirstPacket = false; }; } // end of namespace lidar_conversions diff --git a/ros_wrapping/lidar_slam/launch/slam.launch b/ros_wrapping/lidar_slam/launch/slam.launch index e7bd135c..69b55bdd 100644 --- a/ros_wrapping/lidar_slam/launch/slam.launch +++ b/ros_wrapping/lidar_slam/launch/slam.launch @@ -1,11 +1,14 @@ - + + + + @@ -15,9 +18,10 @@ - + - + + -- GitLab From 51900209042270f0ffe6345fade61cab8417d95c Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Wed, 23 Dec 2020 10:29:47 +0100 Subject: [PATCH 4/6] [ROS][refactor] Change lidar_frequency option to rpm to mimic ROS drivers parameters --- .../lidar_conversions/src/RobosenseToLidarNode.cxx | 6 +++--- .../lidar_conversions/src/RobosenseToLidarNode.h | 5 ++++- .../lidar_conversions/src/VelodyneToLidarNode.cxx | 6 +++--- ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.h | 7 +++++-- ros_wrapping/lidar_slam/launch/slam.launch | 9 +++++---- 5 files changed, 20 insertions(+), 13 deletions(-) diff --git a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx index d4afd872..827680d4 100644 --- a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx +++ b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx @@ -36,8 +36,8 @@ RobosenseToLidarNode::RobosenseToLidarNode(ros::NodeHandle& nh, ros::NodeHandle& : Nh(nh) , PrivNh(priv_nh) { - // Get LiDAR frequency - this->PrivNh.param("lidar_frequency", this->LidarFreq, 10.0); + // Get LiDAR spinning speed + this->PrivNh.param("rpm", this->Rpm, this->Rpm); // Init ROS publisher this->Talker = nh.advertise("lidar_points", 1); @@ -101,7 +101,7 @@ void RobosenseToLidarNode::Callback(const CloudRS& cloudRS) // for a 360 degrees scan at ideal spinning frequency. // 'time' is the offset to add to 'header.stamp' (timestamp of the last RSLidar packet) // to get approximate point-wise timestamp. - slamPoint.time = (frameAdvancementEstimator(slamPoint) - 1) / this->LidarFreq; + slamPoint.time = (frameAdvancementEstimator(slamPoint) - 1) / this->Rpm * 60.; cloudS.push_back(slamPoint); } diff --git a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h index ba57f529..3234211c 100644 --- a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h +++ b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h @@ -65,7 +65,10 @@ private: ros::NodeHandle &Nh, &PrivNh; ros::Subscriber Listener; ros::Publisher Talker; - double LidarFreq; + + // Useful variables for approximate point-wise timestamps computation + // These parameters should be set to the same values as ROS RSLidar driver's. + double Rpm = 600; ///< Spinning speed of sensor [rpm] }; } // end of namespace lidar_conversions diff --git a/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx b/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx index 164af3d4..fee20dba 100644 --- a/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx +++ b/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx @@ -29,8 +29,8 @@ VelodyneToLidarNode::VelodyneToLidarNode(ros::NodeHandle& nh, ros::NodeHandle& p : Nh(nh) , PrivNh(priv_nh) { - // Get LiDAR frequency - this->PrivNh.param("lidar_frequency", this->LidarFreq, 10.0); + // Get LiDAR spinning speed and first timestamp option + this->PrivNh.param("rpm", this->Rpm, this->Rpm); this->PrivNh.param("timestamp_first_packet", this->TimestampFirstPacket, this->TimestampFirstPacket); // Init ROS publisher @@ -93,7 +93,7 @@ void VelodyneToLidarNode::Callback(const CloudV& cloudV) else { double frameAdvancement = frameAdvancementEstimator(slamPoint); - slamPoint.time = (this->TimestampFirstPacket ? frameAdvancement : frameAdvancement - 1) / this->LidarFreq; + slamPoint.time = (this->TimestampFirstPacket ? frameAdvancement : frameAdvancement - 1) / this->Rpm * 60.; } cloudS.push_back(slamPoint); diff --git a/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.h b/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.h index 99d78b7f..5f61782e 100644 --- a/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.h +++ b/ros_wrapping/lidar_conversions/src/VelodyneToLidarNode.h @@ -64,8 +64,11 @@ private: ros::NodeHandle &Nh, &PrivNh; ros::Subscriber Listener; ros::Publisher Talker; - double LidarFreq; - bool TimestampFirstPacket = false; + + // Useful variables for approximate point-wise timestamps computation + // These parameters should be set to the same values as ROS Velodyne driver's. + double Rpm = 600; ///< Spinning speed of sensor [rpm] + bool TimestampFirstPacket = false; ///< Wether timestamping is based on the first or last packet of each scan }; } // end of namespace lidar_conversions diff --git a/ros_wrapping/lidar_slam/launch/slam.launch b/ros_wrapping/lidar_slam/launch/slam.launch index 69b55bdd..916ed287 100644 --- a/ros_wrapping/lidar_slam/launch/slam.launch +++ b/ros_wrapping/lidar_slam/launch/slam.launch @@ -5,9 +5,10 @@ - - + + @@ -20,7 +21,7 @@ - + -- GitLab From 5572c4fe4d656c83d5531179af373f7f14d5d690 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Wed, 23 Dec 2020 14:26:45 +0100 Subject: [PATCH 5/6] [ROS][doc] Add lidar_conversions README and update lidar_slam README --- ros_wrapping/lidar_conversions/README.md | 47 ++++++++++++++++++++++++ ros_wrapping/lidar_slam/README.md | 12 +++++- 2 files changed, 57 insertions(+), 2 deletions(-) create mode 100644 ros_wrapping/lidar_conversions/README.md diff --git a/ros_wrapping/lidar_conversions/README.md b/ros_wrapping/lidar_conversions/README.md new file mode 100644 index 00000000..eab916dd --- /dev/null +++ b/ros_wrapping/lidar_conversions/README.md @@ -0,0 +1,47 @@ +# lidar_conversions + +## Description + +Helper package to convert raw pointclouds output by common LiDAR drivers to the pointcloud format expected by the SLAM algorithm. + +The SLAM algorithm expects input pointclouds as *sensor_msgs/PointCloud2* messages. These pointclouds should have the following fields: +- **x**, **y**, **z** (`float`) : point coordinates +- **time** (`double`) : time offset to add to the pointcloud header timestamp to get approximate point-wise aquisition timestamp +- **intensity** (`float`) : intensity/reflectivity of the point +- **laser_id** (`uint16`) : numeric identifier of the laser ring that shot this point. The lowest/bottom laser ring should be 0, and it should increase upward. +- **device_id** (`uint8`), **label** (`uint8`) : optional inputs, not yet used. + +Especially, the point-wise timestamps are necessary if undistortion is enabled in SLAM. The nodes of this package are able to compute approximate timestamps when those are not available in the input pointcloud. + +Currently, this package implements the following nodes : +- **velodyne_conversion_node** : converts pointclouds output by Velodyne spinning sensors using the [ROS Velodyne driver](https://github.com/ros-drivers/velodyne) to SLAM pointcloud format. +- **robosense_conversion_node** : converts pointclouds output by RoboSense spinning sensors using the [ROS RoboSense-LiDAR driver](https://github.com/RoboSense-LiDAR/ros_rslidar) to SLAM pointcloud format. This has been tested only with RS16 sensor, and could need additional changes to support other RS sensors. + +## Usage + +Direct usage : + +```bash +rosrun lidar_conversions velodyne_conversion_node +``` + +Example of minimal launchfile for a multi-lidar setup: + +```xml + + + + + + + + + + + + + + +``` \ No newline at end of file diff --git a/ros_wrapping/lidar_slam/README.md b/ros_wrapping/lidar_slam/README.md index a7f55784..de4d432c 100644 --- a/ros_wrapping/lidar_slam/README.md +++ b/ros_wrapping/lidar_slam/README.md @@ -42,9 +42,17 @@ rosbag play --clock # in 2nd shell roslaunch lidar_slam slam.launch use_sim_time:=false ``` -This launch file will start a *lidar_slam_node*, a pre-configured RViz session, a *velodyne_conversion_node* (converts the point type for lidar_slam use, node to adapt for other sensors) and, if `gps` arg is enabled, GPS/UTM conversions nodes to publish SLAM pose as a GPS coordinate in WGS84 format, with the prior that full GPS pose and GPS/LiDAR calibration are correctly known and set (see [GPS/SLAM calibration](#gpsslam-calibration) section below). +This launch file will start a *lidar_slam_node*, a pre-configured RViz session, a *velodyne_conversion_node* (converts the point type for lidar_slam use, see next paragraph) and, if `gps` arg is enabled, GPS/UTM conversions nodes to publish SLAM pose as a GPS coordinate in WGS84 format, with the prior that full GPS pose and GPS/LiDAR calibration are correctly known and set (see [GPS/SLAM calibration](#gpsslam-calibration) section below). + +The SLAM algorithm expects input pointclouds on topic *lidar_points* as *sensor_msgs/PointCloud2* messages. These pointclouds should have the following fields: +- **x**, **y**, **z** (`float`) : point coordinates +- **time** (`double`) : time offset to add to the pointcloud header timestamp to get approximate point-wise aquisition timestamp +- **intensity** (`float`) : intensity/reflectivity of the point +- **laser_id** (`uint16`) : numeric identifier of the laser ring that shot this point. The lowest/bottom laser ring should be 0, and it should increase upward. +- **device_id** (`uint8`), **label** (`uint8`) : optional inputs, not yet used. + +If your LiDAR driver does not output such data, you can use the `lidar_conversions` nodes. -For Velodyne use, input pointcloud must be a *sensor_msgs/PointCloud2* message (of points `velodyne_pointcloud::PointXYZIR` or `velodyne_pointcloud::PointXYZIRT`) published on topic '*velodyne_points*'. Optional input GPS (see [Optional GPS use](#optional-gps-use) section) fix must be a *gps_common/GPSFix* message published on topic '*gps_fix*'. SLAM outputs can also be configured out to publish : -- GitLab From 1af15fb113f3b50893911fb33c0eb7c50193a8a9 Mon Sep 17 00:00:00 2001 From: Nicolas Cadart Date: Mon, 4 Jan 2021 13:54:32 +0100 Subject: [PATCH 6/6] [ROS][fix] Estimate RSLidar point-wise timestamps using ID MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Estimate RSLidar point-wise timestamps using point ID in input cloud rather than azimuth angle. This properly manages the scan closure timestamps if there are some azimuth offsets between laser rings. However, this requires that each input scan is a full scan covering 360°. --- .../src/RobosenseToLidarNode.cxx | 17 +++++++++++------ .../src/RobosenseToLidarNode.h | 4 +++- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx index 827680d4..34db5012 100644 --- a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx +++ b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx @@ -66,15 +66,17 @@ void RobosenseToLidarNode::Callback(const CloudRS& cloudRS) Utils::CopyPointCloudMetadata(cloudRS, cloudS); cloudS.is_dense = true; - // Helper to estimate frame advancement - Utils::SpinningFrameAdvancementEstimator frameAdvancementEstimator; + // Helpers to estimate point-wise fields + const unsigned int nLasers = cloudRS.height; + const unsigned int pointsPerRing = cloudRS.size() / nLasers; // Build SLAM pointcloud for (unsigned int i = 0; i < cloudRS.size(); ++i) { const PointRS& rsPoint = cloudRS[i]; - // Check that input point does not have NaNs + // Check that input point does not have NaNs as even invalid points are + // returned by the RSLidar driver if (!Utils::IsFinite(rsPoint)) continue; @@ -94,14 +96,17 @@ void RobosenseToLidarNode::Callback(const CloudRS& cloudRS) // If we are using RS16 sensor, we need to correct the laser number // CHECK this operation for other sensors than RS16 uint16_t laser_id = i / cloudRS.width; - slamPoint.laser_id = (cloudRS.height == 16) ? LASER_ID_MAPPING_RS16[laser_id] : laser_id; + slamPoint.laser_id = (nLasers == 16) ? LASER_ID_MAPPING_RS16[laser_id] : laser_id; - // Build approximate point-wise timestamp from azimuth angle + // Build approximate point-wise timestamp from point id. // 'frame advancement' is 0 for first point, and should match 1 for last point // for a 360 degrees scan at ideal spinning frequency. // 'time' is the offset to add to 'header.stamp' (timestamp of the last RSLidar packet) // to get approximate point-wise timestamp. - slamPoint.time = (frameAdvancementEstimator(slamPoint) - 1) / this->Rpm * 60.; + // NOTE: to be precise, this estimation requires that each input scan is an + // entire scan covering excatly 360°. + double frameAdvancement = static_cast(i % pointsPerRing) / pointsPerRing; + slamPoint.time = (frameAdvancement - 1) / this->Rpm * 60.; cloudS.push_back(slamPoint); } diff --git a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h index 3234211c..841130d6 100644 --- a/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h +++ b/ros_wrapping/lidar_conversions/src/RobosenseToLidarNode.h @@ -68,7 +68,9 @@ private: // Useful variables for approximate point-wise timestamps computation // These parameters should be set to the same values as ROS RSLidar driver's. - double Rpm = 600; ///< Spinning speed of sensor [rpm] + // NOTE: to be precise, this timestamp estimation requires that each input + // scan is an entire scan covering excatly 360°. + double Rpm = 600; ///< Spinning speed of sensor [rpm]. The duration of each input scan will be 60 / Rpm seconds. }; } // end of namespace lidar_conversions -- GitLab