From abe6486e4309398d26587d23b7f8eb667609a3fa Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Wed, 19 Aug 2026 11:17:37 +0800 Subject: [PATCH] Add AGV relocalization support and fix task_type handling in synchronous navigation --- cmvr-es/devices/agv/abstract_agv.h | 7 ++++ .../seer_robokit/include/seer_robokit_agv.h | 4 ++ .../include/seer_robokit_navigation_utils.h | 33 +++++++++++++++ .../include/seer_robokit_protocol.h | 1 + .../seer_robokit/src/seer_robokit_control.cpp | 15 +++---- .../src/seer_robokit_navigation.cpp | 34 +++++++++++++++ .../src/seer_robokit_navigation_wait.cpp | 24 +++++------ .../grpc/server/include/grpc_agv_service.h | 5 +++ .../grpc/server/src/grpc_agv_service.cpp | 41 +++++++++++++++++++ .../service/grpc/server/src/grpc_security.cpp | 6 +-- protos/cmvr/api/agv_command.proto | 17 ++++++++ protos/cmvr/api/agv_service.proto | 3 ++ 12 files changed, 166 insertions(+), 24 deletions(-) diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 8f436a21..8abe3372 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -70,6 +70,13 @@ public: return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "clearFault not implemented"); } + + virtual AgvResult relocalize(const math::Pose2d& pose) + { + (void)pose; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand,"relocalize not implemented"); + } + /** * @brief 发起到世界/地图位姿的导航任务。 */ diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h index f06510f2..5db9d086 100644 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h @@ -48,6 +48,8 @@ public: AgvResult emergencyStop() override; AgvResult clearFault() override; + AgvResult relocalize(const math::Pose2d& pose) override; + AgvResult navigateToPose( const math::Pose2d& pose, @@ -83,6 +85,8 @@ public: AgvUnifiedMapUpdate& update) const override; AgvResult stopMapping() override; + + private: friend class SeerRobokitAgvTestPeer; diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h index 04a08780..89774399 100644 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h @@ -56,6 +56,39 @@ static inline bool globalTaskStateIsKnownTerminal(const int state) return state == 0 || exactTaskStateIsKnownTerminal(state); } +// Some SRC firmware versions report station navigation as task type 3. +// Exact task ids and target ids remain the primary ownership evidence. +static inline bool exactTrackedNavigationTaskTypeMatches( + const AgvTaskType expected_type, + const int actual_type) +{ + switch (expected_type) { + case AgvTaskType::NavigateToPose: + return actual_type == 1; + case AgvTaskType::NavigateToStation: + case AgvTaskType::FollowPath: + return actual_type == 2 || actual_type == 3; + default: + return false; + } +} + +static inline bool globalNavigationTaskTypeMatches( + const AgvTaskType expected_type, + const int actual_type) +{ + switch (expected_type) { + case AgvTaskType::NavigateToPose: + return actual_type == 1; + case AgvTaskType::NavigateToStation: + return actual_type == 2 || actual_type == 3; + case AgvTaskType::FollowPath: + return actual_type == 3; + default: + return false; + } +} + static inline double angleDistance(const double lhs, const double rhs) { return std::abs(std::remainder(lhs - rhs, kTwoPi)); diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h index a0dd5db7..e49cba5c 100644 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h @@ -15,6 +15,7 @@ constexpr std::uint16_t kRobotStatusStation = 1301; constexpr std::uint16_t kRobotStatusMappingFileList = 1780; constexpr std::uint16_t kRobotStatusDownloadFile = 1800; constexpr std::uint16_t kRobotControlStop = 2000; +constexpr std::uint16_t kRobotControlReloc = 2002; constexpr std::uint16_t kRobotControlMotion = 2010; constexpr std::uint16_t kRobotControlLoadMap = 2022; constexpr std::uint16_t kRobotTaskPause = 3001; diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp index 9f3f0b21..4f0fad3c 100644 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp @@ -127,15 +127,12 @@ AgvResult SeerRobokitAgv::sendControlledCommand_( "because 1101 ownership preflight was unavailable: " + snapshot_result.message); } - const int expected_type = expected_active_navigation->type - == AgvTaskType::NavigateToPose - ? 1 - : (expected_active_navigation->type - == AgvTaskType::NavigateToStation - ? 2 - : 3); const bool global_active = exactTaskStateIsActive(snapshot.task_status); + const bool global_type_matches = + globalNavigationTaskTypeMatches( + expected_active_navigation->type, + snapshot.task_type); const bool target_conflicts = global_active && !snapshot.target_id.empty() && !expected_active_navigation->target_ids.empty() @@ -145,7 +142,7 @@ AgvResult SeerRobokitAgv::sendControlledCommand_( snapshot.target_id) == expected_active_navigation->target_ids.end(); if (global_active - && (snapshot.task_type != expected_type + && (!global_type_matches || target_conflicts)) { return AgvResult::failure( AgvErrorCode::TaskCanceled, @@ -167,7 +164,7 @@ AgvResult SeerRobokitAgv::sendControlledCommand_( if (globalTaskStateIsKnownTerminal(snapshot.task_status) && snapshot.task_status != 0 && !clearing_path_queue - && snapshot.task_type == expected_type + && global_type_matches && terminal_target_matches) { return AgvResult::success(); } diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp index 94d5137a..66c496bd 100644 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp @@ -170,6 +170,40 @@ AgvResult SeerRobokitAgv::clearFault() return result.ok() ? resultFromResponse_(response) : result; } +AgvResult SeerRobokitAgv::relocalize(const math::Pose2d& pose) +{ + if (!std::isfinite(pose.x) + || !std::isfinite(pose.y) + || !std::isfinite(pose.theta)) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit relocalization pose x, y, and theta must be finite"); + } + + Json::Value payload(Json::objectValue); + jsonMember(payload, "x") = pose.x; + jsonMember(payload, "y") = pose.y; + jsonMember(payload, "angle") = pose.theta; + + Json::Value response; + std::uint64_t accepted_generation = 0; + auto result = sendControlledCommand_( + sock_control_, + kRobotControlReloc, + payload, + &response, + &accepted_generation); + + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; +} + + + + + AgvResult SeerRobokitAgv::navigateToPose( const math::Pose2d& pose, const AgvMotionOptions& options, diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp index 62aaf35c..308bde5d 100644 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp @@ -618,10 +618,10 @@ AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_( + snapshot_result.message); } last_detail += ", " + snapshot.detail; - const int expected_global_type = - context.type == AgvTaskType::NavigateToPose - ? 1 - : (context.type == AgvTaskType::NavigateToStation ? 2 : 3); + const bool global_type_matches = + globalNavigationTaskTypeMatches( + context.type, + snapshot.task_type); const bool global_target_matches = snapshot.target_id.empty() || context.target_ids.empty() || std::find( @@ -632,7 +632,7 @@ AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_( const bool global_terminal = snapshot.task_status_present && (snapshot.task_status == 0 || (exactTaskStateIsKnownTerminal(snapshot.task_status) - && snapshot.task_type == expected_global_type + && global_type_matches && global_target_matches)); const bool task_termination_confirmed = require_global_stopped ? (!any_exact_task_active && global_terminal) @@ -915,9 +915,9 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_( } if (task_status.type_present) { const bool expected_type = - context.type == AgvTaskType::NavigateToStation - ? task_status.type == 2 - : (task_status.type == 2 || task_status.type == 3); + exactTrackedNavigationTaskTypeMatches( + context.type, + task_status.type); if (!expected_type) { return failAndCancelTrackedNavigation_( context, @@ -1090,8 +1090,6 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_( continue; } - const int expected_global_type = - context.type == AgvTaskType::NavigateToStation ? 2 : 3; const bool global_active = snapshot.task_status >= 1 && snapshot.task_status <= 3; // 1110 and 1101 are separate controller publications. During the @@ -1104,7 +1102,9 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_( const bool attributed_global_active = global_attribution_ready && global_active; const bool global_type_matches = - snapshot.task_type == expected_global_type; + globalNavigationTaskTypeMatches( + context.type, + snapshot.task_type); const bool global_target_matches = context.type == AgvTaskType::NavigateToStation ? (!snapshot.target_id.empty() @@ -1185,7 +1185,7 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_( const bool expected_completed_snapshot = snapshot.task_status == 0 || (snapshot.task_status == 4 - && snapshot.task_type == expected_global_type + && global_type_matches && (context.target_id.empty() || snapshot.target_id.empty() || snapshot.target_id == context.target_id)); diff --git a/cmvr-es/service/grpc/server/include/grpc_agv_service.h b/cmvr-es/service/grpc/server/include/grpc_agv_service.h index d7975948..f52ec9b3 100644 --- a/cmvr-es/service/grpc/server/include/grpc_agv_service.h +++ b/cmvr-es/service/grpc/server/include/grpc_agv_service.h @@ -83,6 +83,11 @@ public: const api::AgvTranslateCommand_Request* request, api::AgvTranslateCommand_Feedback* response) override; + + grpc::Status relocalize(grpc::ServerContext* context, + const api::AgvRelocalizeCommand_Request* request, + api::AgvRelocalizeCommand_Feedback* response) override; + private: device::DeviceManager& dmgr_; std::shared_ptr security_gateway_; diff --git a/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp index 319b9455..0c2b196d 100644 --- a/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp @@ -722,6 +722,47 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext* context, }); } + + + + grpc::Status gRPCAgvServiceImpl::relocalize( + grpc::ServerContext* context, + const api::AgvRelocalizeCommand_Request* request, + api::AgvRelocalizeCommand_Feedback* response) +{ + return executeRegisteredGrpcCommand( + security_gateway_, context, dmgr_.safetyManager(), + "/cmvr.api.AgvService/relocalize", request, response, + [this, request, response](GrpcCommandTransaction& command) { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + + ScopedUnaryAgvControlLease control_lease( + device_id, "relocalize"); + if (!control_lease.acquired()) { + return setControlAdmissionFailure( + response, device_id, control_lease); + } + + auto dispatch = control_lease.tryBeginDispatch(); + if (!dispatch.acquired()) { + return setControlDispatchFailure( + response, device_id, control_lease, "relocalize"); + } + + if (!command.beginDispatch()) { + return command.dispatchStatus(); + } + + return setResponseResult( + response, + agv->relocalize(toPose2d(request->pose()))); + }); +} + grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context, const api::AgvNavigateToPoseCommand_Request* request, api::AgvNavigateToPoseCommand_Feedback* response) diff --git a/cmvr-es/service/grpc/server/src/grpc_security.cpp b/cmvr-es/service/grpc/server/src/grpc_security.cpp index 91271d8d..62e92016 100644 --- a/cmvr-es/service/grpc/server/src/grpc_security.cpp +++ b/cmvr-es/service/grpc/server/src/grpc_security.cpp @@ -466,9 +466,9 @@ const GrpcMethodPolicyRegistry& defaultGrpcMethodPolicyRegistry() GrpcAccessClass::Mutate, CommandIntent::Actuate, SafetyPolicyFamily::Control, true); add_many("AgvService", - {"switchMap", "uploadMap", "startMapping"}, - GrpcAccessClass::Mutate, CommandIntent::Configure, - SafetyPolicyFamily::Control, true); + {"switchMap", "uploadMap", "startMapping", "relocalize"}, + GrpcAccessClass::Mutate, CommandIntent::Configure, + SafetyPolicyFamily::Control, true); add("MotorService", "getStatus", GrpcAccessClass::Read, CommandIntent::Observe, SafetyPolicyFamily::Control, false); diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto index b434d500..47578e2e 100644 --- a/protos/cmvr/api/agv_command.proto +++ b/protos/cmvr/api/agv_command.proto @@ -37,6 +37,23 @@ message AgvNavigationStatusCommand { } } + +// 在当前活动地图中重新设置 AGV 定位位姿。 +message AgvRelocalizeCommand { + message Request { + // header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + // x/y 单位：米，theta 单位：弧度，逆时针为正。 + cmvr.msgs.AgvPose2d pose = 2; + } + + message Feedback { + // 成功仅表示控制器接受命令，不表示定位已经收敛。 + CommandHeader.Feedback header = 1; + } +} + + // 导航到指定地图位姿命令。 message AgvNavigateToPoseCommand { // 请求体。 diff --git a/protos/cmvr/api/agv_service.proto b/protos/cmvr/api/agv_service.proto index b1b01e7e..b8de93da 100644 --- a/protos/cmvr/api/agv_service.proto +++ b/protos/cmvr/api/agv_service.proto @@ -20,6 +20,9 @@ service AgvService { // 清除可恢复故障或告警。 rpc clearFault(CommandHeader.Request) returns (CommandHeader.Feedback); + // 在当前活动地图中重新设置定位位姿。调用前 AGV 必须已经停止。 + rpc relocalize(AgvRelocalizeCommand.Request)returns (AgvRelocalizeCommand.Feedback); + // 导航到指定地图位姿。目标位姿 x/y 单位为米，theta 单位为弧度。 rpc navigateToPose(AgvNavigateToPoseCommand.Request) returns (AgvNavigateToPoseCommand.Feedback);