diff --git a/ros2_wrapping/lidar_conversions/src/GenericConversionNode.cxx b/ros2_wrapping/lidar_conversions/src/GenericConversionNode.cxx index a33974944d33fa6b7ee4283c726eb8ddb5d5dfb6..91dc212affbb7a6211ffa15565492bd00851c181 100644 --- a/ros2_wrapping/lidar_conversions/src/GenericConversionNode.cxx +++ b/ros2_wrapping/lidar_conversions/src/GenericConversionNode.cxx @@ -101,7 +101,7 @@ void GenericConversionNode::Callback(const Pcl2_msg& msg_received) Utils::InitEstimationParameters(cloudRaw, nbLasers, this->Clusters, this->RotationIsClockwise, this->NbThreads); this->RotSenseAndClustersEstimated = true; } - Eigen::Vector2d firstPoint = {cloudRaw[0].x, cloudRaw[0].y}; + Eigen::Vector2d firstPoint = Utils::GetFirstValidPoint(cloudRaw); // Initialize vectors of optional fields std::vector intensities; @@ -191,8 +191,10 @@ void GenericConversionNode::Callback(const Pcl2_msg& msg_received) bool timeIsValid = false; if (!times.empty()) { - double duration = times.back() - times.front(); + auto minmaxTime = std::minmax_element(times.begin(), times.end()); + double duration = *minmaxTime.second - *minmaxTime.first; double factor = Utils::GetTimeFactor(duration, this->RotationDuration); + duration *= factor; timeIsValid = duration > 1e-8 && duration < 2. * this->RotationDuration; if (!timeIsValid) RCLCPP_WARN_STREAM(this->get_logger(), "Invalid 'time' field, it will be built from azimuth advancement."); @@ -224,6 +226,13 @@ void GenericConversionNode::Callback(const Pcl2_msg& msg_received) if (!Utils::HasNanField(slamPoint)) cloudS.push_back(slamPoint); } + + // Publish pointcloud only if it is not empty + if (cloudS.empty()) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Slam pointcloud is empty : frame ignored."); + return; + } PublishMsg(cloudS); } diff --git a/ros2_wrapping/lidar_conversions/src/HesaiToLidarNode.cxx b/ros2_wrapping/lidar_conversions/src/HesaiToLidarNode.cxx index 44aadd2c73311e975331652b63c40839fd9441ec..3de1a222169840163a6f96c926afadbe5bfc0f02 100644 --- a/ros2_wrapping/lidar_conversions/src/HesaiToLidarNode.cxx +++ b/ros2_wrapping/lidar_conversions/src/HesaiToLidarNode.cxx @@ -106,15 +106,18 @@ void HesaiToLidarNode::Callback(const Pcl2_msg& msg_received) } // Check if time field looks properly set - double duration = cloudH.back().timestamp - cloudH.front().timestamp; + auto minmaxTime = std::minmax_element(cloudH.points.begin(), cloudH.points.end(), + [](const PointH& lhs, const PointH& rhs) { return lhs.timestamp < rhs.timestamp; }); + double duration = minmaxTime.second->timestamp - minmaxTime.first->timestamp; double factor = Utils::GetTimeFactor(duration, this->RotationDuration); + duration *= factor; bool timeIsValid = duration > 1e-8 && duration < 2. * this->RotationDuration; if (!timeIsValid) RCLCPP_WARN_STREAM(this->get_logger(), "Invalid 'time' field, it will be built from azimuth advancement."); - Eigen::Vector2d firstPoint = {cloudH[0].x, cloudH[0].y}; + Eigen::Vector2d firstPoint = Utils::GetFirstValidPoint(cloudH); // Build SLAM pointcloud #pragma omp parallel for num_threads(this->NbThreads) @@ -140,7 +143,14 @@ void HesaiToLidarNode::Callback(const Pcl2_msg& msg_received) if (!Utils::HasNanField(slamPoint)) cloudS.push_back(slamPoint); } - //convertion PointCloud to msg + + // Publish pointcloud only if it is not empty + if (cloudS.empty()) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Slam pointcloud is empty : frame ignored."); + return; + } + // Convert PointCloud into msg Pcl2_msg msg_sent; pcl::toROSMsg(cloudS, msg_sent); diff --git a/ros2_wrapping/lidar_conversions/src/LivoxToLidarNode.cxx b/ros2_wrapping/lidar_conversions/src/LivoxToLidarNode.cxx index 9a75947c69afe6308986ac066f99b2b31fd92316..e2837087016f7562c4e1c05067faf246279f68a2 100644 --- a/ros2_wrapping/lidar_conversions/src/LivoxToLidarNode.cxx +++ b/ros2_wrapping/lidar_conversions/src/LivoxToLidarNode.cxx @@ -89,7 +89,13 @@ void LivoxToLidarNode::PointCloud2Callback(const Pcl2_msg& msg_received) cloudS.push_back(slamPoint); } - // Convertion PointCloud to msg + // Publish pointcloud only if it is not empty + if (cloudS.empty()) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Slam pointcloud is empty : frame ignored."); + return; + } + // Convert PointCloud into msg Pcl2_msg msg_sent; pcl::toROSMsg(cloudS, msg_sent); diff --git a/ros2_wrapping/lidar_conversions/src/OusterToLidarNode.cxx b/ros2_wrapping/lidar_conversions/src/OusterToLidarNode.cxx index 1e034c2ca580c2e09931576b50056d9b2133cb3d..77bb21912231a6a7fe4ac6201b6bf26b89e8a7a0 100644 --- a/ros2_wrapping/lidar_conversions/src/OusterToLidarNode.cxx +++ b/ros2_wrapping/lidar_conversions/src/OusterToLidarNode.cxx @@ -110,15 +110,18 @@ void OusterToLidarNode::Callback(const Pcl2_msg& msg_received) } // Check if time field looks properly set - double duration = cloudO.back().t - cloudO.front().t; + auto minmaxTime = std::minmax_element(cloudO.points.begin(), cloudO.points.end(), + [](const PointO& lhs, const PointO& rhs) { return lhs.t < rhs.t; }); + double duration = minmaxTime.second->t - minmaxTime.first->t; double factor = Utils::GetTimeFactor(duration, this->RotationDuration); + duration *= factor; bool timeIsValid = duration > 1e-8 && duration * factor < 2. * this->RotationDuration; if (!timeIsValid) RCLCPP_WARN_STREAM(this->get_logger(), "Invalid 'time' field, it will be built from azimuth advancement."); - Eigen::Vector2d firstPoint = {cloudO[0].x, cloudO[0].y}; + Eigen::Vector2d firstPoint = Utils::GetFirstValidPoint(cloudO); // Build SLAM pointcloud #pragma omp parallel for num_threads(this->NbThreads) @@ -145,7 +148,13 @@ void OusterToLidarNode::Callback(const Pcl2_msg& msg_received) cloudS.push_back(slamPoint); } - //conversion to msg + // Publish pointcloud only if it is not empty + if (cloudS.empty()) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Slam pointcloud is empty : frame ignored."); + return; + } + // Convert PointCloud into msg Pcl2_msg toPublish; pcl::toROSMsg(cloudS, toPublish); diff --git a/ros2_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx b/ros2_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx index e81736eadd89243306cef48a12cb27d48501e8c2..f2f3170f3999f8f3e0badcbbfb3057ae70180e8f 100644 --- a/ros2_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx +++ b/ros2_wrapping/lidar_conversions/src/RobosenseToLidarNode.cxx @@ -103,7 +103,7 @@ void RobosenseToLidarNode::Callback(const Pcl2_msg& msg_received) this->RotSenseAndClustersEstimated = true; } - Eigen::Vector2d firstPoint = {cloudRS[0].x, cloudRS[0].y}; + Eigen::Vector2d firstPoint = Utils::GetFirstValidPoint(cloudRS); // Build SLAM pointcloud #pragma omp parallel for num_threads(this->NbThreads) @@ -133,15 +133,17 @@ void RobosenseToLidarNode::Callback(const Pcl2_msg& msg_received) cloudS.push_back(slamPoint); } - // Publish pointcloud only if non empty - if (!cloudS.empty()) + // Publish pointcloud only if it is not empty + if (cloudS.empty()) { - //convertion to msg - Pcl2_msg msg_sended; - pcl::toROSMsg(cloudS, msg_sended); - - this->Talker->publish(msg_sended); + RCLCPP_ERROR_STREAM(this->get_logger(), "Slam pointcloud is empty : frame ignored."); + return; } + // Convert PointCloud into msg + Pcl2_msg msg_sended; + pcl::toROSMsg(cloudS, msg_sended); + + this->Talker->publish(msg_sended); } //------------------------------------------------------------------------------ diff --git a/ros2_wrapping/lidar_conversions/src/Utilities.h b/ros2_wrapping/lidar_conversions/src/Utilities.h index 6bcaac093a774425b72e4b05986aebfc3f1ecc1f..b1fda86828530bac393357b725b6c0bfa5e7d8de 100644 --- a/ros2_wrapping/lidar_conversions/src/Utilities.h +++ b/ros2_wrapping/lidar_conversions/src/Utilities.h @@ -307,6 +307,24 @@ inline int ComputeLaserId(const Eigen::Vector3d& currentPoint, unsigned int nbLa return std::distance(clusters.begin(), (vertAngle - prev->Mean < insertionIt->Mean - vertAngle) ? prev : insertionIt); } +//---------------------------------------------------------------------------- +/*! + * @brief Get first valid point in the pointcloud + * @return First valid point in the pointcloud. + * @param cloudRaw PointCloud published by lidar driver + */ +template +inline Eigen::Vector2d GetFirstValidPoint(const pcl::PointCloud cloudRaw) +{ + for (auto id = 0; id < cloudRaw.size(); id++) + { + if (Utils::IsPointValid(cloudRaw[id])) + { + return {cloudRaw[id].x, cloudRaw[id].y}; + } + } +} + //---------------------------------------------------------------------------- /*! * @brief Deduce the rotation sense of the lidar @@ -317,10 +335,13 @@ inline int ComputeLaserId(const Eigen::Vector3d& currentPoint, unsigned int nbLa template inline bool IsRotationClockwise(const pcl::PointCloud cloudRaw, unsigned int nbLasers) { - Eigen::Vector2d firstPointFirstLine ({cloudRaw.front().x, cloudRaw.front().y}); - Eigen::Vector2d firstPointSecondLine ({cloudRaw[nbLasers].x, cloudRaw[nbLasers].y}); - double crossZ = firstPointFirstLine.x() * firstPointSecondLine.y() - firstPointFirstLine.y() * firstPointSecondLine.x(); - return crossZ > 0; + for (auto id = 0; id < cloudRaw.size(); id++) + { + if (Utils::IsPointValid(cloudRaw[id]) && Utils::IsPointValid(cloudRaw[id + nbLasers])) + { + return cloudRaw[id].x * cloudRaw[id + nbLasers].y - cloudRaw[id].y * cloudRaw[id + nbLasers].x > 0; + } + } } //---------------------------------------------------------------------------- diff --git a/ros2_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx b/ros2_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx index eac4854c6f9af4ee5f4b3d3267d1147f6f7a84d8..39eb1068587eb4caa4f6eb55412cc74983f2842f 100644 --- a/ros2_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx +++ b/ros2_wrapping/lidar_conversions/src/VelodyneToLidarNode.cxx @@ -106,15 +106,18 @@ void VelodyneToLidarNode::Callback(const Pcl2_msg& msg_received) } // Check if time field looks properly set - double duration = cloudV.back().time - cloudV.front().time; + auto minmaxTime = std::minmax_element(cloudV.points.begin(), cloudV.points.end(), + [](const PointV& lhs, const PointV& rhs) { return lhs.time < rhs.time; }); + double duration = minmaxTime.second->time - minmaxTime.first->time; double factor = Utils::GetTimeFactor(duration, this->RotationDuration); + duration *= factor; bool timeIsValid = duration > 1e-8 && duration < 2. * this->RotationDuration; if (!timeIsValid) RCLCPP_WARN_STREAM(this->get_logger(), "Invalid 'time' field, it will be built from azimuth advancement."); - Eigen::Vector2d firstPoint = {cloudV[0].x, cloudV[0].y}; + Eigen::Vector2d firstPoint = Utils::GetFirstValidPoint(cloudV);; // Build SLAM pointcloud #pragma omp parallel for num_threads(this->NbThreads) @@ -140,7 +143,14 @@ void VelodyneToLidarNode::Callback(const Pcl2_msg& msg_received) if (!Utils::HasNanField(slamPoint)) cloudS.push_back(slamPoint); } - //convertion PointCloud to msg + + // Publish pointcloud only if it is not empty + if (cloudS.empty()) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Slam pointcloud is empty : frame ignored."); + return; + } + // Convert PointCloud into msg Pcl2_msg msg_sent; pcl::toROSMsg(cloudS, msg_sent);