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 <[email protected]> * 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 <[email protected]> * 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 <[email protected]> * Renaming according to Ben's review * Why a ref * Use the same default --------- Co-authored-by: Benedikt Mersch <[email protected]> Co-authored-by: Benedikt Mersch <[email protected]> --------- Co-authored-by: Benedikt Mersch <[email protected]> Co-authored-by: Benedikt Mersch <[email protected]> * 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 <[email protected]> Co-authored-by: Benedikt Mersch <[email protected]>
PRBonn · Nov 22, 2024 · dd0195c · dd0195c
1 parent 59bc9fc
commit dd0195c
Show file tree

Hide file tree

Showing 10 changed files with 90 additions and 21 deletions.
diff --git 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
@@ -27,6 +27,7 @@
 
 // ROS
 #include <filesystem>
+#include <laser_geometry/laser_geometry.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/node_options.hpp>
 #include <sophus/se3.hpp>
@@ -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<LidarOdometryServer> odometry_server_;
     rclcpp::Node::SharedPtr node_;
     // store data for the experiments
     using PoseWithStamp = std::pair<double, Sophus::SE3d>;
     std::vector<PoseWithStamp> 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
@@ -26,7 +26,9 @@
 #include <string>
 
 // ROS
+#include <laser_geometry/laser_geometry.hpp>
 #include <rclcpp/node.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
 #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<LidarOdometryServer> odometry_server_;
     rclcpp::Node::SharedPtr node_;
+    laser_geometry::LaserProjection laser_projector_;
 
     // Online node specifics
     rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
+    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_scan_sub_;
 };
 
 }  // namespace kinematic_icp_ros
diff --git 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
@@ -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
@@ -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
@@ -38,6 +38,7 @@ SOFTWARE.
   <depend>rclcpp_components</depend>
   <depend>rcutils</depend>
   <depend>sensor_msgs</depend>
+  <depend>laser_geometry</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>

diff --git 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 <vector>
 
 // ROS
+#include <laser_geometry/laser_geometry.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/node_options.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 
 #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<std::string>("input");
+    lidar_topic_ = node_->declare_parameter<std::string>("lidar_topic");
+    use_2d_lidar_ = node_->declare_parameter<bool>("use_2d_lidar");
     odometry_server_ = std::make_shared<LidarOdometryServer>(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<std::string>("bag_filename");
     const auto poses_filename = generateOutputFilename(bag_filename);
     output_pose_file_ = std::filesystem::path(node_->declare_parameter<std::string>("output_dir"));
     output_pose_file_ /= poses_filename;
     auto tf_bridge = std::make_shared<BufferableBag::TFBridge>(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<sensor_msgs::msg::PointCloud2>();
+        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<sensor_msgs::msg::PointCloud2> pcl2_serializer;
-        const auto pc_msg = std::make_shared<sensor_msgs::msg::PointCloud2>();
-        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<sensor_msgs::msg::LaserScan> serializer;
+            const auto lidar_msg = std::make_shared<sensor_msgs::msg::LaserScan>();
+            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<sensor_msgs::msg::PointCloud2> serializer;
+            const auto lidar_msg = std::make_shared<sensor_msgs::msg::PointCloud2>();
+            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
@@ -21,12 +21,15 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 // SOFTWARE.
 #include <memory>
+#include <rclcpp/logging.hpp>
 #include <string>
 
 // ROS
+#include <laser_geometry/laser_geometry.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/node_options.hpp>
 #include <rclcpp/subscription.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
 #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<std::string>("input");
+    lidar_topic_ = node_->declare_parameter<std::string>("lidar_topic");
     odometry_server_ = std::make_shared<LidarOdometryServer>(node_);
-    pointcloud_sub_ = node_->create_subscription<sensor_msgs::msg::PointCloud2>(
-        pcl_topic_, rclcpp::SystemDefaultsQoS(),
-        [&](const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) {
-            odometry_server_->RegisterFrame(msg);
-        });
+    const bool use_2d_lidar = node_->declare_parameter<bool>("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<sensor_msgs::msg::LaserScan>(
+            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<sensor_msgs::msg::PointCloud2>();
+                    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<sensor_msgs::msg::PointCloud2>(
+            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
@@ -42,13 +42,14 @@ using Header = std_msgs::msg::Header;
 std::optional<PointField> 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 {};
 }