diff --git a/.gitmodules b/.gitmodules index 99c466699..3f5326f9e 100644 --- a/.gitmodules +++ b/.gitmodules @@ -5,6 +5,3 @@ path = src/hardware/QUTMS_Embedded_Common url = https://github.com/QUT-Motorsport/QUTMS_Embedded_Common branch = comp22 -[submodule "src/navigation/lidarslam_ros2"] - path = src/navigation/lidarslam_ros2 - url = https://github.com/QUT-Motorsport/lidarslam_ros2.git diff --git a/src/navigation/lidarslam_ros2 b/src/navigation/lidarslam_ros2 deleted file mode 160000 index a913f64eb..000000000 --- a/src/navigation/lidarslam_ros2 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit a913f64ebf533c319004a8df1847320419a0e777 diff --git a/src/navigation/nav_bringup/config/lidar_slam_params.yaml b/src/navigation/nav_bringup/config/lidar_slam_params.yaml deleted file mode 100644 index 4eb50ab75..000000000 --- a/src/navigation/nav_bringup/config/lidar_slam_params.yaml +++ /dev/null @@ -1,63 +0,0 @@ -scan_matcher: - ros__parameters: - use_sim_time: true - - global_frame_id: "track" - odom_frame_id: "odom" - robot_frame_id: "base_footprint" - - initial_pose_topic: "/initialpose" - input_cloud_topic: "/velodyne_points" - imu_topic: "/imu/data" - map_topic: "/slam/points_map" - map_array_topic: "/slam/points_map_array" - path_topic: "/slam/trajectory" - pose_topic: "/slam/car_pose" - - registration_method: "NDT" # "NDT" or "GICP" - ndt_resolution: 0.05 # resolution size of voxel[m] - ndt_num_threads: 2 # threads using ndt(if 0 is set, maximum alloawble threads are used - gicp_corr_dist_threshold: 1.0 # the distance threshold between the two corresponding points of the source and target[m] - trans_for_mapupdate: 0.05 # moving distance of map update[m] - vg_size_for_input: 0.05 # down sample size of input cloud[m] - vg_size_for_map: 0.05 # down sample size of map cloud[m] - use_min_max_filter: true - scan_min_range: 1.0 - scan_max_range: 30.0 - scan_period: 0.1 - map_publish_period: 0.2 - num_targeted_cloud: 200 # number of targeted cloud in registration(The higher this number, the less distortion.) - set_initial_pose: true - initial_pose_x: 0.0 - initial_pose_y: 0.0 - initial_pose_z: 0.0 - initial_pose_qx: 0.0 - initial_pose_qy: 0.0 - initial_pose_qz: 0.0 - initial_pose_qw: 1.0 - publish_tf: true - use_imu: true - use_odom: true - debug_flag: true - -graph_based_slam: - ros__parameters: - use_sim_time: true - - map_array_topic: "/slam/points_map_array" - modified_map_topic: "/slam/modified_points_map" - modified_map_array_topic: "/slam/modified_points_map_array" - modified_path_topic: "/slam/modified_trajectory" - - registration_method: "NDT" # "NDT" or "GICP" - ndt_resolution: 0.05 # resolution size of voxel[m] - ndt_num_threads: 2 # threads using ndt(if 0 is set, maximum alloawble threads are used - voxel_leaf_size: 0.05 # down sample size of input cloud[m] - loop_detection_period: 1000 # period of searching loop detection[ms] - threshold_loop_closure_score: 0.5 # fitness score of ndt for loop clousure - distance_loop_closure: 3.0 # distance far from revisit candidates for loop clousure[m] - range_of_searching_loop_closure: 2.0 # search radius for candidate points from the present for loop closure[m] - search_submap_num: 5 # the number of submap points before and after the revisit point used for registration - num_adjacent_pose_cnstraints: 5 # the number of constraints between successive nodes in a pose graph over time - use_save_map_in_loop: false # Whether to save the map when loop close(If the map saving process in loop close is too heavy and the self-position estimation fails, set this to false.) - debug_flag: true diff --git a/src/navigation/nav_bringup/launch/lidar_slam.launch.py b/src/navigation/nav_bringup/launch/lidar_slam.launch.py deleted file mode 100644 index 3232748c5..000000000 --- a/src/navigation/nav_bringup/launch/lidar_slam.launch.py +++ /dev/null @@ -1,47 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_path -import launch -from launch_ros.actions import Node - - -def generate_launch_description(): - pkg_share = get_package_share_path("nav_bringup") - - # Community ROS 2 packages - localisation_node = Node( - package="robot_localization", - executable="ekf_node", - name="odom_filter_node", - output="screen", - parameters=[ - os.path.join(pkg_share, "config/localisation_params.yaml"), - ], - ) - - mapping = Node( - package="scanmatcher", - executable="scanmatcher_node", - output="screen", - remappings=[("/input_cloud", "/velodyne_points")], - parameters=[ - os.path.join(pkg_share, "config/lidar_slam_params.yaml"), - ], - ) - - graphbasedslam = Node( - package="graph_based_slam", - executable="graph_based_slam_node", - output="screen", - parameters=[ - os.path.join(pkg_share, "config/lidar_slam_params.yaml"), - ], - ) - - return launch.LaunchDescription( - [ - localisation_node, - mapping, - graphbasedslam, - ] - )