translate

cmvr-es/common/types/agv/agv_types.h

@@ -91,6 +91,14 @@ enum class AgvTaskType {
     Custom
 };
 
+/**
+ * @brief 固定距离平移使用的距离参考模式。
+ */
+enum class AgvTranslationMode {
+    Odometry = 0,
+    Localization
+};
+
 /**
  * @brief AGV 车体坐标系下的平面速度。
  *
@@ -102,6 +110,20 @@ struct AgvVelocity {
     double wz{0.0};
 };
 
+
+
+/**
+ * @brief AGV 车体坐标系下的固定距离平移参数。
+ */
+struct AgvTranslation {
+    double distance{0.0};
+    double vx{0.0};
+    double vy{0.0};
+    AgvTranslationMode mode{AgvTranslationMode::Odometry};
+};
+
+
+
 /**
  * @brief 导航通用运动约束和执行选项。
  *

cmvr-es/config/manager/device_manager.pb.txt

@@ -132,7 +132,7 @@ device_manager {
     id: "src1100"
     type: DEVICE_TYPE_AGV
     config_file: "devices/agv/seer_robokit.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {

cmvr-es/config/manager/task_manager.pb.txt

@@ -28,7 +28,7 @@ task_manager {
     run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
     config_file: "tasks/quic_edge_task/quic_edge_task.pb.txt"
     # Host-development default: no QUIC Gateway or physical media devices.
-    enable: true
+    enable: false
   }
   tasks {
     id: "ume_teleop"

cmvr-es/devices/agv/abstract_agv.h

@@ -92,6 +92,21 @@ public:
         return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented");
     }
 
+
+
+    /**
+     * @brief 按指定速度执行固定距离平移。
+     *
+     * 返回成功表示控制器已经接受命令，不表示运动已经完成。
+     */
+    virtual AgvResult translate(const AgvTranslation& translation)
+    {
+        (void)translation;
+        return AgvResult::failure(
+            AgvErrorCode::UnsupportedCommand,
+            "translate not implemented");
+    }
+
     /**
      * @brief 发起显式站点到站点路径导航任务，并指定同步/异步选项。
      *

cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h

@@ -51,6 +51,7 @@ public:
     AgvResult followPath(
         const std::vector<AgvPathSegment>& path,
         const AgvMotionOptions& options) override;
+    AgvResult translate(const AgvTranslation& translation) override;
     AgvResult pauseNavigation() override;
     AgvResult resumeNavigation() override;
     AgvResult cancelNavigation() override;

cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h

@@ -21,6 +21,7 @@ constexpr std::uint16_t kRobotTaskPause = 3001;
 constexpr std::uint16_t kRobotTaskResume = 3002;
 constexpr std::uint16_t kRobotTaskCancel = 3003;
 constexpr std::uint16_t kRobotTaskGoTarget = 3051;
+constexpr std::uint16_t kRobotTaskTranslate = 3055;
 constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
 constexpr std::uint16_t kRobotTaskClearTargetList = 3067;
 constexpr std::uint16_t kRobotConfigLock = 4005;

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

@@ -621,6 +621,68 @@ AgvResult SeerRobokitAgv::followPath(
             options));
 }
 
+
+AgvResult SeerRobokitAgv::translate(
+        const AgvTranslation& translation)
+{
+    if (!std::isfinite(translation.distance)
+        || !std::isfinite(translation.vx)
+        || !std::isfinite(translation.vy)) {
+        return AgvResult::failure(
+            AgvErrorCode::InvalidArgument,
+            "SEER Robokit translation distance, vx, and vy must be finite");
+        }
+    if (translation.distance <= 0.0) {
+        return AgvResult::failure(
+            AgvErrorCode::InvalidArgument,
+            "SEER Robokit translation distance must be greater than zero");
+    }
+    if (translation.vx == 0.0 && translation.vy == 0.0) {
+        return AgvResult::failure(
+            AgvErrorCode::InvalidArgument,
+            "SEER Robokit translation requires a non-zero vx or vy");
+    }
+
+    int mode = 0;
+    switch (translation.mode) {
+    case AgvTranslationMode::Odometry:
+        mode = 0;
+        break;
+    case AgvTranslationMode::Localization:
+        mode = 1;
+        break;
+    default:
+        return AgvResult::failure(
+            AgvErrorCode::InvalidArgument,
+            "SEER Robokit translation mode must be odometry or localization");
+    }
+
+    Json::Value payload(Json::objectValue);
+    jsonMember(payload, "dist") = translation.distance;
+    jsonMember(payload, "vx") = translation.vx;
+    jsonMember(payload, "vy") = translation.vy;
+    jsonMember(payload, "mode") = mode;
+
+    Json::Value response;
+    std::uint64_t accepted_generation = 0;
+    auto result = sendControlledCommand_(
+        sock_navigation_,
+        kRobotTaskTranslate,
+        payload,
+        &response,
+        &accepted_generation);
+
+    // API 3055 does not expose a task id. Do not publish a fake tracked task,
+    // but invalidate navigation state if the command may have been accepted.
+    if (accepted_generation != 0) {
+        clearPoseTask_(accepted_generation);
+    }
+    return result;
+}
+
+
+
+
 AgvResult SeerRobokitAgv::pauseNavigation()
 {
     Json::Value response;

cmvr-es/service/grpc/include/grpc_agv_service.h

@@ -72,6 +72,10 @@ public:
     grpc::Status stopMapping(grpc::ServerContext* context,
                              const api::CommandHeader_Request* request,
                              api::CommandHeader_Feedback* response) override;
+    grpc::Status translate(
+            grpc::ServerContext* context,
+            const api::AgvTranslateCommand_Request* request,
+            api::AgvTranslateCommand_Feedback* response) override;
 
 private:
     device::DeviceManager& dmgr_;

cmvr-es/service/grpc/src/grpc_agv_service.cpp

@@ -121,6 +121,16 @@ device::AgvVelocity toVelocity(const msgs::AgvVelocity& src)
     return {src.vx(), src.vy(), src.wz()};
 }
 
+device::AgvTranslation toTranslation(const msgs::AgvTranslation& src)
+{
+    return {
+        src.distance(),
+        src.vx(),
+        src.vy(),
+        static_cast<device::AgvTranslationMode>(src.mode())
+    };
+}
+
 device::AgvPathSegment toPathSegment(const msgs::AgvPathSegment& src)
 {
     device::AgvPathSegment dst;
@@ -501,6 +511,35 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context,
     }
 }
 
+
+
+grpc::Status gRPCAgvServiceImpl::translate(
+        grpc::ServerContext* context,
+        const api::AgvTranslateCommand_Request* request,
+        api::AgvTranslateCommand_Feedback* response)
+{
+    try {
+        if (context && context->IsCancelled()) {
+            return setNavigationRequestCanceled(response);
+        }
+        const std::string device_id = request->header().device_id();
+        auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
+        if (!agv) {
+            return setDeviceNotFound(response, device_id);
+        }
+        return setResponseResult(
+            response,
+            agv->translate(toTranslation(request->translation())));
+    } catch (const std::exception& e) {
+        fillFeedback(response->mutable_header(), false, e.what());
+        return grpc::Status(
+            grpc::StatusCode::INTERNAL,
+            e.what());
+    }
+}
+
+
+
 grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*,
                                                  const api::CommandHeader_Request* request,
                                                  api::CommandHeader_Feedback* response)

protos/cmvr/api/agv_command.proto

@@ -95,6 +95,23 @@ message AgvFollowPathCommand {
   }
 }
 
+
+// 按指定速度执行固定距离平移命令。
+message AgvTranslateCommand {
+  message Request {
+    // 通用请求头。header.device_id 指定目标 AGV 设备。
+    CommandHeader.Request header = 1;
+    // 固定距离平移参数。
+    cmvr.msgs.AgvTranslation translation = 2;
+  }
+
+  message Feedback {
+    // 仅表示控制器是否接受命令，不表示平移已经完成。
+    CommandHeader.Feedback header = 1;
+  }
+}
+
+
 // 下发底盘速度命令。
 message AgvSetVelocityCommand {
   // 请求体。

protos/cmvr/api/agv_service.proto

@@ -69,4 +69,8 @@ service AgvService {
 
   // 停止当前建图/扫图会话。
   rpc stopMapping(CommandHeader.Request) returns (CommandHeader.Feedback);
+
+
+  // 按指定速度平移固定距离。成功返回仅表示控制器已接受命令。
+  rpc translate(AgvTranslateCommand.Request) returns (AgvTranslateCommand.Feedback);
 }

protos/cmvr/api/agv_utils.proto

@@ -14,6 +14,29 @@ message AgvPose2d {
   double theta = 3;
 }
 
+// 固定距离平移使用的距离参考模式。
+enum AgvTranslationMode {
+  // 根据底盘里程计算运动距离。
+  AGV_TRANSLATION_MODE_ODOMETRY = 0;
+  // 根据定位结果计算运动距离。
+  AGV_TRANSLATION_MODE_LOCALIZATION = 1;
+}
+
+
+
+// AGV 车体坐标系下的固定距离平移参数。
+message AgvTranslation {
+  // 平移距离的绝对值，单位：米，必须大于 0。
+  double distance = 1;
+  // 车体 X 方向速度，单位：米/秒；正为向前，负为向后。
+  double vx = 2;
+  // 车体 Y 方向速度，单位：米/秒；正为向左，负为向右。
+  double vy = 3;
+  // 距离参考模式；默认使用里程模式。
+  AgvTranslationMode mode = 4;
+}
+
+
 // AGV 车体坐标系下的平面速度。
 message AgvVelocity {
   // 车体 X 方向线速度，单位：米/秒。
@@ -24,6 +47,9 @@ message AgvVelocity {
   double wz = 3;
 }
 
+
+
+
 // AGV 电池状态。
 message AgvBatteryState {
   // 电量比例，范围：[0, 1]，例如 0.8 表示 80%。