Add gripper control for simulation.

Also add caching to the apt-get installs

.devcontainer/jazzy/Dockerfile.jazzy

@@ -1,3 +1,4 @@
+# syntax=docker/dockerfile:1.7-labs
 FROM osrf/ros:jazzy-desktop-full
 ARG USERNAME=USERNAME
 ARG USER_UID=1000
@@ -19,13 +20,15 @@ RUN groupadd --gid $USER_GID $USERNAME \
     && rm -rf /var/lib/apt/lists/*
 
 # Make sure the base image is completely up to date
-RUN apt-get update \
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    apt-get update \
     && apt-get upgrade -y \
     && rm -rf /var/lib/apt/lists/*
 
 
 # C++ development tools
-RUN apt-get update \
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    apt-get update \
     && apt-get install -y --no-install-recommends \
     build-essential \
     cmake \
@@ -37,7 +40,8 @@ RUN apt-get update \
     && rm -rf /var/lib/apt/lists/*
 
 # Python development tools
-RUN apt-get update \
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    apt-get update \
     && apt-get install -y --no-install-recommends \
     python3-pip \
     python3-dev \
@@ -48,21 +52,20 @@ RUN apt-get update \
     python3-vcstool \
     && rm -rf /var/lib/apt/lists/*
 
-# system deps from frozen rosdep manifest — no rosdep at build or startup
-# NOTE: There should be a check in your CI that this file is up to date.
-# Something like: 
-#   ./scripts/freeze-rosdep.sh && git diff --exit-code rosdep-packages.txt
-COPY rosdep-packages.txt /tmp/rosdep-packages.txt
-# The --mount=type=cache option caches the apt packages with buildkit
-# The id-apt-cache-${TARGETARCH} option allows for separate caches for different architectures
-# this enables parallel builds for different architectures without cache conflicts
+# Copy boilerplate project config into workspace so it can be parsed by rosdep.
+# This will be obscured by the volume mount or the workspace in the devcontainer
+# Don't include all sources, because the rosdep will re-process on any file change
+COPY --parents src/**/package.xml src/**/COLCON_IGNORE src/
+
 RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
-    rm -f /etc/apt/apt.conf.d/docker-clean \
-    && apt-get update \
-    && xargs -r -a /tmp/rosdep-packages.txt apt-get install -y --no-install-recommends \
+    apt-get update \
+    && rosdep update --rosdistro $ROS_DISTRO \
+    && rosdep install --from-paths src --ignore-src -y \
     && rm -rf /var/lib/apt/lists/*
 
-RUN apt-get update \
+
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    apt-get update \
     && apt-get install -y --no-install-recommends \
     ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
     && rm -rf /var/lib/apt/lists/*

Dockerfile

@@ -8,59 +8,49 @@ COPY src ./src/
 COPY patches ./patches/
 COPY upstream.jazzy.repos ./upstream.jazzy.repos
 
-# Stage any dual ROS1/ROS2 packages (e.g. livox_ros_driver2) for ROS2 without
-# modifying their upstream source: generate package.xml from package_ROS2.xml so
-# colcon can discover them. The ROS2 build is selected via -DROS_EDITION below.
-# RUN for d in $(find /ros2_ws/src -type f -name package_ROS2.xml -exec dirname {} \;); do \
-#       cp -f "$d/package_ROS2.xml" "$d/package.xml"; \
-#     done
-
 # Import all upstream repostories
-RUN vcs import ./src/upstream < ./upstream.jazzy.repos
-
-# Fixup the livox package for ros2 build
-RUN cd ./src/upstream/livox_ros_driver2 && \
-    rm -rf launch package.xml && \
-    ln -sfn launch_ROS2 launch && \
-    ln -sfn package_ROS2.xml package.xml
+RUN mkdir -p src/upstream && vcs import ./src/upstream < ./upstream.jazzy.repos
 
 # Apply patches to upstream
-# RUN ./patches/apply_patches.sh
+RUN ./patches/apply_patches.sh
 
 # system deps from frozen rosdep manifest — no rosdep at build or startup
 # NOTE: There should be a check in your CI that this file is up to date.
 # Something like: 
 #   ./scripts/freeze-rosdep.sh && git diff --exit-code rosdep-packages.txt
-COPY rosdep-packages.txt /tmp/rosdep-packages.txt
+# COPY rosdep-packages.txt /tmp/rosdep-packages.txt
 # The --mount=type=cache option caches the apt packages with buildkit
 # The id-apt-cache-${TARGETARCH} option allows for separate caches for different architectures
 # this enables parallel builds for different architectures without cache conflicts
+# RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+#     rm -f /etc/apt/apt.conf.d/docker-clean \
+#     && apt-get update \
+#     && xargs -r -a /tmp/rosdep-packages.txt apt-get install -y --no-install-recommends \
+#     && rm -rf /var/lib/apt/lists/*
+
+# Make sure the base image is completely up to date
+# ros:jazzy ships /etc/apt/apt.conf.d/docker-clean which purges downloaded .debs;
+# remove it so the --mount=type=cache on /var/cache/apt actually retains archives.
 RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
     rm -f /etc/apt/apt.conf.d/docker-clean \
     && apt-get update \
-    && xargs -r -a /tmp/rosdep-packages.txt apt-get install -y --no-install-recommends \
+    && apt-get upgrade -y \
     && rm -rf /var/lib/apt/lists/*
 
+
 # Install dependencies with rosdep
 # This will catch anything that wasn't installed via .deb's from the frozen rosdep manifest.
-RUN apt-get update && \
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked,id=apt-cache-${TARGETARCH} \
+    rm -f /etc/apt/apt.conf.d/docker-clean && \
+    apt-get update && \
     rosdep update && \
     rosdep install --from-paths src --ignore-src -r -y && \
     rm -rf /var/lib/apt/lists/*
 
-RUN cd src/upstream/livox_sdk && \
-    mkdir -p build && \
-    cd build && rm -rf * && \
-    cmake .. -DCMAKE_BUILD_TYPE=Release && \
-    make -j && \
-    make install
-
 # Build the workspace with symlink install
+# livox_sdk is now an ament_cmake package built by colcon (no manual /usr/local install needed).
 RUN . /opt/ros/jazzy/setup.sh && \
-    colcon build --symlink-install \
-    --packages-ignore unitree_lidar_ros2 unitree_lidar_ros unitree_lidar_sdk fast_lio \
-    --cmake-args -DROS_EDITION=ROS2 -DDISTRO_ROS=jazzy
-
+    colcon build --symlink-install
 # Source the overlay on container startup
 RUN echo "source /opt/ros/jazzy/setup.bash" >> /root/.bashrc && \
     echo "source /ros2_ws/install/setup.bash" >> /root/.bashrc

patches/apply_patches.sh

@@ -1,7 +1,8 @@
 #!/usr/bin/env bash
 set -u
 
-ROOT_DIR="/workspaces/agv_pro_ros2"
+PATCH_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+ROOT_DIR="$(dirname "$PATCH_DIR")"
 PATCH_DIR="$ROOT_DIR/patches"
 
 apply_patch_once() {
@@ -23,5 +24,8 @@ apply_patch_once() {
     || git -C "$target_dir" apply --reverse --check "$patch_file" >/dev/null 2>&1
 }
 
+apply_patch_once "$ROOT_DIR/src/upstream/FAST_LIO" "$PATCH_DIR/fast_lio.patch"
 apply_patch_once "$ROOT_DIR/src/upstream/livox_sdk" "$PATCH_DIR/livox_sdk.patch"
+apply_patch_once "$ROOT_DIR/src/upstream/livox_ros_driver2" "$PATCH_DIR/livox_ros_driver2.patch"
 apply_patch_once "$ROOT_DIR/src/upstream/slam_gmapping" "$PATCH_DIR/slam_gmapping.patch"
+apply_patch_once "$ROOT_DIR/src/upstream/unilidar_sdk2" "$PATCH_DIR/unilidar_sdk2.patch"

patches/fast_lio.patch

@@ -0,0 +1,49 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index be3040b..3176847 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -5,17 +5,39 @@ if(NOT CMAKE_BUILD_TYPE)
+   set(CMAKE_BUILD_TYPE Release)
+ endif()
+ 
+-ADD_COMPILE_OPTIONS(-std=c++14)
+-ADD_COMPILE_OPTIONS(-std=c++14)
+-set(CMAKE_CXX_FLAGS "-std=c++14 -O3")
++# Ensure the bundled ikd-Tree submodule (include/ikd-Tree/ikd_Tree.cpp) is present.
++# Auto-initialize it when building from a git checkout; otherwise fail with guidance.
++if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include/ikd-Tree/ikd_Tree.cpp")
++  find_package(Git QUIET)
++  if(GIT_FOUND AND EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/.git")
++    message(STATUS "fast_lio: initializing git submodule include/ikd-Tree")
++    execute_process(
++      COMMAND ${GIT_EXECUTABLE} submodule update --init --recursive
++      WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
++      RESULT_VARIABLE GIT_SUBMOD_RESULT)
++    if(NOT GIT_SUBMOD_RESULT EQUAL "0")
++      message(FATAL_ERROR
++        "git submodule update --init failed with ${GIT_SUBMOD_RESULT}. "
++        "Run it manually in ${CMAKE_CURRENT_SOURCE_DIR}.")
++    endif()
++  endif()
++  if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/include/ikd-Tree/ikd_Tree.cpp")
++    message(FATAL_ERROR
++      "include/ikd-Tree/ikd_Tree.cpp is missing and could not be fetched automatically "
++      "(no git checkout available). Run: git submodule update --init --recursive")
++  endif()
++endif()
++
++ADD_COMPILE_OPTIONS(-std=c++17)
++set(CMAKE_CXX_FLAGS "-std=c++17 -O3")
+ 
+ add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
+ 
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions")
+-set(CMAKE_CXX_STANDARD 14)
++set(CMAKE_CXX_STANDARD 17)
+ set(CMAKE_CXX_STANDARD_REQUIRED ON)
+ set(CMAKE_CXX_EXTENSIONS OFF)
+-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
++set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -pthread -fexceptions")
+ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+ 
+ message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")

patches/livox_ros_driver2.patch

@@ -0,0 +1,89 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index 7344d08..3d531a9 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -1,3 +1,9 @@
++# Default to the ROS2 build unless ROS1 is explicitly requested, so the else()
++# (ROS2) branch below is taken when ROS_EDITION is unset or set to "ROS2".
++if(NOT ROS_EDITION)
++  set(ROS_EDITION "ROS2")
++endif()
++
+ # judge which cmake codes to use 
+ if(ROS_EDITION STREQUAL "ROS1")
+ 
+@@ -191,6 +197,12 @@ else(ROS_EDITION STREQUAL "ROS2")
+   cmake_minimum_required(VERSION 3.14)
+   project(livox_ros_driver2)
+ 
++  # Fall back to the sourced ROS distro when -DDISTRO_ROS was not passed, so a
++  # plain `colcon build` behaves the same as the flagged Dockerfile build.
++  if(NOT DISTRO_ROS)
++    set(DISTRO_ROS "$ENV{ROS_DISTRO}")
++  endif()
++
+   # Default to C99
+   if(NOT CMAKE_C_STANDARD)
+     set(CMAKE_C_STANDARD 99)
+diff --git a/launch b/launch
+new file mode 120000
+index 0000000..163fb01
+--- /dev/null
++++ b/launch
+@@ -0,0 +1 @@
++launch_ROS2
+\ No newline at end of file
+diff --git a/package.xml b/package.xml
+new file mode 100644
+index 0000000..0000000
+--- /dev/null
++++ b/package.xml
+@@ -0,0 +1,36 @@
++<?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>livox_ros_driver2</name>
++  <version>1.0.0</version>
++  <description>The ROS device driver for Livox 3D LiDARs, for ROS2</description>
++  <maintainer email="dev@livoxtech.com">feng</maintainer>
++  <license>MIT</license>
++
++  <buildtool_depend>ament_cmake_auto</buildtool_depend>
++  <build_depend>rosidl_default_generators</build_depend>
++  <member_of_group>rosidl_interface_packages</member_of_group>
++
++  <depend>rclcpp</depend>
++  <depend>rclcpp_components</depend>
++  <depend>std_msgs</depend>
++  <depend>sensor_msgs</depend>
++  <depend>rcutils</depend>
++  <depend>pcl_conversions</depend>
++  <depend>rcl_interfaces</depend>
++  <depend>libpcl-all-dev</depend>
++
++  <exec_depend>rosbag2</exec_depend>
++  <exec_depend>rosidl_default_runtime</exec_depend>
++
++  <test_depend>ament_lint_auto</test_depend>
++  <test_depend>ament_lint_common</test_depend>
++
++  <depend>git</depend>
++  <depend>apr</depend>
++  <depend>livox_sdk2</depend>
++
++  <export>
++    <build_type>ament_cmake</build_type>
++  </export>
++</package>
+diff --git a/package_ROS2.xml b/package_ROS2.xml
+index 96f5762..c66785b 100644
+--- a/package_ROS2.xml
++++ b/package_ROS2.xml
+@@ -28,6 +28,7 @@
+ 
+   <depend>git</depend>
+   <depend>apr</depend>
++  <depend>livox_sdk2</depend>
+ 
+   <export>
+     <build_type>ament_cmake</build_type>

patches/livox_sdk.patch

@@ -1,24 +1,24 @@
-diff --git a/sdk_core/comm/define.h b/sdk_core/comm/define.h
-index 0872328..28d989b 100644
---- a/sdk_core/comm/define.h
-+++ b/sdk_core/comm/define.h
-@@ -31,6 +31,7 @@
- #include <functional>
- #include <vector>
- #include <atomic>
-+#include <cstdint>
- 
- #include "livox_lidar_def.h"
- 
-diff --git a/sdk_core/logger_handler/file_manager.h b/sdk_core/logger_handler/file_manager.h
-index 242a431..14cd76a 100644
---- a/sdk_core/logger_handler/file_manager.h
-+++ b/sdk_core/logger_handler/file_manager.h
-@@ -28,6 +28,7 @@
- #include <string>
- #include <vector>
- #include <map>
-+#include <cstdint>
- 
- namespace livox {
- namespace lidar {
+diff --git a/package.xml b/package.xml
+new file mode 100644
+index 0000000..ffdf3ce
+--- /dev/null
++++ b/package.xml
+@@ -0,0 +1,18 @@
++<?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>livox_sdk2</name>
++  <version>1.3.1</version>
++  <description>
++    Livox SDK2: official C++ library for Livox LiDAR sensors.
++    Provides the livox_sdk2 ament target consumed by livox_ros_driver2.
++  </description>
++  <maintainer email="sdk@livoxtech.com">Livox</maintainer>
++  <license>Apache-2.0</license>
++
++  <buildtool_depend>ament_cmake</buildtool_depend>
++
++  <export>
++    <build_type>ament_cmake</build_type>
++  </export>
++</package>

patches/unilidar_sdk2.patch

@@ -0,0 +1,133 @@
+diff --git a/unitree_lidar_ros/src/unitree_lidar_ros/COLCON_IGNORE b/unitree_lidar_ros/src/unitree_lidar_ros/COLCON_IGNORE
+new file mode 100644
+index 0000000..e69de29
+diff --git a/unitree_lidar_ros2/src/unitree_lidar_ros2/CMakeLists.txt b/unitree_lidar_ros2/src/unitree_lidar_ros2/CMakeLists.txt
+index 041ef0d..dd57d47 100644
+--- a/unitree_lidar_ros2/src/unitree_lidar_ros2/CMakeLists.txt
++++ b/unitree_lidar_ros2/src/unitree_lidar_ros2/CMakeLists.txt
+@@ -23,16 +23,15 @@ find_package(pcl_conversions REQUIRED)
+ find_package(PCL REQUIRED)
+ find_package(tf2_ros REQUIRED)
+ find_package(sensor_msgs REQUIRED)
++find_package(unitree_lidar_sdk REQUIRED)
+ 
+ include_directories(
+   ${PCL_INCLUDE_DIRS} 
+   include
+-  ../../../unitree_lidar_sdk/include
+ )
+ 
+ link_directories(
+   ${PCL_LIBRARY_DIRS}
+-  ../../../unitree_lidar_sdk/lib/${CMAKE_SYSTEM_PROCESSOR}
+ )
+ 
+ add_definitions(${PCL_DEFINITIONS})
+@@ -42,7 +41,6 @@ add_executable(unitree_lidar_ros2_node src/unitree_lidar_ros2_node.cpp)
+ target_link_libraries( unitree_lidar_ros2_node
+   ${Boost_SYSTEM_LIBRARY}
+   ${PCL_LIBRARIES}
+-  libunilidar_sdk2.a
+ )
+ 
+ ament_target_dependencies(
+@@ -52,6 +50,7 @@ ament_target_dependencies(
+   geometry_msgs 
+   tf2_ros
+   pcl_conversions
++  unitree_lidar_sdk
+ )
+ 
+ install(TARGETS
+diff --git a/unitree_lidar_ros2/src/unitree_lidar_ros2/package.xml b/unitree_lidar_ros2/src/unitree_lidar_ros2/package.xml
+index 74539aa..8b186d8 100644
+--- a/unitree_lidar_ros2/src/unitree_lidar_ros2/package.xml
++++ b/unitree_lidar_ros2/src/unitree_lidar_ros2/package.xml
+@@ -17,10 +17,11 @@
+   <depend>sensor_msgs</depend>
+   
+   <depend>pcl_conversions</depend>
+-  <depend>pcl</depend>
++  <depend>libpcl-all-dev</depend>
+ 
+   <depend>geometry_msgs</depend>
+   <depend>tf2_ros</depend>
++  <depend>unitree_lidar_sdk</depend>
+ 
+   <export>
+     <build_type>ament_cmake</build_type>
+diff --git a/unitree_lidar_sdk/CMakeLists.txt b/unitree_lidar_sdk/CMakeLists.txt
+index 80af9c9..00725ea 100644
+--- a/unitree_lidar_sdk/CMakeLists.txt
++++ b/unitree_lidar_sdk/CMakeLists.txt
+@@ -1,39 +1,14 @@
+-cmake_minimum_required(VERSION 3.0)
++cmake_minimum_required(VERSION 3.5)
+ project(unitree_lidar_sdk)
+ 
+-set(CMAKE_BUILD_TYPE "Release")
+-set(CMAKE_CXX_FLAGS "-std=c++17")
+-set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb")
+-set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall -DNDEBUG") 
++find_package(ament_cmake REQUIRED)
+ 
+-include_directories(include)
++# Install the SDK headers and the architecture-specific prebuilt static library.
++install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
++install(FILES lib/${CMAKE_SYSTEM_PROCESSOR}/libunilidar_sdk2.a DESTINATION lib)
+ 
+-link_directories(lib/${CMAKE_SYSTEM_PROCESSOR})
++# Export them so ament packages can consume the SDK via find_package().
++ament_export_include_directories(include/${PROJECT_NAME})
++ament_export_libraries(unilidar_sdk2)
+ 
+-SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+-set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib/${CMAKE_SYSTEM_PROCESSOR})
+-
+-add_executable(example_lidar_udp
+-  examples/example_lidar_udp.cpp
+-)
+-target_link_libraries(example_lidar_udp  libunilidar_sdk2.a )
+-
+-add_executable(example_lidar_serial
+-    examples/example_lidar_serial.cpp
+-)
+-target_link_libraries(example_lidar_serial  libunilidar_sdk2.a  )
+-
+-add_executable(set_ip_address
+-    examples/set_ip_address.cpp
+-)
+-target_link_libraries(set_ip_address  libunilidar_sdk2.a )
+-
+-add_executable(set_to_serial_mode
+-    examples/set_to_serial_mode.cpp
+-)
+-target_link_libraries(set_to_serial_mode  libunilidar_sdk2.a )
+-
+-add_executable(set_to_udp_mode
+-    examples/set_to_udp_mode.cpp
+-)
+-target_link_libraries(set_to_udp_mode  libunilidar_sdk2.a )
+\ No newline at end of file
++ament_package()
+diff --git a/unitree_lidar_sdk/package.xml b/unitree_lidar_sdk/package.xml
+new file mode 100644
+index 0000000..ef88d9a
+--- /dev/null
++++ b/unitree_lidar_sdk/package.xml
+@@ -0,0 +1,15 @@
++<?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>unitree_lidar_sdk</name>
++  <version>0.0.0</version>
++  <description>Unitree LiDAR SDK (prebuilt static library and headers)</description>
++  <maintainer email="lukelbmeng@gmail.com">jo</maintainer>
++  <license>BSD-3-Clause</license>
++
++  <buildtool_depend>ament_cmake</buildtool_depend>
++
++  <export>
++    <build_type>ament_cmake</build_type>
++  </export>
++</package>

setup_upstream.sh

@@ -0,0 +1,36 @@
+#!/usr/bin/env bash
+#
+# setup_upstream.sh — fetch the upstream sources agv_pro_ros2 depends on
+# and apply any required patches.
+#
+# Sources are declared in upstream.jazzy.repos and cloned under src/upstream/.
+# Patches in patches/ are applied idempotently (already-applied patches are
+# silently skipped).
+#
+# Usage:
+#   ./setup_upstream.sh
+set -euo pipefail
+
+REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+cd "$REPO_ROOT"
+
+echo "[setup_upstream] importing sources into src/upstream ..."
+mkdir -p src/upstream
+
+# Reset working-tree changes in already-cloned repos so that vcs import can
+# pull upstream updates without hitting dirty-working-tree conflicts.
+# Use -x so that gitignored files (e.g. generated package.xml) are also removed.
+# Patches are re-applied unconditionally after the import step below.
+for repo_dir in src/upstream/*/; do
+    if [[ -d "${repo_dir}/.git" ]]; then
+        git -C "${repo_dir}" checkout -- . 2>/dev/null || true
+        git -C "${repo_dir}" clean -fdx 2>/dev/null || true
+    fi
+done
+
+vcs import src/upstream < upstream.jazzy.repos
+
+echo "[setup_upstream] applying patches ..."
+bash patches/apply_patches.sh
+
+echo "[setup_upstream] done."

src/elephant_robotics/agv_pro_autocharge/package.xml

@@ -7,9 +7,6 @@
   <maintainer email="your-email@example.com">Your Name</maintainer>
   <license>MIT</license>
 
-  <!-- Build dependencies -->
-  <buildtool_depend>ament_python</buildtool_depend>
-  
   <!-- Runtime dependencies -->
   <depend>rclpy</depend>
   <depend>geometry_msgs</depend>

src/elephant_robotics/agv_pro_bringup/config/ros2_controllers.yaml

@@ -31,6 +31,12 @@ mecanum_drive_controller:
     base_frame_id:  base_footprint
     enable_odom_tf: true
 
+    # How long a /cmd_vel (reference) command stays valid, in seconds. The
+    # controller default is 0.0, which treats every command as instantly expired
+    # so the controller halts between messages. 0.5 s holds the last command
+    # between messages (standard cmd_vel behaviour) so the base runs at speed.
+    reference_timeout: 0.5
+
     kinematics:
       # wheel_radius: height of wheel centre above ground from URDF joint origin
       # (base_footprint→base_link = 0.020 m + wheel joint z-offset ≈ 0.052 m = 0.072 m)

src/elephant_robotics/agv_pro_description/urdf/agv_pro.gazebo.xacro

@@ -7,13 +7,72 @@
          controller_manager inside gz-sim and reads the controller yaml.
     Per-wheel mecanum friction is emitted by the wheel macro when
     gazebo_ignition (sim) is true, so it is NOT repeated here.
+
+    controllers_file         : primary controller params (the AGV base controllers).
+    controllers_file_arm     : OPTIONAL per-function params files, each loaded into
+    controllers_file_gripper   the SAME controller_manager. Used when the AGV is
+                             composed into a larger robot (e.g. a mobile
+                             manipulator) so the additional controllers (e.g. an
+                             arm_controller / gripper_action_controller) share the
+                             single gz_ros2_control controller_manager. Named by
+                             function for clarity; empty by default -> standalone
+                             AGV unaffected.
   -->
-  <xacro:macro name="agv_pro_gazebo" params="prefix controllers_file">
+  <xacro:macro name="agv_pro_gazebo" params="prefix controllers_file controllers_file_arm:='' controllers_file_gripper:=''">
     <gazebo>
       <plugin filename="gz_ros2_control-system"
         name="gz_ros2_control::GazeboSimROS2ControlPlugin">
         <parameters>${controllers_file}</parameters>
+        <xacro:if value="${controllers_file_arm != ''}">
+          <parameters>${controllers_file_arm}</parameters>
+        </xacro:if>
+        <xacro:if value="${controllers_file_gripper != ''}">
+          <parameters>${controllers_file_gripper}</parameters>
+        </xacro:if>
       </plugin>
     </gazebo>
+
+    <!--
+      2D lidar sensor on laser_link. Publishes a gz LaserScan on the gz topic
+      'scan'; gazebo.launch bridges it to the ROS /scan (sensor_msgs/LaserScan)
+      that Nav2 / SLAM consume. The gz-sim-sensors-system plugin in the world
+      SDF renders it. gz_frame_id stamps the scan with the laser_link frame so
+      TF lines up. Mirrors a generic 360deg 2D planar lidar; on the real robot
+      the physical LSLiDAR/Livox/Unitree driver publishes the same /scan.
+    -->
+    <gazebo reference="${prefix}laser_link">
+      <sensor name="${prefix}lidar" type="gpu_lidar">
+        <pose>0 0 0 0 0 0</pose>
+        <topic>scan</topic>
+        <gz_frame_id>${prefix}laser_link</gz_frame_id>
+        <update_rate>10</update_rate>
+        <always_on>1</always_on>
+        <visualize>true</visualize>
+        <lidar>
+          <scan>
+            <horizontal>
+              <samples>360</samples>
+              <resolution>1</resolution>
+              <min_angle>-3.141592653589793</min_angle>
+              <max_angle>3.141592653589793</max_angle>
+            </horizontal>
+          </scan>
+          <!--
+            The front-mounted 360 deg lidar physically sees the robot's own
+            chassis behind it (returns from ~0.19 m out to ~0.38 m at the rear
+            angles). Those self-returns are removed downstream by a
+            laser_filters box filter (see agv_pro_gazebo scan_filter.yaml) in the
+            base_footprint frame, which keeps full forward/side sensing while
+            dropping any point inside the chassis outline. Both simulation and
+            the physical robot run the same filter, so /scan stays identical.
+          -->
+          <range>
+            <min>0.15</min>
+            <max>12.0</max>
+            <resolution>0.01</resolution>
+          </range>
+        </lidar>
+      </sensor>
+    </gazebo>
   </xacro:macro>
 </robot>

src/elephant_robotics/agv_pro_description/urdf/agv_pro.urdf.xacro

@@ -18,6 +18,17 @@
     <xacro:arg name="controllers_file" default="" />
     <xacro:property name="controllers_file" value="$(arg controllers_file)" />
 
+    <!-- Optional per-function controller params files, each loaded into the
+         SAME gz_ros2_control controller_manager. Used when composing the AGV
+         into a larger robot (e.g. a mobile manipulator) so its controllers
+         share the single controller_manager. Named by function for clarity;
+         empty -> standalone behaviour unchanged. -->
+    <xacro:arg name="controllers_file_arm" default="" />
+    <xacro:property name="controllers_file_arm" value="$(arg controllers_file_arm)" />
+
+    <xacro:arg name="controllers_file_gripper" default="" />
+    <xacro:property name="controllers_file_gripper" value="$(arg controllers_file_gripper)" />
+
     <link name="${prefix}base_footprint" />
 
     <joint name="${prefix}base_joint" type="fixed">
@@ -146,7 +157,9 @@
     <!-- Gazebo system plugin (hosts controller_manager); wheel friction is in the wheel macro -->
     <xacro:if value="${sim}">
         <xacro:include filename="$(find agv_pro_description)/urdf/agv_pro.gazebo.xacro" />
-        <xacro:agv_pro_gazebo prefix="${prefix}" controllers_file="${controllers_file}" />
+        <xacro:agv_pro_gazebo prefix="${prefix}" controllers_file="${controllers_file}"
+            controllers_file_arm="${controllers_file_arm}"
+            controllers_file_gripper="${controllers_file_gripper}" />
     </xacro:if>
 
 </robot>

src/elephant_robotics/agv_pro_description/urdf/parts/wheel_macro.xacro

@@ -86,7 +86,8 @@
                     <surface>
                         <friction>
                             <ode>
-                                <mu>1.0</mu>
+                             
+                                <mu>5.0</mu>
                                 <mu2>0.0</mu2>
                                 <xacro:if value="${reflect}">
                                     <fdir1
@@ -98,12 +99,6 @@
                                 </xacro:unless>
                             </ode>
                         </friction>
-                        <contact>
-                            <ode>
-                                <kp>100000.0</kp>
-                                <kd>10.0</kd>
-                            </ode>
-                        </contact>
                     </surface>
                 </collision>
             </gazebo>

src/elephant_robotics/agv_pro_gazebo/config/agv_control.yaml

@@ -29,6 +29,13 @@ mecanum_drive_controller:
     base_frame_id:  base_footprint
     enable_odom_tf: true
 
+    # How long a /cmd_vel (reference) command stays valid, in seconds. The
+    # controller default is 0.0, which treats every command as instantly expired
+    # and so mostly halts between messages — the wheels only reached ~16-31% of
+    # the commanded speed and the robot crawled. 0.5 s holds the last command
+    # between messages (standard cmd_vel behaviour) so the wheels run at speed.
+    reference_timeout: 0.5
+
     kinematics:
       wheels_radius: 0.072  # 轮子半径 (wheel centre height above ground)
       # sum_of_robot_center_projection_on_X_Y_axis = lx + ly (0.172 + 0.180)

src/elephant_robotics/agv_pro_gazebo/config/scan_filter.yaml

@@ -0,0 +1,30 @@
+# laser_filters chain for the AGV Pro 2D lidar.
+#
+# The 360 deg lidar is mounted at the front of the robot (laser_link is ~0.179 m
+# ahead of base_link), so its rearward beams strike the robot's own chassis and
+# report returns from ~0.19 m out to ~0.38 m. Left unfiltered, those self-returns
+# were baked into the SLAM map and both Nav2 costmaps, marking the robot's own
+# cell lethal (cost 253) so the planner refused to plan from an in-collision
+# start ("Failed to create plan with tolerance").
+#
+# The box filter drops any scan point that falls inside the chassis outline
+# (expressed in base_footprint), which removes the self-returns while keeping all
+# real obstacles in front of and beside the robot. The physical robot runs the
+# same filter, so the /scan consumed by SLAM/Nav2 is identical to simulation.
+scan_filter_chain:
+  ros__parameters:
+    filter1:
+      name: chassis_box_filter
+      type: laser_filters/LaserScanBoxFilter
+      params:
+        box_frame: base_footprint
+        # Chassis extent in base_footprint (metres). The laser sits at x=+0.179,
+        # so the body occupies roughly x in [-0.35, 0.18] and y in [-0.30, 0.30].
+        min_x: -0.35
+        max_x: 0.18
+        min_y: -0.30
+        max_y: 0.30
+        min_z: -1.0
+        max_z: 1.0
+        # false => remove points that fall INSIDE the box (the chassis).
+        invert: false

src/elephant_robotics/agv_pro_gazebo/launch/agv_pro_gazebo.launch.py

@@ -29,12 +29,13 @@ def generate_launch_description():
     # Shared robot description (agv_pro_description) built for simulation.
     xacro_file = os.path.join(pkg_description, 'urdf', 'agv_pro.urdf.xacro')
     controllers_file = os.path.join(pkg_gazebo, 'config', 'agv_control.yaml')
-    world_file = os.path.join(pkg_gazebo, 'worlds', 'empty.world')
+    default_world = os.path.join(pkg_gazebo, 'worlds', 'empty.world')
     rviz_config = os.path.join(pkg_gazebo, 'rviz', 'agvpro_display.rviz')
 
     use_sim_time = LaunchConfiguration('use_sim_time')
     use_rviz = LaunchConfiguration('use_rviz')
     headless = LaunchConfiguration('headless')
+    world = LaunchConfiguration('world')
 
     robot_description = {
         'robot_description': ParameterValue(
@@ -64,9 +65,8 @@ def generate_launch_description():
     # Start gz-sim (Gazebo Harmonic) with the world.
     # '-s' (server only) is added when headless:=true.
     gz_args = PythonExpression([
-        "'-r -v4 -s ' + ", repr(world_file),
-        " if '", headless, "' == 'true' ",
-        "else '-r -v4 ' + ", repr(world_file),
+        "'-r -v4 -s ' + '", world, "' if '", headless,
+        "' == 'true' else '-r -v4 ' + '", world, "'",
     ])
     gz_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -104,6 +104,35 @@ def generate_launch_description():
         output='screen',
     )
 
+    # Bridge the 2D lidar: gz LaserScan -> ROS /scan_raw (sensor_msgs/LaserScan).
+    # The sensor is defined on laser_link in agv_pro.gazebo.xacro. The raw scan
+    # goes to /scan_raw and is cleaned by the box filter below before Nav2/SLAM
+    # consume /scan. use_sim_time so the scan stamps use the Gazebo clock.
+    scan_bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan'],
+        parameters=[{'use_sim_time': use_sim_time}],
+        remappings=[('/scan', '/scan_raw')],
+        output='screen',
+    )
+
+    # Remove the robot's own chassis returns from the front-mounted lidar.
+    # The box filter (config/scan_filter.yaml) drops any point inside the chassis
+    # outline in base_footprint, publishing the cleaned scan on /scan. Without it
+    # the self-returns mark the robot's own cell lethal and Nav2 cannot plan.
+    scan_filter = Node(
+        package='laser_filters',
+        executable='scan_to_scan_filter_chain',
+        name='scan_filter_chain',
+        parameters=[
+            os.path.join(pkg_gazebo, 'config', 'scan_filter.yaml'),
+            {'use_sim_time': use_sim_time},
+        ],
+        remappings=[('scan', '/scan_raw'), ('scan_filtered', '/scan')],
+        output='screen',
+    )
+
     # Controllers (controller_manager runs inside the gz_ros2_control plugin)
     joint_state_broadcaster_spawner = Node(
         package='controller_manager',
@@ -117,15 +146,20 @@ def generate_launch_description():
 
     # Remap the controller's reference topic (~/reference) to the standard
     # /cmd_vel so teleop and nav2 (which publish geometry_msgs/TwistStamped)
-    # drive the robot directly. NOTE: the remap key must be the private name
-    # '~/reference' — a bare 'reference:=/cmd_vel' is silently ignored.
+    # drive the robot directly. Also remap the controller's private odometry
+    # outputs to the global names the rest of the stack expects:
+    #   ~/tf_odometry -> /tf   (so odom->base_footprint reaches the TF tree)
+    #   ~/odometry    -> /odom (nav2 odom_topic, robot_localization, etc.)
+    # NOTE: the remap keys must be the private names ('~/...'); a bare
+    # 'reference:=/cmd_vel' is silently ignored.
     mecanum_drive_controller_spawner = Node(
         package='controller_manager',
         executable='spawner',
         arguments=[
             'mecanum_drive_controller',
             '--controller-manager', '/controller_manager',
-            '--controller-ros-args', '-r ~/reference:=/cmd_vel',
+            '--controller-ros-args',
+            '-r ~/reference:=/cmd_vel -r ~/tf_odometry:=/tf -r ~/odometry:=/odom',
         ],
         output='screen',
     )
@@ -156,10 +190,17 @@ def generate_launch_description():
             default_value='false',
             description='Run gz-sim without the GUI (server only)',
         ),
+        DeclareLaunchArgument(
+            'world',
+            default_value=default_world,
+            description='Full path to the Gazebo world (SDF) to load',
+        ),
         gz_resource_path,
         gz_sim,
         robot_state_publisher,
         clock_bridge,
+        scan_bridge,
+        scan_filter,
         delayed_spawn,
         # Load controllers only after the robot has been spawned
         RegisterEventHandler(

src/elephant_robotics/agv_pro_gazebo/package.xml

@@ -18,6 +18,7 @@
   <!-- Gazebo (gz-sim Harmonic) integration for ROS 2 Jazzy -->
   <exec_depend>ros_gz_sim</exec_depend>
   <exec_depend>ros_gz_bridge</exec_depend>
+  <exec_depend>laser_filters</exec_depend>
   <depend>gz_ros2_control</depend>
 
   <depend>robot_state_publisher</depend>

src/elephant_robotics/agv_pro_gazebo/worlds/room.world

@@ -0,0 +1,133 @@
+<?xml version="1.0" ?>
+<!--
+  room.world - a simple enclosed room for testing SLAM and Nav2 in simulation.
+  Same required gz-sim system plugins as empty.world (physics, scene broadcaster,
+  user commands, sensors) plus four perimeter walls so the 2D lidar has features
+  to map and localise against.
+-->
+<sdf version="1.10">
+  <world name="room_world">
+
+    <plugin filename="gz-sim-physics-system"
+            name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin filename="gz-sim-user-commands-system"
+            name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin filename="gz-sim-scene-broadcaster-system"
+            name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin filename="gz-sim-sensors-system"
+            name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane><normal>0 0 1</normal><size>100 100</size></plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane><normal>0 0 1</normal><size>100 100</size></plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- 8m x 8m room: four walls, 0.1m thick, 1m tall, centred on origin -->
+    <model name="walls">
+      <static>true</static>
+      <link name="link">
+        <!-- North wall (+X) -->
+        <collision name="north_col">
+          <pose>4 0 0.5 0 0 0</pose>
+          <geometry><box><size>0.1 8 1</size></box></geometry>
+        </collision>
+        <visual name="north_vis">
+          <pose>4 0 0.5 0 0 0</pose>
+          <geometry><box><size>0.1 8 1</size></box></geometry>
+          <material><ambient>0.6 0.6 0.65 1</ambient><diffuse>0.6 0.6 0.65 1</diffuse></material>
+        </visual>
+        <!-- South wall (-X) -->
+        <collision name="south_col">
+          <pose>-4 0 0.5 0 0 0</pose>
+          <geometry><box><size>0.1 8 1</size></box></geometry>
+        </collision>
+        <visual name="south_vis">
+          <pose>-4 0 0.5 0 0 0</pose>
+          <geometry><box><size>0.1 8 1</size></box></geometry>
+          <material><ambient>0.6 0.6 0.65 1</ambient><diffuse>0.6 0.6 0.65 1</diffuse></material>
+        </visual>
+        <!-- East wall (+Y) -->
+        <collision name="east_col">
+          <pose>0 4 0.5 0 0 0</pose>
+          <geometry><box><size>8 0.1 1</size></box></geometry>
+        </collision>
+        <visual name="east_vis">
+          <pose>0 4 0.5 0 0 0</pose>
+          <geometry><box><size>8 0.1 1</size></box></geometry>
+          <material><ambient>0.6 0.6 0.65 1</ambient><diffuse>0.6 0.6 0.65 1</diffuse></material>
+        </visual>
+        <!-- West wall (-Y) -->
+        <collision name="west_col">
+          <pose>0 -4 0.5 0 0 0</pose>
+          <geometry><box><size>8 0.1 1</size></box></geometry>
+        </collision>
+        <visual name="west_vis">
+          <pose>0 -4 0.5 0 0 0</pose>
+          <geometry><box><size>8 0.1 1</size></box></geometry>
+          <material><ambient>0.6 0.6 0.65 1</ambient><diffuse>0.6 0.6 0.65 1</diffuse></material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- A couple of interior obstacles to make mapping/localisation non-trivial -->
+    <model name="pillar_1">
+      <static>true</static>
+      <link name="link">
+        <pose>1.5 1.0 0.5 0 0 0</pose>
+        <collision name="c"><geometry><cylinder><radius>0.25</radius><length>1.0</length></cylinder></geometry></collision>
+        <visual name="v"><geometry><cylinder><radius>0.25</radius><length>1.0</length></cylinder></geometry>
+          <material><ambient>0.7 0.5 0.3 1</ambient><diffuse>0.7 0.5 0.3 1</diffuse></material></visual>
+      </link>
+    </model>
+    <model name="pillar_2">
+      <static>true</static>
+      <link name="link">
+        <pose>-1.8 -1.5 0.5 0 0 0</pose>
+        <collision name="c"><geometry><box><size>0.5 0.5 1.0</size></box></geometry></collision>
+        <visual name="v"><geometry><box><size>0.5 0.5 1.0</size></box></geometry>
+          <material><ambient>0.3 0.5 0.7 1</ambient><diffuse>0.3 0.5 0.7 1</diffuse></material></visual>
+      </link>
+    </model>
+
+  </world>
+</sdf>

src/elephant_robotics/agv_pro_navigation2/CMakeLists.txt

@@ -24,7 +24,7 @@ if(BUILD_TESTING)
 endif()
 
 install(
-  DIRECTORY launch map param rviz scripts
+  DIRECTORY config launch map param rviz scripts
   DESTINATION share/${PROJECT_NAME}
 )
 

src/elephant_robotics/agv_pro_navigation2/config/slam_toolbox_sim.yaml

@@ -0,0 +1,46 @@
+# slam_toolbox online-async mapping config for the AGV Pro in simulation.
+# Builds a map from /scan and publishes the map -> odom transform that Nav2
+# needs. Frames match the robot: odom (from mecanum_drive_controller) and
+# base_footprint (robot root). use_sim_time is supplied by the launch file.
+slam_toolbox:
+  ros__parameters:
+    # Frames / topics
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_footprint
+    scan_topic: /scan
+    mode: mapping
+
+    # Solver
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # Mapping behaviour
+    map_update_interval: 1.0
+    resolution: 0.05
+    max_laser_range: 12.0
+    minimum_time_interval: 0.2
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.0
+    stack_size_to_use: 40000000
+    enable_interactive_mode: true
+
+    # Scan matching
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.3
+    minimum_travel_heading: 0.3
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 12.0
+    link_match_minimum_response_fine: 0.1
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45

src/elephant_robotics/agv_pro_navigation2/launch/simulation.launch.py

@@ -0,0 +1,115 @@
+"""
+simulation.launch.py — Nav2 + SLAM in Gazebo for the AGV Pro.
+
+Brings up the full autonomous-navigation stack in simulation:
+
+  1. Gazebo (agv_pro_gazebo.launch.py) in the walled `room.world`, with the
+     2D lidar publishing /scan and the mecanum_drive_controller publishing
+     odom -> base_footprint (TF) and /odom, consuming /cmd_vel.
+  2. slam_toolbox (online async) — builds a map from /scan and publishes
+     map -> odom.
+  3. Nav2 (navigation_launch.py) — planner / controller (MPPI, Omni motion
+     model for the holonomic base) / behaviours / bt_navigator, using
+     param/nav2_sim.yaml.
+  4. RViz with the navigation view.
+
+Everything runs on the Gazebo clock (use_sim_time:=true).
+
+Usage:
+  ros2 launch agv_pro_navigation2 simulation.launch.py
+  ros2 launch agv_pro_navigation2 simulation.launch.py headless:=true
+  ros2 launch agv_pro_navigation2 simulation.launch.py use_rviz:=false
+"""
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    TimerAction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_gazebo = get_package_share_directory('agv_pro_gazebo')
+    pkg_nav2 = get_package_share_directory('agv_pro_navigation2')
+    pkg_slam = get_package_share_directory('slam_toolbox')
+    pkg_nav2_bringup = get_package_share_directory('nav2_bringup')
+
+    use_rviz = LaunchConfiguration('use_rviz')
+    headless = LaunchConfiguration('headless')
+    world = LaunchConfiguration('world')
+
+    default_world = os.path.join(pkg_gazebo, 'worlds', 'room.world')
+    slam_params = os.path.join(pkg_nav2, 'config', 'slam_toolbox_sim.yaml')
+    nav2_params = os.path.join(pkg_nav2, 'param', 'nav2_sim.yaml')
+    rviz_config = os.path.join(pkg_nav2, 'rviz', 'agvpro_navigation2.rviz')
+
+    # 1. Gazebo + robot + lidar + controllers
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo, 'launch', 'agv_pro_gazebo.launch.py')
+        ),
+        launch_arguments={
+            'use_sim_time': 'true',
+            'use_rviz': 'false',
+            'headless': headless,
+            'world': world,
+        }.items(),
+    )
+
+    # 2. SLAM (map -> odom). Delayed so /scan and odom TF are up first.
+    slam = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_slam, 'launch', 'online_async_launch.py')
+        ),
+        launch_arguments={
+            'use_sim_time': 'true',
+            'slam_params_file': slam_params,
+        }.items(),
+    )
+
+    # 3. Nav2 navigation stack (no localization; SLAM provides map -> odom).
+    nav2 = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_nav2_bringup, 'launch', 'navigation_launch.py')
+        ),
+        launch_arguments={
+            'use_sim_time': 'true',
+            'params_file': nav2_params,
+        }.items(),
+    )
+
+    # 4. RViz
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        arguments=['-d', rviz_config],
+        parameters=[{'use_sim_time': True}],
+        condition=IfCondition(use_rviz),
+        output='screen',
+    )
+
+    # Give Gazebo time to spawn the robot and activate the controllers (which
+    # publish odom -> base_footprint) before SLAM and Nav2 start looking for TF.
+    delayed_bringup = TimerAction(period=8.0, actions=[slam, nav2, rviz])
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_rviz', default_value='true',
+            description='Launch RViz with the navigation view'),
+        DeclareLaunchArgument(
+            'headless', default_value='false',
+            description='Run gz-sim without the GUI (server only)'),
+        DeclareLaunchArgument(
+            'world', default_value=default_world,
+            description='Full path to the Gazebo world (SDF) to load'),
+        gazebo,
+        delayed_bringup,
+    ])

src/elephant_robotics/agv_pro_navigation2/package.xml

@@ -12,6 +12,10 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <exec_depend>nav2_bringup</exec_depend>
+  <exec_depend>slam_toolbox</exec_depend>
+  <exec_depend>agv_pro_gazebo</exec_depend>
+  <exec_depend>agv_pro_description</exec_depend>
+  <exec_depend>rviz2</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/elephant_robotics/agv_pro_navigation2/param/agvpro.yaml

@@ -109,6 +109,9 @@ bt_navigator_rclcpp_node:
 controller_server:
   ros__parameters:
     use_sim_time: False
+    # Publish /cmd_vel as geometry_msgs/TwistStamped to match the
+    # mecanum_drive_controller's ~/reference input (both sim and real robot).
+    enable_stamped_cmd_vel: true
     controller_frequency: 20.0
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5

src/elephant_robotics/agv_pro_navigation2/param/nav2_sim.yaml

@@ -0,0 +1,482 @@
+amcl:
+  ros__parameters:
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+bt_navigator:
+  ros__parameters:
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    wait_for_service_timeout: 1000
+    action_server_result_timeout: 900.0
+    navigators: ["navigate_to_pose", "navigate_through_poses"]
+    navigate_to_pose:
+      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
+    navigate_through_poses:
+      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+
+    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
+    # Built-in plugins are added automatically
+    # plugin_lib_names: []
+
+    error_code_names:
+      - compute_path_error_code
+      - follow_path_error_code
+
+controller_server:
+  ros__parameters:
+    # Mecanum controller consumes geometry_msgs/TwistStamped on ~/reference.
+    enable_stamped_cmd_vel: true
+    controller_frequency: 20.0
+    costmap_update_timeout: 0.30
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugins: ["progress_checker"]
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+    use_realtime_priority: false
+
+    # Progress checker parameters
+    # PoseProgressChecker (not SimpleProgressChecker) so that in-place rotation
+    # counts as progress. At the goal the holonomic base often must rotate to
+    # meet yaw_goal_tolerance; SimpleProgressChecker only credits *translation*
+    # (required_movement_radius) so that final yaw alignment tripped "Failed to
+    # make progress" -> recovery spin -> rotated further off goal -> Goal failed.
+    progress_checker:
+      plugin: "nav2_controller::PoseProgressChecker"
+      required_movement_radius: 0.5
+      required_movement_angle: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    #precise_goal_checker:
+    #  plugin: "nav2_controller::SimpleGoalChecker"
+    #  xy_goal_tolerance: 0.25
+    #  yaw_goal_tolerance: 0.25
+    #  stateful: True
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      ax_max: 3.0
+      ax_min: -3.0
+      ay_max: 3.0
+      ay_min: -3.0
+      az_max: 3.5
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.5
+      wz_max: 1.9
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Omni"
+      visualize: true
+      regenerate_noises: true
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics: [
+        "ConstraintCritic", "CostCritic", "GoalCritic",
+        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
+        "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        near_collision_cost: 253
+        critical_cost: 300.0
+        consider_footprint: false
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        mode: 0
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_footprint
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.30
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.70
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_footprint
+      robot_radius: 0.30
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.7
+      always_send_full_costmap: True
+
+# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
+# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
+# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
+# map_server:
+#   ros__parameters:
+#     yaml_filename: ""
+
+map_saver:
+  ros__parameters:
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    planner_plugins: ["GridBased"]
+    costmap_update_timeout: 1.0
+    GridBased:
+      plugin: "nav2_navfn_planner::NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+smoother_server:
+  ros__parameters:
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    enable_stamped_cmd_vel: true
+    local_costmap_topic: local_costmap/costmap_raw
+    global_costmap_topic: global_costmap/costmap_raw
+    local_footprint_topic: local_costmap/published_footprint
+    global_footprint_topic: global_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    spin:
+      plugin: "nav2_behaviors::Spin"
+    backup:
+      plugin: "nav2_behaviors::BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors::DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors::Wait"
+    assisted_teleop:
+      plugin: "nav2_behaviors::AssistedTeleop"
+    local_frame: odom
+    global_frame: map
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.1
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    action_server_result_timeout: 900.0
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+route_server:
+  ros__parameters:
+    # The graph_filepath does not need to be specified since it going to be set by defaults in launch.
+    # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s).
+    # file & provide full path to map below. If graph config or launch default is provided, it is used
+    # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson
+    boundary_radius_to_achieve_node: 1.0
+    radius_to_achieve_node: 2.0
+    smooth_corners: true
+    operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"]
+    ReroutingService:
+      plugin: "nav2_route::ReroutingService"
+    AdjustSpeedLimit:
+      plugin: "nav2_route::AdjustSpeedLimit"
+    CollisionMonitor:
+      plugin: "nav2_route::CollisionMonitor"
+      max_collision_dist: 3.0
+    edge_cost_functions: ["DistanceScorer", "CostmapScorer"]
+    DistanceScorer:
+      plugin: "nav2_route::DistanceScorer"
+    CostmapScorer:
+      plugin: "nav2_route::CostmapScorer"
+
+velocity_smoother:
+  ros__parameters:
+    enable_stamped_cmd_vel: true
+    smoothing_frequency: 20.0
+    stamp_smoothed_velocity_with_smoothing_time: False
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.5, 0.5, 2.0]
+    min_velocity: [-0.5, -0.5, -2.0]
+    max_accel: [2.5, 2.5, 3.2]
+    max_decel: [-2.5, -2.5, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0
+
+collision_monitor:
+  ros__parameters:
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    # Publish the final /cmd_vel as TwistStamped to match the mecanum controller.
+    enable_stamped_cmd_vel: true
+    cmd_vel_in_topic: "cmd_vel_smoothed"
+    cmd_vel_out_topic: "cmd_vel"
+    state_topic: "collision_monitor_state"
+    transform_tolerance: 0.2
+    source_timeout: 1.0
+    base_shift_correction: True
+    stop_pub_timeout: 2.0
+    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
+    # and robot footprint for "approach" action type.
+    polygons: ["FootprintApproach"]
+    FootprintApproach:
+      type: "polygon"
+      action_type: "approach"
+      footprint_topic: "/local_costmap/published_footprint"
+      time_before_collision: 1.2
+      simulation_time_step: 0.1
+      min_points: 6
+      visualize: False
+      enabled: True
+    observation_sources: ["scan"]
+    scan:
+      type: "scan"
+      topic: "scan"
+      min_height: 0.15
+      max_height: 2.0
+      enabled: True
+
+docking_server:
+  ros__parameters:
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.1
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['simple_charging_dock']
+    simple_charging_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.05
+      staging_x_offset: -0.7
+      use_external_detection_pose: true
+      use_battery_status: false # true
+      use_stall_detection: false # true
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: -1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    # Dock instances
+    # The following example illustrates configuring dock instances.
+    # docks: ['home_dock']  # Input your docks here
+    # home_dock:
+    #   type: 'simple_charging_dock'
+    #   frame: map
+    #   pose: [0.0, 0.0, 0.0]
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15
+      use_collision_detection: true
+      costmap_topic: "local_costmap/costmap_raw"
+      footprint_topic: "local_costmap/published_footprint"
+      transform_tolerance: 0.1
+      projection_time: 5.0
+      simulation_step: 0.1
+      dock_collision_threshold: 0.3
+
+loopback_simulator:
+  ros__parameters:
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    map_frame_id: "map"
+    scan_frame_id: "base_scan"  # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
+    update_duration: 0.02
+    scan_range_min: 0.05
+    scan_range_max: 30.0
+    scan_angle_min: -3.1415
+    scan_angle_max: 3.1415
+    scan_angle_increment: 0.02617
+    scan_use_inf: true

upstream.jazzy.repos

@@ -2,33 +2,33 @@ repositories:
     FAST_LIO:
         type: git
         url: https://github.com/hku-mars/FAST_LIO.git
-        version: ROS2
+        version: a4743b095409588842a5b30ddfa27e29d2f99164
     livox_ros_driver2:
         type: git
         url: https://github.com/Livox-SDK/livox_ros_driver2.git
-        version: master
+        version: 13eb05e4e6dd7a765b934d0c5fd6236676a57b49
     livox_sdk:
         type: git
         url: https://github.com/Livox-SDK/Livox-SDK2.git
-        version: master
+        version: 68ae1e1dc77f61f03c95d7c2809831e198d0aedd
     Lslidar_ROS2_driver:
         type: git
         url: https://github.com/Lslidar/Lslidar_ROS2_driver.git
-        version: LS-S1_V1.0
+        version: e23139673c34c51d07053837910020aaa81736fa
     pcd2pgm:
         type: git
         url: https://github.com/LihanChen2004/pcd2pgm.git
-        version: main
+        version: 885561552baae33685e59eef41a5d8c996538702
     point_lio_ros2:
         type: git
         url: https://github.com/dfloreaa/point_lio_ros2.git
-        version: main
+        version: a8e2d0d5090af97ead8dd4fac3d37cf3dbb33ff7
     slam_gmapping:
         type: git
         url: https://github.com/Project-MANAS/slam_gmapping.git
-        version: eloquent-devel
+        version: 3c3de50c071d2c64ffe516e1ed84a574fc447b97
     unilidar_sdk2:
         type: git
         url: https://github.com/unitreerobotics/unilidar_sdk2.git
-        version: main
+        version: 0e3c51f512e6b8ff60b8c32f160b412cb48445c2