robotics-laboratory · apmilko · Aug 24, 2024
diff --git a/packages/localization/CMakeLists.txt b/packages/localization/CMakeLists.txt
@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.8)
+project(localization)
+
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geom REQUIRED)
+find_package(common REQUIRED)
+find_package(libpointmatcher REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(rosbag2_cpp REQUIRED)
+find_package(
+  Boost
+  COMPONENTS thread
+             filesystem
+             system
+             program_options
+             date_time
+             chrono
+             REQUIRED
+)
+
+add_executable(
+  ${PROJECT_NAME}_node src/main.cpp src/localization_node.cpp
+                       src/conversion.cpp
+)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}_node
+  rclcpp
+  geom
+  common
+  libpointmatcher
+  tf2_ros
+  tf2_msgs
+  tf2_geometry_msgs
+  sensor_msgs
+  visualization_msgs
+  pcl_conversions
+  rosbag2_cpp
+  Boost
+)
+
+install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME})
+
+target_include_directories(
+  ${PROJECT_NAME}_node
+  PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+         "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
+         "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
+
+ament_package()
diff --git a/packages/localization/config/icp.yaml b/packages/localization/config/icp.yaml
@@ -0,0 +1,32 @@
+readingDataPointsFilters:
+  - RandomSamplingDataPointsFilter:
+      prob: 0.5
+
+referenceDataPointsFilters:
+  - SamplingSurfaceNormalDataPointsFilter:
+      knn: 30
+
+matcher:
+  KDTreeMatcher:
+    knn: 1
+
+outlierFilters:
+  - TrimmedDistOutlierFilter:
+      ratio: 0.7
+
+errorMinimizer:
+  PointToPlaneErrorMinimizer
+
+transformationCheckers:
+  - CounterTransformationChecker:
+      maxIterationCount: 30
+  - DifferentialTransformationChecker:
+      minDiffRotErr: 0.02
+      minDiffTransErr: 0.02
+      smoothLength: 4
+
+inspector:
+  NullInspector
+
+logger:
+  NullLogger
diff --git a/packages/localization/include/localization/conversion.h b/packages/localization/include/localization/conversion.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <pointmatcher/PointMatcher.h>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+using Matcher = PointMatcher<float>;
+using DataPoints = Matcher::DataPoints;
+
+namespace truck::localization {
+
+DataPoints toDataPoints(const sensor_msgs::msg::LaserScan& msg);
+
+DataPoints toDataPoints(const sensor_msgs::msg::PointCloud2& msg);
+
+sensor_msgs::msg::PointCloud2 toPointCloud2(
+    const std_msgs::msg::Header& header, const DataPoints& data_points);
+
+}  // namespace truck::localization
diff --git a/packages/localization/include/localization/localization_node.h b/packages/localization/include/localization/localization_node.h
@@ -0,0 +1,114 @@
+#pragma once
+
+#include "geom/vector.h"
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <pointmatcher/PointMatcher.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace truck::localization {
+
+using Matcher = PointMatcher<float>;
+using ICP = Matcher::ICP;
+using DataPoints = Matcher::DataPoints;
+
+struct LocalizationParams {
+    std::chrono::duration<double> period;
+    bool verbose;
+    std::string icp_config;
+
+    struct BBoxFilter {
+        bool enable;
+        double radius;
+        double z_min;
+        double z_max;
+    } bbox_filter;
+
+    struct LocalScan {
+        struct Rendering {
+            struct BBoxFiltered {
+                bool enable;
+            } bbox_filtered;
+        } rendering;
+    } local_scan;
+
+    struct GlobalScan {
+        std::string config;
+
+        struct Rendering {
+            struct Main {
+                bool enable;
+                std::chrono::duration<double> period;
+            } main;
+
+            struct BBoxFiltered {
+                bool enable;
+            } bbox_filtered;
+        } rendering;
+    } global_scan;
+};
+
+class LocalizationNode : public rclcpp::Node {
+  public:
+    LocalizationNode();
+
+  private:
+    void initializeParams();
+    void initializeTopicHandlers();
+
+    void loadScanGlobal();
+
+    void onReset(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
+    void onLocalScan(const sensor_msgs::msg::LaserScan::SharedPtr msg);
+
+    void makeLocalizationTick();
+
+    void publishTf();
+    void publishScanGlobal();
+
+    std::optional<tf2::Transform> getLatestTranform(
+        const std::string& source, const std::string& target);
+
+    struct Signals {
+        rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tf = nullptr;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr global_scan = nullptr;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr global_scan_bbox_filtered =
+            nullptr;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr local_scan_bbox_filtered =
+            nullptr;
+    } signals_;
+
+    struct Slots {
+        rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr local_scan = nullptr;
+        rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose = nullptr;
+    } slots_;
+
+    struct Timers {
+        rclcpp::TimerBase::SharedPtr main = nullptr;
+        rclcpp::TimerBase::SharedPtr global_scan = nullptr;
+    } timers_;
+
+    struct Scans {
+        struct Global {
+            DataPoints data_points;
+            sensor_msgs::msg::PointCloud2 point_cloud;
+        } global;
+        sensor_msgs::msg::LaserScan local;
+    } scans_;
+
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_ = nullptr;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_ = nullptr;
+
+    ICP icp_;
+    tf2::Transform tf_world_ekf_;
+
+    LocalizationParams params_;
+};
+
+}  // namespace truck::localization
diff --git a/packages/localization/launch/localization.yaml b/packages/localization/launch/localization.yaml
@@ -0,0 +1,43 @@
+launch:
+  - arg: { name: "simulation", default: "false" }
+  - arg: { name: "lidar_map_config", default: "" }
+  - node:
+      pkg: "localization"
+      exec: "localization_node"
+      name: "localization_node"
+      output: "log"
+      param:
+        - { name: "use_sim_time", value: "$(var simulation)" }
+        - { name: "period", value: 0.1 }
+        - { name: "verbose", value: true }
+        - { name: "icp_config", value: "$(find-pkg-share localization)/config/icp.yaml" }
+
+        - name: "bbox_filter"
+          param:
+            - { name: "enable", value: true }
+            - { name: "radius", value: 8.0 }
+            - { name: "z_min", value: 0.0 }
+            - { name: "z_max", value: 2.0 }
+
+        - name: "local_scan"
+          param:
+            - name: "rendering"
+              param:
+                - name: "bbox_filtered"
+                  param:
+                    - { name: "enable", value: true }
+
+        - name: "global_scan"
+          param:
+            - { name: "config", value: "$(var lidar_map_config)" }
+
+            - name: "rendering"
+              param:
+                - name: "main"
+                  param:
+                    - { name: "enable", value: true }
+                    - { name: "period", value: 1.0 }
+
+                - name: "bbox_filtered"
+                  param:
+                    - { name: "enable", value: true }
diff --git a/packages/localization/package.xml b/packages/localization/package.xml
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>localization</name>
+  <version>0.0.0</version>
+  <description>localization</description>
+  <maintainer email="[email protected]">apmilko</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>geom</depend>
+  <depend>common</depend>
+  <depend>libpointmatcher</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>rosbag2_cpp</depend>
+  <depend>Boost</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>