diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f4133bb..c73cb9e 100644 --- a/.github/workflows/ci.yml +++ b/.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 diff --git a/CMakeLists.txt b/CMakeLists.txt index f5fbed2..3a72c4d 100644 --- a/CMakeLists.txt +++ b/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} diff --git a/package.xml b/package.xml index b89ba8f..8382a8d 100644 --- a/package.xml +++ b/package.xml @@ -29,7 +29,8 @@ rclpy tf2_ros rmcs_local_map - + rmcs_relocation + ament_cmake diff --git a/src/cxx/component.cc b/src/cxx/component.cc index 6c543c8..94210a6 100644 --- a/src/cxx/component.cc +++ b/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 #include +#include +#include +#include +#include +#include #include #include @@ -43,6 +49,8 @@ class Navigation details::Context context; details::Navigation navigation; + std::unique_ptr localization; + bool relocalization_enabled = true; struct Command { OutputInterface chassis_speed; @@ -102,6 +110,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(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() { @@ -232,6 +268,21 @@ class Navigation , navigation{*this} { mock_context = param("mock_context"); + relocalization_enabled = + has_parameter("enable_relocalization") + ? get_parameter_or("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(std::move(config)); + } else { + warn("relocalization is disabled by parameter enable_relocalization=false"); + } context.init(io_mutex, mock_context); command.init(*this); diff --git a/src/cxx/util/localization/engine.cc b/src/cxx/util/localization/engine.cc index b7bc6fe..68ec6ea 100644 --- a/src/cxx/util/localization/engine.cc +++ b/src/cxx/util/localization/engine.cc @@ -1,74 +1,206 @@ #include "cxx/util/localization/engine.hh" #include "cxx/util/node_mixin.hh" -#include -#include -#include -#include - -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include namespace rmcs::navigation { -struct Localization::Impl { - Config config; - LoggerWrap logging; - - struct Context { - using Point = pcl::PointXYZ; - using Cloud = pcl::PointCloud; - - std::shared_ptr> subscription; - std::atomic stop_collecting = false; - - std::shared_ptr map; - std::shared_ptr collected = std::make_shared(); - - explicit Context(Config& config) { - const auto& filename = config.map_filename; - if (pcl::io::loadPCDFile(filename, *map) != 0) - throw std::runtime_error{"Couldn't read " + filename}; - - auto& rclcpp = config.rclcpp; - subscription = rclcpp.create_subscription( - config.topic_registered, 10, - [this](const std::unique_ptr& msg) { - if (!stop_collecting) { - auto received = Cloud{}; - pcl::fromROSMsg(*msg, received); - - *collected += received; - } - }); - } - } context; +namespace { + +using Relocalize = rmcs_relocation::srv::Relocalize; +using RelocalizeClient = rclcpp::Client; + +auto yaw_to_pose(double x, double y, double yaw) -> geometry_msgs::msg::Pose { + auto q = tf2::Quaternion{}; + q.setRPY(0.0, 0.0, yaw); + q.normalize(); + auto pose = geometry_msgs::msg::Pose{}; + pose.position.x = x; + pose.position.y = y; + pose.position.z = 0.0; + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + pose.orientation.w = q.w(); + return pose; +} + +auto failed_status(std::string message) -> RelocalizeStatus { + return RelocalizeStatus{ + .state = RelocalizeState::FAILED, + .success = false, + .message = std::move(message), + }; +} + +auto status_from_response(const Relocalize::Response& response, bool success) -> RelocalizeStatus { + const auto& pose = response.estimated_world_base; + return RelocalizeStatus{ + .state = success ? RelocalizeState::SUCCEEDED : RelocalizeState::FAILED, .success = success, + .message = response.message, .fitness_score = response.fitness_score, .confidence = response.confidence, + .estimated_x = pose.position.x, .estimated_y = pose.position.y, .estimated_z = pose.position.z, + .estimated_qx = pose.orientation.x, .estimated_qy = pose.orientation.y, .estimated_qz = pose.orientation.z, + .estimated_qw = pose.orientation.w, + }; +} - pcl::NormalDistributionsTransform engine; +constexpr auto mode_to_msg(RelocalizeMode mode) -> std::uint8_t { + switch (mode) { + case RelocalizeMode::Initial: return Relocalize::Request::MODE_INITIAL; + case RelocalizeMode::Local: return Relocalize::Request::MODE_LOCAL; + case RelocalizeMode::Wide: return Relocalize::Request::MODE_WIDE; + } + return Relocalize::Request::MODE_INITIAL; +} - explicit Impl(Localization::Config config) - : config{std::move(config)} - , logging{config.rclcpp} - , context{config} { +} // namespace + +struct Session { + mutable std::mutex mutex; + std::shared_ptr client; + rclcpp::Logger logger; + std::string service_name; + + std::optional pending_id; + RelocalizeStatus last_status{}; + rclcpp::TimerBase::SharedPtr timeout_timer; + + Session(std::shared_ptr c, rclcpp::Logger lg, std::string name) + : client{std::move(c)}, logger{std::move(lg)}, service_name{std::move(name)} {} + + /// 抢占一个 in-flight 槽位。若已在飞行中返回 false。 + auto begin() -> bool { + auto lock = std::scoped_lock{mutex}; + if (last_status.state == RelocalizeState::IN_FLIGHT) + return false; + pending_id.reset(); + last_status = + RelocalizeStatus{.state = RelocalizeState::IN_FLIGHT, .message = "in_flight"}; + return true; + } - engine.setTransformationEpsilon(config.ndt_result_epsilon); // 收敛判定阈值 - engine.setStepSize(config.ndt_step_size); // More-Thuente 线搜索最大步长 - engine.setResolution(config.ndt_resolution); // NDT 网格分辨率 - engine.setMaximumIterations(config.ndt_max_iterations); // 最大迭代次数 + auto track(std::int64_t id) -> void { + auto lock = std::scoped_lock{mutex}; + pending_id = id; } - auto start_collecting(std::chrono::seconds seconds) -> std::expected { - std::ignore = this; - std::ignore = seconds; - return {}; + /// 写入终态。expected_id != nullopt 时仅当与当前 pending 一致才接受（过滤迟到响应）。 + auto end(RelocalizeStatus status, std::optional expected_id = std::nullopt) + -> bool { + auto lock = std::scoped_lock{mutex}; + if (expected_id && pending_id != *expected_id) + return false; + clear_locked(); + last_status = std::move(status); + return true; } - auto start_localizing(const Eigen::Isometry3d& initial_solution) - -> std::future> { - std::ignore = this; - std::ignore = initial_solution; - return {}; + auto snapshot() const -> RelocalizeStatus { + auto lock = std::scoped_lock{mutex}; + return last_status; + } + + /// 必须在持锁状态下调用：取消定时器、从 client 移除 pending 条目。 + auto clear_locked() -> void { + if (timeout_timer) { + timeout_timer->cancel(); + timeout_timer.reset(); + } + if (pending_id) { + client->remove_pending_request(*pending_id); + pending_id.reset(); + } + } +}; + +struct Localization::Impl : NodeMixin { + Config config; + rclcpp::Node& node; + std::shared_ptr session; + + explicit Impl(Localization::Config cfg) + : config{std::move(cfg)} + , node{config.rclcpp} + , session{std::make_shared( + node.create_client(config.service_name), node.get_logger(), + config.service_name)} {} + + ~Impl() { + auto lock = std::scoped_lock{session->mutex}; + session->clear_locked(); + } + + auto get_logger() const { return node.get_logger(); } + + auto arm_timeout(const std::shared_ptr& s, const std::shared_ptr& pending_id) + -> void { + const auto timeout = std::chrono::duration_cast( + std::chrono::duration{std::max(0.1, config.request_timeout_sec)}); + auto timer = node.create_wall_timer(timeout, [s, pending_id]() { + if (s->end(failed_status("request timeout: " + s->service_name), *pending_id)) + RCLCPP_WARN(s->logger, "relocalize request timeout: %s", s->service_name.c_str()); + }); + auto lock = std::scoped_lock{s->mutex}; + if (s->timeout_timer) + s->timeout_timer->cancel(); + s->timeout_timer = std::move(timer); + } + + auto send(RelocalizeMode mode, double x, double y, double yaw) -> bool { + if (!session->begin()) { + warn("relocalize skipped: previous request is still in flight"); + return false; + } + + if (!session->client->service_is_ready() + && !session->client->wait_for_service(std::chrono::seconds(0))) { + warn("relocalize service unavailable: {}", config.service_name); + session->end(failed_status("service unavailable: " + config.service_name)); + return false; + } + + auto request = std::make_shared(); + request->mode = mode_to_msg(mode); + request->initial_guess_world_base = yaw_to_pose(x, y, yaw); + + // executor 异步触发 callback。 + auto pending_id = std::make_shared(-1); + arm_timeout(session, pending_id); + auto s = session; + auto result = session->client->async_send_request( + std::move(request), [s, pending_id](RelocalizeClient::SharedFuture future) { + auto response = future.get(); + if (!response) { + RCLCPP_WARN(s->logger, "relocalize response is null"); + s->end(failed_status("response is null"), *pending_id); + return; + } + if (!response->success) { + RCLCPP_WARN(s->logger, "relocalize failed: %s", response->message.c_str()); + s->end(status_from_response(*response, false), *pending_id); + return; + } + RCLCPP_INFO( + s->logger, "relocalize success: score=%.4f, confidence=%.3f", + response->fitness_score, response->confidence); + s->end(status_from_response(*response, true), *pending_id); + }); + *pending_id = result.request_id; + session->track(result.request_id); + return true; } }; @@ -77,14 +209,12 @@ Localization::Localization(Config config) Localization::~Localization() noexcept = default; -auto Localization::start_collecting(std::chrono::seconds seconds) - -> std::expected { - return pimpl->start_collecting(seconds); +auto Localization::relocalize(RelocalizeMode mode, double x, double y, double yaw) -> bool { + return pimpl->send(mode, x, y, yaw); } -auto Localization::start_localizing(const Eigen::Isometry3d& initial_solution) - -> std::future> { - return pimpl->start_localizing(initial_solution); +auto Localization::relocalize_status() const -> RelocalizeStatus { + return pimpl->session->snapshot(); } } // namespace rmcs::navigation diff --git a/src/cxx/util/localization/engine.hh b/src/cxx/util/localization/engine.hh index ac111e5..17a2a4b 100644 --- a/src/cxx/util/localization/engine.hh +++ b/src/cxx/util/localization/engine.hh @@ -1,37 +1,49 @@ #pragma once #include "cxx/util/pimpl.hh" -#include -#include -#include +#include #include +#include namespace rmcs::navigation { +enum class RelocalizeState : std::uint8_t { + IDLE = 0, + IN_FLIGHT = 1, + SUCCEEDED = 2, + FAILED = 3, +}; + +enum class RelocalizeMode : std::uint8_t { + Initial = 0, + Local = 1, + Wide = 2, +}; + +struct RelocalizeStatus { + RelocalizeState state = RelocalizeState::IDLE; + bool success = false; + std::string message; + double fitness_score = 0.0, confidence = 0.0; + double estimated_x = 0.0, estimated_y = 0.0, estimated_z = 0.0; + double estimated_qx = 0.0, estimated_qy = 0.0, estimated_qz = 0.0, estimated_qw = 1.0; +}; + class Localization { RMCS_PIMPL_DEFINITION(Localization) public: struct Config { rclcpp::Node& rclcpp; - - std::string topic_registered; - std::string map_filename; - - float ndt_resolution = 1.0; - double ndt_step_size = 0.1; - double ndt_result_epsilon = 0.01; - int ndt_max_iterations = 50; + std::string service_name = "/rmcs_relocation/relocalize"; + double request_timeout_sec = 10.0; }; explicit Localization(Config config); - // 开始收集配准好的点云 - auto start_collecting(std::chrono::seconds seconds) -> std::expected; - - // 开始重定位 - auto start_localizing(const Eigen::Isometry3d& initial_solution) - -> std::future>; + /// 唯一入口，返回 true=请求已挂起；false=被 in_flight / service 不可用拦截。 + auto relocalize(RelocalizeMode mode, double x, double y, double yaw) -> bool; + auto relocalize_status() const -> RelocalizeStatus; }; } // namespace rmcs::navigation diff --git a/src/lua/action.lua b/src/lua/action.lua index e9193b6..7210e8e 100644 --- a/src/lua/action.lua +++ b/src/lua/action.lua @@ -1,9 +1,17 @@ local util = require("util.math") local api = require("api") local request = require("util.scheduler").request +local blackboard = require("blackboard").singleton() local NaN = 0 / 0 +local RelocalizeState = { + IDLE = 0, + IN_FLIGHT = 1, + SUCCEEDED = 2, + FAILED = 3, +} + local action = { target = { x = NaN, @@ -11,6 +19,10 @@ local action = { }, } +local function pose_unavailable(x, y, yaw) + return x == nil or y == nil or yaw == nil or util.check_nan(x, y, yaw) +end + --- 绑定 action 的后台任务。 --- @param scheduler Scheduler function action:bind(scheduler) @@ -77,6 +89,68 @@ function action:update_chassis_vel(x, y) api.update_chassis_vel(x, y) end +--- 三模式共享：发起 → 轮询 → 终态。 +--- @return ok boolean 是否被服务端 accept +--- @return st table? 末态 status（被 in-flight 拦截时为 nil） +local function send_and_await(self, mode, fn, x, y, yaw) + if not fn(x, y, yaw) then + self:warn(string.format("reloc skip %s (in_flight)", mode)) + return false, nil + end + while true do + local st = api.relocalize_status() + if st.state == RelocalizeState.SUCCEEDED then + self:info(string.format( + "reloc ok [%s] score=%.4f conf=%.3f", mode, st.fitness_score, st.confidence)) + return true, st + end + if st.state ~= RelocalizeState.IN_FLIGHT then + self:warn(string.format( + "reloc fail [%s] score=%.4f conf=%.3f | %s", + mode, st.fitness_score, st.confidence, tostring(st.message))) + return false, st + end + request:sleep(0.1) + end +end + +--- INITIAL 模式：x/y/yaw 是 GICP 起点（建图原点，一般为（0,0,0））。开局重定位用。 +function action:relocalize_initial(x, y, yaw) + return send_and_await(self, "initial", api.relocalize_initial, x, y, yaw) +end + +--- LOCAL 模式（SC-driven）：持续重试直到成功。 +--- blackboard.user.{x,y,yaw} 仅作 validator 锚点（拦镜像错配，红蓝对称场地有双高峰现象）； +--- LIO/TF 丢失（nil/NaN）时直接返回失败，不重试也不切 WIDE。 +function action:relocalize_local() + local attempt = 1 + while true do + local user = blackboard.user + if pose_unavailable(user.x, user.y, user.yaw) then + self:warn("reloc skip local (LIO/TF lost, no validator anchor)") + return false, nil + end + + local ok, st = send_and_await(self, "local_", api.relocalize_local, user.x, user.y, user.yaw) + if ok then return true, st end + self:warn(string.format("local failed, retrying (attempt %d)", attempt)) + attempt = attempt + 1 + request:yield() + end +end + +--- WIDE 模式：blackboard.user 作 validator prior（NaN 时退化到原点 + 无 prior 验收，pointlio崩了就和原点配）。 +function action:relocalize_wide() + local user = blackboard.user + local x, y, yaw = user.x, user.y, user.yaw + if pose_unavailable(x, y, yaw) then x, y, yaw = 0.0, 0.0, 0.0 end + return send_and_await(self, "wide", api.relocalize_wide, x, y, yaw) +end + +function action:relocalize_status() + return api.relocalize_status() +end + function action:restart_navigation(config) return api.restart_navigation(config) end diff --git a/src/lua/api.lua b/src/lua/api.lua index e7cef9e..be08d30 100644 --- a/src/lua/api.lua +++ b/src/lua/api.lua @@ -17,6 +17,10 @@ local util = require("util.native") --- @field update_gimbal_dominator fun(name: string) --- @field update_chassis_mode fun(mode: string) --- @field update_chassis_vel fun(x: number, y: number) +--- @field relocalize_initial fun(x: number, y: number, yaw: number): boolean +--- @field relocalize_local fun(x: number, y: number, yaw: number): boolean +--- @field relocalize_wide fun(x: number, y: number, yaw: number): boolean +--- @field relocalize_status fun(): { state: integer, success: boolean, message: string, fitness_score: number, confidence: number } --- local api = setmetatable({}, { __index = function(_, name)