From 459b76db1d612ca96f5d3101dd608a1e7a94da8b Mon Sep 17 00:00:00 2001 From: tankaitao <1767759995@qq.com> Date: Tue, 4 Aug 2026 16:30:42 +0800 Subject: [PATCH] translate --- cmvr-es/common/types/agv/agv_types.h | 22 +++++++ cmvr-es/config/manager/device_manager.pb.txt | 2 +- cmvr-es/config/manager/task_manager.pb.txt | 2 +- cmvr-es/devices/agv/abstract_agv.h | 15 +++++ .../seer_robokit/include/seer_robokit_agv.h | 1 + .../include/seer_robokit_protocol.h | 1 + .../src/seer_robokit_navigation.cpp | 62 +++++++++++++++++++ .../service/grpc/include/grpc_agv_service.h | 4 ++ cmvr-es/service/grpc/src/grpc_agv_service.cpp | 39 ++++++++++++ protos/cmvr/api/agv_command.proto | 17 +++++ protos/cmvr/api/agv_service.proto | 4 ++ protos/cmvr/api/agv_utils.proto | 26 ++++++++ 12 files changed, 193 insertions(+), 2 deletions(-) diff --git a/cmvr-es/common/types/agv/agv_types.h b/cmvr-es/common/types/agv/agv_types.h index 0e01a02f..d5e02b4d 100644 --- a/cmvr-es/common/types/agv/agv_types.h +++ b/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 导航通用运动约束和执行选项。 * diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 8c8c691d..134e2a9e 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/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 { diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index 4d5df7a7..223f9873 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/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" diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 643fa974..40ec6cb9 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/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 发起显式站点到站点路径导航任务，并指定同步/异步选项。 * 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 37028a0f..5a8ec3a1 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 @@ -51,6 +51,7 @@ public: AgvResult followPath( const std::vector& path, const AgvMotionOptions& options) override; + AgvResult translate(const AgvTranslation& translation) override; AgvResult pauseNavigation() override; AgvResult resumeNavigation() override; AgvResult cancelNavigation() override; 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 2d581cb5..89df8282 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 @@ -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; 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 39b1ebeb..edf22ead 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 @@ -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; diff --git a/cmvr-es/service/grpc/include/grpc_agv_service.h b/cmvr-es/service/grpc/include/grpc_agv_service.h index 6ec4e65d..56e6115c 100644 --- a/cmvr-es/service/grpc/include/grpc_agv_service.h +++ b/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_; diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp index f289a587..4615e83c 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/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(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_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) diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto index 81c7676d..b434d500 100644 --- a/protos/cmvr/api/agv_command.proto +++ b/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 { // 请求体。 diff --git a/protos/cmvr/api/agv_service.proto b/protos/cmvr/api/agv_service.proto index f0bdc5b2..b1b01e7e 100644 --- a/protos/cmvr/api/agv_service.proto +++ b/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); } diff --git a/protos/cmvr/api/agv_utils.proto b/protos/cmvr/api/agv_utils.proto index e3f697a9..c05f1a79 100644 --- a/protos/cmvr/api/agv_utils.proto +++ b/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%。