Reapply "Yuyuyu/relocation"

.github/workflows/ci.yml

@@ -67,6 +67,14 @@ jobs:
           tar -C "${GITHUB_WORKSPACE}" --exclude=".git" -cf - . \
             | tar -C /tmp/RMCS/rmcs_ws/src/skills/rmcs-navigation -xf -
 
+      - name: Clone rmcs_relocation dependency
+        shell: bash
+        run: |
+          set -euo pipefail
+          git clone --depth 1 --branch main \
+            https://github.com/Alliance-Algorithm/rmcs_relocation.git \
+            /tmp/RMCS/rmcs_ws/src/skills/rmcs_relocation
+
       - name: Build rmcs-navigation with colcon
         shell: bash
         working-directory: /tmp/RMCS/rmcs_ws

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.30)
+cmake_minimum_required(VERSION 3.28)
 
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
@@ -30,6 +30,7 @@ find_package(nav2_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rmcs_msgs REQUIRED)
+find_package(rmcs_relocation REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(tf2_ros REQUIRED)
@@ -64,6 +65,7 @@ target_include_directories(
         ${rmcs_executor_INCLUDE_DIRS}
         ${geometry_msgs_INCLUDE_DIRS}
         ${rmcs_msgs_INCLUDE_DIRS}
+        ${rmcs_relocation_INCLUDE_DIRS}
         ${std_srvs_INCLUDE_DIRS}
         ${ament_index_cpp_INCLUDE_DIRS}
         ${tf2_ros_INCLUDE_DIRS}
@@ -88,6 +90,7 @@ target_link_libraries(
         ${geometry_msgs_LIBRARIES}
         ${std_srvs_LIBRARIES}
         ${rmcs_msgs_LIBRARIES}
+        ${rmcs_relocation_LIBRARIES}
         ${tf2_ros_LIBRARIES}
         ${rclcpp_action_LIBRARIES}
         ${nav2_msgs_LIBRARIES}

package.xml

@@ -29,7 +29,8 @@
   <exec_depend>rclpy</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>rmcs_local_map</exec_depend>
-
+  <depend>rmcs_relocation</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/cxx/component.cc

@@ -5,10 +5,16 @@
 #endif
 
 #include "cxx/context.hh"
+#include "cxx/util/localization/engine.hh"
 #include "cxx/util/navigation/navigation.hh"
 #include "cxx/util/node_mixin.hh"
-
+#include <atomic>
 #include <filesystem>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
 
 #include <Eigen/Geometry>
 #include <ament_index_cpp/get_package_share_directory.hpp>
@@ -43,6 +49,8 @@ class Navigation
 
     details::Context context;
     details::Navigation navigation;
+    std::unique_ptr<Localization> localization;
+    bool relocalization_enabled = true;
 
     struct Command {
         using ChassisMode = rmcs_msgs::ChassisMode;
@@ -108,6 +116,34 @@ class Navigation
         api.set_function("update_chassis_vel", [this](double x, double y) {
             *command.chassis_speed = Eigen::Vector2d{x, y};
         });
+        // 三个 mode 共享同一个 disabled-guard + 委托模板。lua 端别名（initial / local_ / wide）
+        const auto bind_relocalize = [this, &api](std::string_view name, RelocalizeMode mode) {
+            api.set_function(
+                std::string{name}, [this, name, mode](double x, double y, double yaw) {
+                    if (!relocalization_enabled || !localization) {
+                        warn("{} ignored: disabled", name);
+                        return false;
+                    }
+                    return localization->relocalize(mode, x, y, yaw);
+                });
+        };
+        bind_relocalize("relocalize_initial", RelocalizeMode::Initial);
+        bind_relocalize("relocalize_local",   RelocalizeMode::Local);
+        bind_relocalize("relocalize_wide",    RelocalizeMode::Wide);
+
+        // status 转 lua table：disabled/enabled。
+        api.set_function("relocalize_status", [this]() {
+            const auto status = (relocalization_enabled && localization)
+                                  ? localization->relocalize_status()
+                                  : RelocalizeStatus{.message = "disabled"};
+            return lua->create_table_with(
+                "state", static_cast<int>(status.state), "success", status.success,
+                "message", status.message, "fitness_score", status.fitness_score, "confidence",
+                status.confidence, "estimated_x", status.estimated_x, "estimated_y",
+                status.estimated_y, "estimated_z", status.estimated_z, "estimated_qx",
+                status.estimated_qx, "estimated_qy", status.estimated_qy, "estimated_qz",
+                status.estimated_qz, "estimated_qw", status.estimated_qw);
+        });
     }
 
     auto make_option_injection() {
@@ -238,6 +274,21 @@ class Navigation
         , navigation{*this} {
 
         mock_context = param<bool>("mock_context");
+        relocalization_enabled =
+            has_parameter("enable_relocalization")
+                ? get_parameter_or<bool>("enable_relocalization", true)
+                : true;
+        if (relocalization_enabled) {
+            auto config = Localization::Config{ .rclcpp = *this };
+            if (has_parameter("localization.service_name"))
+                config.service_name = get_parameter("localization.service_name").as_string();
+            if (has_parameter("localization.request_timeout_sec"))
+                config.request_timeout_sec =
+                    get_parameter("localization.request_timeout_sec").as_double();
+            localization = std::make_unique<Localization>(std::move(config));
+        } else {
+            warn("relocalization is disabled by parameter enable_relocalization=false");
+        }
 
         context.init(io_mutex, mock_context);
         command.init(*this);