Feature/port ndt matching ros2

core_perception/lidar_localizer_ros2/CMakeLists.txt

@@ -0,0 +1,75 @@
+# Copyright (C) 2023 LEIDOS.
+#
+# 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.
+
+cmake_minimum_required(VERSION 3.5)
+project(lidar_localizer_ros2)
+
+# Declare carma package and check ROS version
+find_package(carma_cmake_common REQUIRED)
+carma_check_ros_version(2)
+carma_package()
+
+find_package(autoware_build_flags REQUIRED)
+find_package(PCL COMPONENTS REQUIRED)
+find_package(CUDA)
+find_package(Eigen3 QUIET)
+
+AW_CHECK_CUDA()
+
+if(USE_CUDA)
+  add_definitions(-DCUDA_FOUND)
+  list(APPEND PCL_OPENMP_PACKAGES ndt_gpu_ros2)
+endif()
+
+if(NOT EIGEN3_FOUND)
+  # Fallback to cmake_modules
+  find_package(cmake_modules REQUIRED)
+  find_package(Eigen REQUIRED)
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+  set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
+  # Possibly map additional variables to the EIGEN3_ prefix.
+else()
+  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+endif()
+
+
+## Find dependencies using ament auto
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+
+
+# Includes
+include_directories(
+  ${PCL_INCLUDE_DIRS}
+  ${CUDA_INCLUDE_DIRS}
+  include
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+ament_auto_add_library(ndt_matching_lib SHARED
+  src/ndt_matching.cpp)
+
+rclcpp_components_register_nodes(ndt_matching_lib "ndt_matching::NDTMatching")
+
+target_link_libraries(ndt_matching_lib
+${PCL_LIBRARIES}
+)
+
+
+
+
+ament_auto_package(
+  INSTALL_TO_SHARE launch
+)

core_perception/lidar_localizer_ros2/include/ndt_matching.hpp

@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) 2023 LEIDOS.
+ *
+ * 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 <pthread.h>
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <string>
+
+#include <boost/filesystem.hpp>
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <velodyne_pointcloud/rawdata.hpp>
+
+#include <tf2_ros/static_transform_broadcaster.h>
+#include "tf2_ros/transform_broadcaster.h"
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/registration/ndt.h>
+
+#include <ndt_cpu_ros2/NormalDistributionsTransform.h>
+#ifdef CUDA_FOUND
+#include <ndt_gpu_ros2/NormalDistributionsTransform.h>
+#endif
+#include <autoware_config_msgs/msg/config_ndt.hpp>
+#include <autoware_msgs/msg/ndt_stat.hpp>
+
+#include <carma_ros2_utils/carma_lifecycle_node.hpp>
+
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+#define PREDICT_POSE_THRESHOLD 0.5
+
+#define Wa 0.4
+#define Wb 0.3
+#define Wc 0.3
+
+namespace ndt_matching{
+struct pose
+{
+    double x;
+    double y;
+    double z;
+    double roll;
+    double pitch;
+    double yaw;
+};
+
+enum class MethodType
+{
+    PCL_GENERIC = 0,
+    PCL_ANH = 1,
+    PCL_ANH_GPU = 2,
+    PCL_OPENMP = 3,
+};
+
+class NDTMatching : public carma_ros2_utils::CarmaLifecycleNode {
+
+    private:
+
+        // Define publishers                                        
+        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr predict_pose_pub;
+        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr predict_pose_imu_pub;
+        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr predict_pose_odom_pub;
+        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr predict_pose_imu_odom_pub;
+        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr ndt_pose_pub;
+
+        // current_pose is published by vel_pose_mux
+        // static ros::Publisher current_pose_pub;
+        // static geometry_msgs::PoseStamped current_pose_msg;
+
+
+        rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr localizer_pose_pub;
+        rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr estimate_twist_pub;
+        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr estimated_vel_mps_pub, estimated_vel_kmph_pub;
+        rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr estimated_vel_pub;
+        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr time_ndt_matching_pub;
+        rclcpp::Publisher<autoware_msgs::msg::NDTStat>::SharedPtr ndt_stat_pub;
+        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr ndt_reliability_pub;
+
+        // Define subscribers
+        rclcpp::Subscription<autoware_config_msgs::msg::ConfigNDT>::SharedPtr param_sub;
+        rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr gnss_sub;
+        rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initialpose_sub;
+        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr points_sub;
+        rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub;
+        rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub;
+        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_sub;
+
+        int method_type_tmp = 0;
+
+        void param_callback(const autoware_config_msgs::msg::ConfigNDT::SharedPtr input);
+        void map_callback(const sensor_msgs::msg::PointCloud2::SharedPtr input);
+        void gnss_callback(const geometry_msgs::msg::PoseStamped::SharedPtr input);
+        void initialpose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr input);
+        void points_callback(const sensor_msgs::msg::PointCloud2::SharedPtr input);
+        void odom_callback(const nav_msgs::msg::Odometry::SharedPtr input);
+
+        void imu_odom_calc(rclcpp::Time current_time);
+        void odom_calc(rclcpp::Time current_time);
+        void imu_calc(rclcpp::Time current_time);
+
+        double wrapToPm(double a_num, const double a_max);
+        double wrapToPmPi(const double a_angle_rad);
+        double calcDiffForRadian(const double lhs_rad, const double rhs_rad);
+
+        void imuUpsideDown(const sensor_msgs::msg::Imu::SharedPtr input);
+        void imu_callback(const sensor_msgs::msg::Imu::SharedPtr input);
+        pose convertPoseIntoRelativeCoordinate(const pose &target_pose, const pose &reference_pose);
+
+
+        // Note: the function below and its definitions were taken from pcl_ros package, to support local use while the package is being ported to ros2
+        // https://github.com/ros-perception/perception_pcl.git
+        template<typename PointT>
+        void transformPointCloud(const pcl::PointCloud<PointT> & cloud_in, pcl::PointCloud<PointT> & cloud_out, const tf2::Transform & transform);
+
+    public:
+        explicit NDTMatching(const rclcpp::NodeOptions &);
+        carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state);
+
+};
+}
+

core_perception/lidar_localizer_ros2/launch/ndt_matching.launch.py

@@ -0,0 +1,112 @@
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace
+
+import os
+
+def generate_launch_description():
+
+    log_level = LaunchConfiguration('log_level')
+    declare_log_level_arg = DeclareLaunchArgument(name = 'log_level', default_value='WARN')
+
+    method_type = LaunchConfiguration('method_type')
+    declare_method_type = DeclareLaunchArgument(name='method_type', default_value='0')
+
+    use_gnss = LaunchConfiguration('use_gnss')
+    declare_use_gnss = DeclareLaunchArgument(name='use_gnss', default_value='1')
+
+    use_odom = LaunchConfiguration('use_odom')
+    declare_use_odom = DeclareLaunchArgument(name='use_odom', default_value="false")
+
+    use_imu = LaunchConfiguration('use_imu')
+    declare_use_imu = DeclareLaunchArgument(name='use_imu', default_value="false")
+
+    imu_upside_down = LaunchConfiguration('imu_upside_down')
+    declare_imu_upside_down = DeclareLaunchArgument(name='imu_upside_down', default_value='false')
+
+    imu_topic = LaunchConfiguration('imu_topic')
+    declare_imu_topic = DeclareLaunchArgument(name='imu_topic', default_value="/imu_raw")
+
+    queue_size = LaunchConfiguration('queue_size')
+    declare_queue_size = DeclareLaunchArgument(name='queue_size', default_value='1')
+
+    offset = LaunchConfiguration('offset')
+    declare_offset = DeclareLaunchArgument(name='offset', default_value="linear")
+
+    get_height = LaunchConfiguration('get_height')
+    declare_get_height = DeclareLaunchArgument(name='get_height', default_value="false")
+
+    use_local_transform = LaunchConfiguration('use_local_transform')
+    declare_use_local_transform = DeclareLaunchArgument(name='use_local_transform', default_value="false")
+
+    sync = LaunchConfiguration('sync')
+    declare_sync = DeclareLaunchArgument(name='sync', default_value="false")
+
+    output_log_data = LaunchConfiguration('output_log_data')
+    declare_output_log_data = DeclareLaunchArgument(name='output_log_data', default_value="false")
+
+    gnss_reinit_fitness = LaunchConfiguration('gnss_reinit_fitness')
+    declare_gnss_reinit_fitness = DeclareLaunchArgument(name='gnss_reinit_fitness', default_value="500.0")
+
+    base_frame = LaunchConfiguration('base_frame')
+    declare_base_frame = DeclareLaunchArgument(name='base_frame', default_value="base_link")
+
+
+    container = ComposableNodeContainer(
+        package='carma_ros2_utils',
+        name='ndt_matching_container',
+        namespace=GetCurrentNamespace(),
+        executable='carma_component_container_mt',
+        composable_node_descriptions=[
+
+            ComposableNode(
+                package='lidar_localizer_ros2',
+                plugin='ndt_matching::NDTMatching',
+                name='ndt_matching',
+                extra_arguments=[
+                    {'use_intra_process_comms': True},
+                    {'log_level':log_level}
+                ],
+                parameters=[
+                    {'method_type': method_type},
+                    {'use_gnss': use_gnss},
+                    {'use_odom': use_odom},
+                    {'use_imu': use_imu},
+                    {'imu_upside_down': imu_upside_down},
+                    {'imu_topic': imu_topic},
+                    {'queue_size' : queue_size},
+                    {'offset': offset},
+                    {'get_height': get_height},
+                    {'use_local_transform': use_local_transform},
+                    {'sync': sync},
+                    {'output_log_data': output_log_data},
+                    {'gnss_reinit_fitness': gnss_reinit_fitness},
+                    {'base_frame': base_frame}
+                    ]
+            ),
+        ]
+    )
+
+    return LaunchDescription([
+        declare_log_level_arg,
+        declare_method_type,
+        declare_use_gnss,
+        declare_use_odom,
+        declare_use_imu,
+        declare_imu_upside_down,
+        declare_imu_topic,
+        declare_queue_size,
+        declare_offset,
+        declare_get_height,
+        declare_use_local_transform,
+        declare_sync,
+        declare_output_log_data,
+        declare_gnss_reinit_fitness,
+        declare_base_frame,
+        container
+    ])