Feat/slam: 3D Lidar-Inertial SLAM pipeline with iSAM2 Factor Graph

urc_bringup/launch/sim.launch.py

@@ -24,7 +24,6 @@ def generate_launch_description():
     path_ros_gazebo_sim = get_package_share_directory("ros_gz_sim")
     path_urc_hw_description = get_package_share_directory("urc_hw_description")
     path_urc_bringup = get_package_share_directory("urc_bringup")
-    path_urc_localization = get_package_share_directory("urc_localization")
 
     controller_config_file_dir = os.path.join(
         path_urc_bringup, "config", "test_controllers.yaml"
@@ -197,12 +196,6 @@ def generate_launch_description():
         output="screen",
     )
 
-    launch_ekf = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(path_urc_localization, "launch", "ekf.launch.py")
-        )
-    )
-
     launch_autonomy = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(path_urc_bringup, "launch", "autonomy.launch.py")
@@ -355,7 +348,6 @@ def generate_launch_description():
             robot_state_publisher_node,
             covariances_on_imu,
             covariances_on_gps,
-            launch_ekf,
             launch_autonomy,
             rocker_tf_broadcaster,
             rocker_effort_pid_node,

urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro

@@ -50,13 +50,13 @@
         <lidar>
           <scan>
             <horizontal>
-              <samples>2880</samples>
+              <samples>360</samples>
               <resolution>1</resolution>
               <min_angle>-3.14</min_angle>
               <max_angle>3.14</max_angle>
             </horizontal>
             <vertical>
-              <samples>144</samples>
+              <samples>16</samples>
               <resolution>1</resolution>
               <min_angle>-0.366519</min_angle>
               <max_angle>0.366519</max_angle>

urc_localization/CMakeLists.txt

@@ -25,7 +25,6 @@ include_directories(
 )
 
 add_library(${PROJECT_NAME} SHARED
-  src/gps_imu_localizer.cpp
   src/covariances_on_imu.cpp
   src/covariances_on_gps.cpp
   src/ground_truth.cpp
@@ -52,12 +51,6 @@ ament_target_dependencies(${PROJECT_NAME}
 )
 
 
-rclcpp_components_register_node(
-  ${PROJECT_NAME}
-  PLUGIN "gps_imu_localizer::GpsImuLocalizer"
-  EXECUTABLE ${PROJECT_NAME}_GpsImuLocalizer
-)
-
 rclcpp_components_register_node(
   ${PROJECT_NAME}
   PLUGIN "covariances_on_imu::CovariancesOnImu"

urc_slam/CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 3.5)
+project(urc_slam)
+
+include(../cmake/default_settings.cmake)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
+
+find_package(Eigen3 REQUIRED)
+cmake_policy(SET CMP0074 NEW)
+find_package(PCL REQUIRED COMPONENTS
+  common
+  filters
+  registration
+)
+find_package(GTSAM REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+  src/slam_backend.cpp
+  src/lidar_frontend.cpp
+  src/slam_node.cpp
+)
+
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+    ${PCL_INCLUDE_DIRS}
+)
+
+target_compile_definitions(${PROJECT_NAME}
+  PRIVATE
+    ${PCL_DEFINITIONS}
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  nav_msgs
+  geometry_msgs
+  tf2_ros
+  pcl_conversions
+)
+
+target_link_libraries(${PROJECT_NAME}
+  Eigen3::Eigen
+  gtsam
+  ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(
+  ${PROJECT_NAME}
+  PLUGIN "urc_slam::SlamNode"
+  EXECUTABLE SlamNode
+)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_targets(
+  export_${PROJECT_NAME}
+  HAS_LIBRARY_TARGET
+)
+
+ament_export_include_directories(include)
+
+ament_export_dependencies(
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  nav_msgs
+  geometry_msgs
+  tf2_ros
+  pcl_conversions
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

urc_slam/config/slam_params.yaml

@@ -0,0 +1,37 @@
+slam_node:
+  ros__parameters:
+    use_sim_time: true
+    imu_topic: /imu/fused
+    lidar_topic: /scan/points
+    slam_odom_topic: /slam/odometry
+
+    map_frame: map
+    base_link_frame: base_link
+
+    keyframe:
+      translation_threshold_m: 0.5
+      rotation_threshold_rad: 0.174533
+
+    lidar_frontend:
+      voxel_size_m: 0.2
+      minimum_range_m: 1.0
+      maximum_range_m: 80.0
+      maximum_correspondence_distance_m: 2.0
+      maximum_iterations: 64
+      transformation_epsilon: 1.0e-6
+      fitness_epsilon: 1.0e-6
+      maximum_fitness_score: 2.5
+
+slam_navsat_transform:
+  ros__parameters:
+    use_sim_time: true
+    frequency: 10.0
+    delay: 0.0
+    zero_altitude: false
+    magnetic_declination_radians: 0.0
+    yaw_offset: 0.0
+    use_odometry_yaw: false
+    wait_for_datum: false
+    publish_filtered_gps: false
+    broadcast_utm_transform: false
+    broadcast_cartesian_transform: false

urc_slam/include/lidar_frontend.hpp

@@ -0,0 +1,58 @@
+#ifndef LIDAR_FRONTEND_HPP_
+#define LIDAR_FRONTEND_HPP_
+
+#include <limits>
+
+#include <gtsam/geometry/Pose3.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace urc_slam {
+
+    struct LidarRegistrationResult {
+        bool converged = false;
+        double fitness_score = std::numeric_limits<double>::infinity();
+        gtsam::Pose3 relative_pose;
+    };
+
+    class LidarFrontend {
+        public:
+            using Point = pcl::PointXYZ;
+            using Cloud = pcl::PointCloud<Point>;
+
+            LidarFrontend(
+                double voxel_size_m,
+                double minimum_range_m,
+                double maximum_range_m,
+                double maximum_correspondence_distance_m,
+                int maximum_iterations,
+                double transformation_epsilon,
+                double fitness_epsilon,
+                const gtsam::Pose3 &base_T_lidar
+            );
+
+            Cloud::Ptr preprocess(
+                const Cloud::ConstPtr &cloud
+            ) const;
+
+            LidarRegistrationResult registerClouds(
+                const Cloud::ConstPtr &previous_cloud,
+                const Cloud::ConstPtr &current_cloud,
+                const gtsam::Pose3 &initial_base_relative_pose
+            ) const;
+        private:
+            double voxel_size_m_;
+            double minimum_range_m_;
+            double maximum_range_m_;
+            double maximum_correspondence_distance_m_;
+            int maximum_iterations_;
+            double transformation_epsilon_;
+            double fitness_epsilon_;
+
+            // Convert between base link and Lidar frame
+            gtsam::Pose3 base_T_lidar_;
+    };
+
+
+} 
+#endif