cmvr-es/devices/agv/abstract_agv.h

@@ -70,13 +70,6 @@ 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,8 +48,6 @@ public:
 
     AgvResult emergencyStop() override;
     AgvResult clearFault() override;
-    AgvResult relocalize(const math::Pose2d& pose) override;
-
 
     AgvResult navigateToPose(
         const math::Pose2d& pose,
@@ -85,8 +83,6 @@ 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,39 +56,6 @@ 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,7 +15,6 @@ 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,12 +127,15 @@ 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()
@@ -142,7 +145,7 @@ AgvResult SeerRobokitAgv::sendControlledCommand_(
                    snapshot.target_id)
                 == expected_active_navigation->target_ids.end();
         if (global_active
-            && (!global_type_matches
+            && (snapshot.task_type != expected_type
                 || target_conflicts)) {
             return AgvResult::failure(
                 AgvErrorCode::TaskCanceled,
@@ -164,7 +167,7 @@ AgvResult SeerRobokitAgv::sendControlledCommand_(
             if (globalTaskStateIsKnownTerminal(snapshot.task_status)
                 && snapshot.task_status != 0
                 && !clearing_path_queue
-                && global_type_matches
+                && snapshot.task_type == expected_type
                 && terminal_target_matches) {
                 return AgvResult::success();
             }

cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp

@@ -170,40 +170,6 @@ 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 bool global_type_matches =
-            globalNavigationTaskTypeMatches(
-                context.type,
-                snapshot.task_type);
+        const int expected_global_type =
+            context.type == AgvTaskType::NavigateToPose
+                ? 1
+                : (context.type == AgvTaskType::NavigateToStation ? 2 : 3);
         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)
-                 && global_type_matches
+                 && snapshot.task_type == expected_global_type
                  && 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 =
-                    exactTrackedNavigationTaskTypeMatches(
-                        context.type,
-                        task_status.type);
+                    context.type == AgvTaskType::NavigateToStation
+                        ? task_status.type == 2
+                        : (task_status.type == 2 || task_status.type == 3);
                 if (!expected_type) {
                     return failAndCancelTrackedNavigation_(
                         context,
@@ -1090,6 +1090,8 @@ 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
@@ -1102,9 +1104,7 @@ AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_(
         const bool attributed_global_active = global_attribution_ready
             && global_active;
         const bool global_type_matches =
-            globalNavigationTaskTypeMatches(
-                context.type,
-                snapshot.task_type);
+            snapshot.task_type == expected_global_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
-                    && global_type_matches
+                    && snapshot.task_type == expected_global_type
                     && (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,11 +83,6 @@ 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,47 +722,6 @@ 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", "relocalize"},
-             GrpcAccessClass::Mutate, CommandIntent::Configure,
-             SafetyPolicyFamily::Control, true);
+                 {"switchMap", "uploadMap", "startMapping"},
+                 GrpcAccessClass::Mutate, CommandIntent::Configure,
+                 SafetyPolicyFamily::Control, true);
 
         add("MotorService", "getStatus", GrpcAccessClass::Read,
             CommandIntent::Observe, SafetyPolicyFamily::Control, false);

protos/cmvr/api/agv_command.proto

@@ -37,23 +37,6 @@ 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,9 +20,6 @@ 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);