From dd0195c16bf40dc382e5ceb19c8d3bf9903a6123 Mon Sep 17 00:00:00 2001 From: Tiziano Guadagnino <37455909+tizianoGuadagnino@users.noreply.github.com> Date: Fri, 22 Nov 2024 18:16:52 +0100 Subject: [PATCH] Add support for 2D LiDARs (#17) * We didnt have proper flagging in the online node * Clean launch common args * Add new threshold * Update CMakeLists.txt (#7) Make *USE_SYSTEM_SOPHUS* consistent with the rest of the options. * Update README.md (#6) More accurate statement in the readme, the system does not need to follow a unicycle motion model, just the pose correction needs to do that, which applies to much more robotics platforms. * Add python checks to pre-commit (#9) * Add black and isort * Format * use black for isort * Fixing ROS launch system (#8) * We didnt have proper flagging in the online node * Clean launch common args * Fix formatting python * Tiny modification, but makes sense * Renaming and add configuration for the correspondence threshold * Fix pipeline * Consistency in the CMakeLists * Remove ternary operator and make the two functions consistent * Add 2d lidar support, need testing * Restore ros utils to master * Revise timestamping (#18) * Query from last stamp in case of frame drops * Rename * WIP * Fix conversions * Fix build * Make CI happy * Add TimeStampHandler module to handle this madness * Minor renaming * Try to take care of absolute timestamping of points + scan stamped at the end * Comments * Should fix all cases that came to my mind at this time * Some comments to make stuff more clear * Remove useless case, we need to fix deskewing * Fix condition for clarity --------- Co-authored-by: tizianoGuadagnino * Kinematic ICP Threshold (#15) * We didnt have proper flagging in the online node * Clean launch common args * Add new threshold * Update CMakeLists.txt (#7) Make *USE_SYSTEM_SOPHUS* consistent with the rest of the options. * Update README.md (#6) More accurate statement in the readme, the system does not need to follow a unicycle motion model, just the pose correction needs to do that, which applies to much more robotics platforms. * Add python checks to pre-commit (#9) * Add black and isort * Format * use black for isort * Fixing ROS launch system (#8) * We didnt have proper flagging in the online node * Clean launch common args * Fix formatting python * Tiny modification, but makes sense * Renaming and add configuration for the correspondence threshold * Fix pipeline * Consistency in the CMakeLists * Remove ternary operator and make the two functions consistent * Update cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp Co-authored-by: Benedikt Mersch * Renaming according to Ben's review * Why a ref * Odometry Regularization (#19) * Natural extension to make also the odometry regularization optional * Consistency in the CMakeLists * Remove ternary operator and make the two functions consistent * Update cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp Co-authored-by: Benedikt Mersch * Renaming according to Ben's review * Why a ref * Use the same default --------- Co-authored-by: Benedikt Mersch Co-authored-by: Benedikt Mersch --------- Co-authored-by: Benedikt Mersch Co-authored-by: Benedikt Mersch * We didnt have proper flagging in the online node * Add new threshold * Renaming and add configuration for the correspondence threshold * Fix pipeline * fixx * Include timestamps from the projected laser --------- Co-authored-by: Benedikt Mersch Co-authored-by: Benedikt Mersch --- ros/CMakeLists.txt | 2 + .../kinematic_icp_ros/nodes/offline_node.hpp | 5 ++- .../kinematic_icp_ros/nodes/online_node.hpp | 6 ++- ros/launch/common_args.launch.py | 6 +++ ros/launch/offline_node.launch.py | 3 +- ros/launch/online_node.launch.py | 4 +- ros/package.xml | 1 + .../kinematic_icp_ros/nodes/offline_node.cpp | 45 +++++++++++++++---- .../kinematic_icp_ros/nodes/online_node.cpp | 34 +++++++++++--- .../utils/TimeStampHandler.cpp | 5 ++- 10 files changed, 90 insertions(+), 21 deletions(-) diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index 932dc5c..58595f9 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -36,6 +36,7 @@ find_package(rcutils REQUIRED) find_package(rosbag2_cpp REQUIRED) find_package(rosbag2_storage REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(laser_geometry REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_ros REQUIRED) @@ -50,6 +51,7 @@ set(dependencies rosbag2_cpp rosbag2_storage sensor_msgs + laser_geometry std_msgs std_srvs tf2_ros diff --git a/ros/include/kinematic_icp_ros/nodes/offline_node.hpp b/ros/include/kinematic_icp_ros/nodes/offline_node.hpp index 1abcfd8..df37065 100644 --- a/ros/include/kinematic_icp_ros/nodes/offline_node.hpp +++ b/ros/include/kinematic_icp_ros/nodes/offline_node.hpp @@ -27,6 +27,7 @@ // ROS #include +#include #include #include #include @@ -46,13 +47,15 @@ class OfflineNode { private: // Common for offline/online nodes - std::string pcl_topic_; + std::string lidar_topic_; + bool use_2d_lidar_; std::shared_ptr odometry_server_; rclcpp::Node::SharedPtr node_; // store data for the experiments using PoseWithStamp = std::pair; std::vector poses_with_timestamps_; std::filesystem::path output_pose_file_; + laser_geometry::LaserProjection laser_projector_; // Offline node specifics BagMultiplexer bag_multiplexer_; diff --git a/ros/include/kinematic_icp_ros/nodes/online_node.hpp b/ros/include/kinematic_icp_ros/nodes/online_node.hpp index d23932b..1c6c92c 100644 --- a/ros/include/kinematic_icp_ros/nodes/online_node.hpp +++ b/ros/include/kinematic_icp_ros/nodes/online_node.hpp @@ -26,7 +26,9 @@ #include // ROS +#include #include +#include #include #include "kinematic_icp_ros/server/LidarOdometryServer.hpp" @@ -43,12 +45,14 @@ class OnlineNode { private: // Common for offline/online nodes - std::string pcl_topic_; + std::string lidar_topic_; std::shared_ptr odometry_server_; rclcpp::Node::SharedPtr node_; + laser_geometry::LaserProjection laser_projector_; // Online node specifics rclcpp::Subscription::SharedPtr pointcloud_sub_; + rclcpp::Subscription::SharedPtr laser_scan_sub_; }; } // namespace kinematic_icp_ros diff --git a/ros/launch/common_args.launch.py b/ros/launch/common_args.launch.py index 9ff7c4c..0248248 100644 --- a/ros/launch/common_args.launch.py +++ b/ros/launch/common_args.launch.py @@ -9,6 +9,12 @@ def generate_launch_description(): "lidar_topic", description="", ), + DeclareLaunchArgument( + "use_2d_lidar", + default_value="false", + description="", + choices=["true", "false"], + ), DeclareLaunchArgument( "lidar_odometry_topic", default_value="lidar_odometry", diff --git a/ros/launch/offline_node.launch.py b/ros/launch/offline_node.launch.py index bd66551..a9e1727 100644 --- a/ros/launch/offline_node.launch.py +++ b/ros/launch/offline_node.launch.py @@ -43,7 +43,8 @@ def _generate_launch_description(context: launch.LaunchContext, *args, **kwargs) + "/config/kinematic_icp_ros.yaml", { # Input topic, is not a remap to marry API with offline node - "input": LaunchConfiguration("lidar_topic"), + "lidar_topic": LaunchConfiguration("lidar_topic"), + "use_2d_lidar": LaunchConfiguration("use_2d_lidar"), # ROS node configuration "lidar_odom_frame": LaunchConfiguration("lidar_odom_frame"), "wheel_odom_frame": LaunchConfiguration("wheel_odom_frame"), diff --git a/ros/launch/online_node.launch.py b/ros/launch/online_node.launch.py index 603e973..ac0cce1 100644 --- a/ros/launch/online_node.launch.py +++ b/ros/launch/online_node.launch.py @@ -38,7 +38,8 @@ def generate_launch_description(): + "/config/kinematic_icp_ros.yaml", { # Input topic, is not a remap to marry API with offline node - "input": LaunchConfiguration("lidar_topic"), + "lidar_topic": LaunchConfiguration("lidar_topic"), + "use_2d_lidar": LaunchConfiguration("use_2d_lidar"), # ROS node configuration "lidar_odom_frame": LaunchConfiguration("lidar_odom_frame"), "wheel_odom_frame": LaunchConfiguration("wheel_odom_frame"), @@ -51,6 +52,7 @@ def generate_launch_description(): }, ], ) + rviz_node = Node( package="rviz2", executable="rviz2", diff --git a/ros/package.xml b/ros/package.xml index 4e89867..526514e 100644 --- a/ros/package.xml +++ b/ros/package.xml @@ -38,6 +38,7 @@ SOFTWARE. rclcpp_components rcutils sensor_msgs + laser_geometry std_msgs std_srvs tf2_ros diff --git a/ros/src/kinematic_icp_ros/nodes/offline_node.cpp b/ros/src/kinematic_icp_ros/nodes/offline_node.cpp index a7c9577..6120521 100644 --- a/ros/src/kinematic_icp_ros/nodes/offline_node.cpp +++ b/ros/src/kinematic_icp_ros/nodes/offline_node.cpp @@ -29,8 +29,11 @@ #include // ROS +#include #include #include +#include +#include #include "kinematic_icp_ros/nodes/offline_node.hpp" #include "kinematic_icp_ros/server/LidarOdometryServer.hpp" @@ -51,15 +54,23 @@ namespace kinematic_icp_ros { OfflineNode::OfflineNode(const rclcpp::NodeOptions &options) { node_ = rclcpp::Node::make_shared("kinematic_icp_offline_node", options); - pcl_topic_ = node_->declare_parameter("input"); + lidar_topic_ = node_->declare_parameter("lidar_topic"); + use_2d_lidar_ = node_->declare_parameter("use_2d_lidar"); odometry_server_ = std::make_shared(node_); + if (use_2d_lidar_) { + RCLCPP_INFO_STREAM(node_->get_logger(), + "Started in 2D scanner mode with topic: " << lidar_topic_); + } else { + RCLCPP_INFO_STREAM(node_->get_logger(), + "Started in 3D Lidar mode with topic: " << lidar_topic_); + } auto bag_filename = node_->declare_parameter("bag_filename"); const auto poses_filename = generateOutputFilename(bag_filename); output_pose_file_ = std::filesystem::path(node_->declare_parameter("output_dir")); output_pose_file_ /= poses_filename; auto tf_bridge = std::make_shared(node_); - bag_multiplexer_.AddBag(BufferableBag(bag_filename, tf_bridge, pcl_topic_)); + bag_multiplexer_.AddBag(BufferableBag(bag_filename, tf_bridge, lidar_topic_)); } void OfflineNode::writePosesInTumFormat() { @@ -98,14 +109,30 @@ void OfflineNode::Run() { indicators::option::ShowRemainingTime{true}, indicators::option::Stream{std::cout}, indicators::option::MaxProgress{bag_multiplexer_.message_count()}}; + + auto lidar2d_to_3d = [this](const sensor_msgs::msg::LaserScan::SharedPtr &msg) { + auto projected_scan = std::make_shared(); + laser_projector_.projectLaser(*msg, *projected_scan, -1.0, + laser_geometry::channel_option::Timestamp); + return projected_scan; + }; // Deserialize the next pointcloud message from the bagfiles - auto GetNextMsg = [this] { - const rclcpp::Serialization pcl2_serializer; - const auto pc_msg = std::make_shared(); - const auto &msg = bag_multiplexer_.GetNextMessage(); - rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); - pcl2_serializer.deserialize_message(&serialized_msg, pc_msg.get()); - return pc_msg; + auto GetNextMsg = [&] { + if (use_2d_lidar_) { + const rclcpp::Serialization serializer; + const auto lidar_msg = std::make_shared(); + const auto &msg = bag_multiplexer_.GetNextMessage(); + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + serializer.deserialize_message(&serialized_msg, lidar_msg.get()); + return lidar2d_to_3d(lidar_msg); + } else { + const rclcpp::Serialization serializer; + const auto lidar_msg = std::make_shared(); + const auto &msg = bag_multiplexer_.GetNextMessage(); + rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); + serializer.deserialize_message(&serialized_msg, lidar_msg.get()); + return lidar_msg; + } }; // This is the main blocking loop that this simulates the subscription form the online node, diff --git a/ros/src/kinematic_icp_ros/nodes/online_node.cpp b/ros/src/kinematic_icp_ros/nodes/online_node.cpp index a5dfeb1..96a8260 100644 --- a/ros/src/kinematic_icp_ros/nodes/online_node.cpp +++ b/ros/src/kinematic_icp_ros/nodes/online_node.cpp @@ -21,12 +21,15 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. #include +#include #include // ROS +#include #include #include #include +#include #include #include "kinematic_icp_ros/nodes/online_node.hpp" @@ -36,13 +39,32 @@ namespace kinematic_icp_ros { OnlineNode ::OnlineNode(const rclcpp::NodeOptions &options) { node_ = rclcpp::Node::make_shared("kinematic_icp_online_node", options); - pcl_topic_ = node_->declare_parameter("input"); + lidar_topic_ = node_->declare_parameter("lidar_topic"); odometry_server_ = std::make_shared(node_); - pointcloud_sub_ = node_->create_subscription( - pcl_topic_, rclcpp::SystemDefaultsQoS(), - [&](const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { - odometry_server_->RegisterFrame(msg); - }); + const bool use_2d_lidar = node_->declare_parameter("use_2d_lidar"); + if (use_2d_lidar) { + RCLCPP_INFO_STREAM(node_->get_logger(), + "Started in 2D scanner mode with topic: " << lidar_topic_); + laser_scan_sub_ = node_->create_subscription( + lidar_topic_, rclcpp::SystemDefaultsQoS(), + [&](const sensor_msgs::msg::LaserScan::ConstSharedPtr &msg) { + const sensor_msgs::msg::PointCloud2::ConstSharedPtr lidar_msg = [&]() { + auto projected_scan = std::make_shared(); + laser_projector_.projectLaser(*msg, *projected_scan, -1.0, + laser_geometry::channel_option::Timestamp); + return projected_scan; + }(); + odometry_server_->RegisterFrame(lidar_msg); + }); + } else { + RCLCPP_INFO_STREAM(node_->get_logger(), + "Started in 3D Lidar mode with topic: " << lidar_topic_); + pointcloud_sub_ = node_->create_subscription( + lidar_topic_, rclcpp::SystemDefaultsQoS(), + [&](const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { + odometry_server_->RegisterFrame(msg); + }); + } } rclcpp::node_interfaces::NodeBaseInterface::SharedPtr OnlineNode::get_node_base_interface() { diff --git a/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp b/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp index d8ce8bb..f1c5bf8 100644 --- a/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp +++ b/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp @@ -42,13 +42,14 @@ using Header = std_msgs::msg::Header; std::optional GetTimestampField(const PointCloud2::ConstSharedPtr msg) { PointField timestamp_field; for (const auto &field : msg->fields) { - if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) { + if ((field.name == "t" || field.name == "timestamp" || field.name == "time" || + field.name == "stamps")) { timestamp_field = field; } } if (timestamp_field.count) return timestamp_field; RCLCPP_WARN_ONCE(rclcpp::get_logger("kinematic_icp_ros"), - "Field 't', 'timestamp', or 'time' does not exist. " + "Field 't', 'timestamp', 'time', or 'stamps' does not exist. " "Disabling scan deskewing"); return {}; }