feat/relocation

CMakeLists.txt

@@ -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)
@@ -65,6 +66,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}
@@ -89,6 +91,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}

launch/sensor.launch.yaml

@@ -12,6 +12,9 @@ launch:
   - arg:
       name: "global_map"
       default: "empty"
+  - arg:
+      name: "launch_relocation"
+      default: "true"
 
   # Private
   - arg:
@@ -33,6 +36,9 @@ launch:
   - include:
       if: "$(var launch_livox)"
       file: "$(find-pkg-share point_lio)/launch/point_lio.launch.py"
+  - include:
+      if: "$(var launch_relocation)"
+      file: "$(find-pkg-share rmcs_relocation)/launch/location.launch.py"
 
   # 2. 生命周期管理器
   - let:
@@ -85,6 +91,7 @@ launch:
 
   # 5. 初始变换
   - node:
+      unless: "$(var launch_relocation)"
       pkg: "tf2_ros"
       exec: "static_transform_publisher"
       args: "0 0 0 0 0 0 world odom"

package.xml

@@ -15,6 +15,7 @@
   <depend>rclcpp_action</depend>
   <depend>rmcs_executor</depend>
   <depend>rmcs_msgs</depend>
+  <depend>rmcs_relocation</depend>
   <depend>nav2_msgs</depend>
 
   <exec_depend>launch</exec_depend>

src/cxx/component.cc

@@ -2,10 +2,14 @@
 #include "cxx/controller/normal.hh"
 #include "cxx/lua_context.hh"
 #include "cxx/navigation.hh"
+#include "cxx/util/localization/engine.hh"
 #include "cxx/util/node_mixin.hh"
 
+#include <atomic>
 #include <chrono>
 #include <cmath>
+#include <limits>
+#include <memory>
 #include <unordered_map>
 
 #include <Eigen/Geometry>
@@ -33,6 +37,8 @@ class Navigation
     details::LuaContext lua_context{*this};
     details::Navigation navigation{*this};
     details::Context context{*this, *this};
+    std::unique_ptr<Localization> localization;
+    bool relocalization_enabled = true;
 
     // 控制器框架
     IController* selected_controller = nullptr;
@@ -97,6 +103,20 @@ class Navigation
         : rclcpp::Node{get_component_name(), option()} {
 
         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 {
+            logging::warn("relocalization is disabled by parameter enable_relocalization=false");
+        }
 
         context.init(io_mutex, mock_context);
         command.init(*this);
@@ -117,6 +137,41 @@ class Navigation
                     selected_controller = controllers.at(mode).get();
                     logging::info("switched to controller '{}'", mode);
                 },
+            .relocalize_initial =
+                [this](double x, double y, double yaw) {
+                    if (!relocalization_enabled || !localization) {
+                        logging::warn("relocalize_initial ignored: disabled");
+                        return false;
+                    }
+                    return localization->relocalize(RelocalizeMode::Initial, x, y, yaw);
+                },
+            .relocalize_local =
+                [this](double x, double y, double yaw) {
+                    if (!relocalization_enabled || !localization) {
+                        logging::warn("relocalize_local ignored: disabled");
+                        return false;
+                    }
+                    return localization->relocalize(RelocalizeMode::Local, x, y, yaw);
+                },
+            .relocalize_wide =
+                [this](double x, double y, double yaw) {
+                    if (!relocalization_enabled || !localization) {
+                        logging::warn("relocalize_wide ignored: disabled");
+                        return false;
+                    }
+                    return localization->relocalize(RelocalizeMode::Wide, x, y, yaw);
+                },
+            .relocalize_status =
+                [this] {
+                    if (!relocalization_enabled || !localization) {
+                        return RelocalizeStatus{
+                            .state = RelocalizeState::FAILED,
+                            .success = false,
+                            .message = "disabled",
+                        };
+                    }
+                    return localization->relocalize_status();
+                },
         });
 
         controllers["normal"] = std::make_unique<NormalController>();

src/cxx/lua_context.cc

@@ -39,6 +39,7 @@ struct LuaContext::Impl {
             "failed to get lua api");
 
         auto api = api_result.get<sol::table>();
+        auto relocalize_status = std::move(api_impl.relocalize_status);
         api.set_function(
             "info", [this](const std::string& text) { logging.info("Lua: {}", text); });
         api.set_function(
@@ -50,6 +51,25 @@ struct LuaContext::Impl {
         api.set_function("switch_topic_forward", std::move(api_impl.switch_topic_forward));
         api.set_function("update_gimbal_direction", std::move(api_impl.update_gimbal_direction));
         api.set_function("switch_controller", std::move(api_impl.switch_controller));
+        api.set_function("relocalize_initial", std::move(api_impl.relocalize_initial));
+        api.set_function("relocalize_local", std::move(api_impl.relocalize_local));
+        api.set_function("relocalize_wide", std::move(api_impl.relocalize_wide));
+        api.set_function(
+            "relocalize_status", [this, relocalize_status = std::move(relocalize_status)] {
+                const auto status = relocalize_status ? relocalize_status()
+                                                      : RelocalizeStatus{
+                                                            .state = RelocalizeState::FAILED,
+                                                            .success = false,
+                                                            .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() -> void {

src/cxx/lua_context.hh

@@ -1,4 +1,5 @@
 #pragma once
+#include "cxx/util/localization/engine.hh"
 #include "cxx/util/pimpl.hh"
 
 #include <functional>
@@ -19,6 +20,10 @@ public:
         std::function<void(bool)> switch_topic_forward;
         std::function<void(double)> update_gimbal_direction;
         std::function<void(const std::string&)> switch_controller;
+        std::function<bool(double, double, double)> relocalize_initial;
+        std::function<bool(double, double, double)> relocalize_local;
+        std::function<bool(double, double, double)> relocalize_wide;
+        std::function<RelocalizeStatus()> relocalize_status;
     };
 
     explicit LuaContext(rclcpp::Node& node) noexcept;