Add AGV relocalization support and fix task_type handling in synchronous navigation

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 发起到世界/地图位姿的导航任务。
      */

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;
 

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));

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;

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();
             }

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,

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 =
+        const bool global_type_matches =
-            context.type == AgvTaskType::NavigateToPose
+            globalNavigationTaskTypeMatches(
-                ? 1
+                context.type,
-                : (context.type == AgvTaskType::NavigateToStation ? 2 : 3);
+                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
+                    exactTrackedNavigationTaskTypeMatches(
-                        ? task_status.type == 2
+                        context.type,
-                        : (task_status.type == 2 || task_status.type == 3);
+                        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));

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<GrpcSecurityGateway> security_gateway_;

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::AbstractAGV>(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)

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"},
+             {"switchMap", "uploadMap", "startMapping", "relocalize"},
-                 GrpcAccessClass::Mutate, CommandIntent::Configure,
+             GrpcAccessClass::Mutate, CommandIntent::Configure,
-                 SafetyPolicyFamily::Control, true);
+             SafetyPolicyFamily::Control, true);
 
         add("MotorService", "getStatus", GrpcAccessClass::Read,
             CommandIntent::Observe, SafetyPolicyFamily::Control, false);

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 {
   // 请求体。

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);