Add 3D lidar slam for alternative

.gitmodules

@@ -9,6 +9,6 @@
 	path = src/hardware/QUTMS_Embedded_Common
 	url = https://github.com/QUT-Motorsport/QUTMS_Embedded_Common
 	branch = comp22
-[submodule "src/navigation/QUTMS_Nav_Integration"]
-	path = src/navigation/QUTMS_Nav_Integration
-	url = https://github.com/b1n-ch1kn/QUTMS_Nav_Integration
+[submodule "src/navigation/lidarslam_ros2"]
+	path = src/navigation/lidarslam_ros2
+	url = https://github.com/QUT-Motorsport/lidarslam_ros2.git

docker/Dockerfile.roscube

@@ -11,7 +11,9 @@ WORKDIR /tmp
 # installs for controllers?
 RUN apt-get update && \
     apt-get install -y --no-install-recommends \
-        libgl1-mesa-glx can-utils net-tools \
+        can-utils net-tools usbutils udev mesa-common-dev \
+        # lots of dependencies for opengl
+        libgl1-mesa-glx libgl1-mesa-dev libglu1-mesa-dev libegl1-mesa-dev libgl-dev \
     && rm -rf /var/lib/apt/lists/* \
     && apt-get clean
 
@@ -28,12 +30,6 @@ RUN source /home/${USERNAME}/mambaforge/bin/activate; \
     mamba install -y --file /tmp/requirements.conda &&\
     rm -f /tmp/requirements.conda
 
-COPY src/navigation/QUTMS_Nav_Integration/conda_requirements /tmp/requirements.conda
-RUN source /home/${USERNAME}/mambaforge/bin/activate; \
-    conda activate driverless_env &&\
-    mamba install -y --file /tmp/requirements.conda &&\
-    rm -f /tmp/requirements.conda
-
 USER ${USERNAME}
 
 # copy in source
@@ -43,7 +39,14 @@ COPY --chown=${USERNAME}:${HOST_GROUP} ./src/ ./src
 RUN source /home/${USERNAME}/mambaforge/bin/activate; \
     conda activate driverless_env &&\
     colcon build --symlink-install --packages-up-to \
-        roscube_machine qutms_nav2 cone_association
+        roscube_machine qutms_nav2 nav_interfaces lidarslam --cmake-args\
+        ' -DOPENGL_gl_LIBRARY=/usr/lib/x86_64-linux-gnu/libGL.so' \
+        ' -DOPENGL_opengl_LIBRARY=/usr/lib/x86_64-linux-gnu/libGL.so' \
+        ' -DOPENGL_glu_LIBRARY=/usr/lib/x86_64-linux-gnu/libGLU.so' \
+        ' -DOPENGL_egl_LIBRARY=/usr/lib/x86_64-linux-gnu/libEGL.so' \
+        ' -DOPENGL_glx_LIBRARY=/usr/lib/x86_64-linux-gnu/libGLX.so' \
+        ' -DOPENGL_INCLUDE_DIR=/usr/include/GL' \
+        ' -DEGL_INCLUDE_DIR=/usr/include/EGL'
 
 # delete source
 RUN rm -rf ./src

src/machines/roscube_machine/roscube_requirements.conda

@@ -1,6 +1,8 @@
 cython
 dataclasses
 eigen
+g2o
+qt
 matplotlib
 pycurl
 opencv
@@ -10,7 +12,12 @@ ros-humble-tf-transformations
 ros-humble-velodyne
 ros-humble-geodesy
 ros-humble-xacro
+ros-humble-pcl-ros
 ros-humble-robot-state-publisher
+ros-humble-robot-localization
+ros-humble-slam-toolbox
+ros-humble-navigation2
+ros-humble-nav2-bringup
 scipy
 scikit-learn
 transforms3d

src/navigation/nav_bringup/config/lidar_slam_params.yaml

@@ -0,0 +1,63 @@
+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

src/navigation/nav_bringup/launch/lidar_slam.launch.py

@@ -0,0 +1,47 @@
+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,
+        ]
+    )