From 5a6b75938debb8c4230ce0c7d23404200ae315cf Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Thu, 2 Jul 2026 12:30:07 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E4=BB=99=E5=B7=A5=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E5=99=A8SRC1100=E7=9A=84=E9=9B=86=E6=88=90=EF=BC=8C?= =?UTF-8?q?=E6=9C=AA=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 6 +- cmvr-es/common/types/agv/agv_types.h | 251 +++++++ cmvr-es/common/types/geometry_types.h | 2 +- cmvr-es/config/devices/agv/agv.pb.txt | 15 + cmvr-es/config/manager/device_manager.pb.txt | 7 + cmvr-es/devices/agv/CMakeLists.txt | 2 + cmvr-es/devices/agv/abstract_agv.h | 215 +++++- cmvr-es/devices/agv/agv_factory.h | 11 + cmvr-es/devices/agv/my_agv/include/my_agv.h | 16 +- cmvr-es/devices/agv/my_agv/src/my_agv.cpp | 44 +- cmvr-es/devices/agv/src1100/CMakeLists.txt | 12 + .../devices/agv/src1100/include/src1100_agv.h | 104 +++ .../devices/agv/src1100/src/src1100_agv.cpp | 619 ++++++++++++++++++ cmvr-es/devices/state_define.h | 10 +- cmvr-es/service/CMakeLists.txt | 1 + .../service/grpc/include/grpc_agv_service.h | 79 +++ cmvr-es/service/grpc/src/grpc_agv_service.cpp | 523 +++++++++++++++ .../include/grpc_server_task.h | 1 + .../grpc_server_task/src/grpc_server_task.cpp | 4 + protos/cmvr/api/agv_command.proto | 114 ++++ protos/cmvr/api/agv_service.proto | 28 + .../cmvr/config/agv_config/agv_config.proto | 15 + protos/cmvr/msgs/agv.proto | 73 +++ 23 files changed, 2078 insertions(+), 74 deletions(-) create mode 100644 cmvr-es/common/types/agv/agv_types.h create mode 100644 cmvr-es/devices/agv/src1100/CMakeLists.txt create mode 100644 cmvr-es/devices/agv/src1100/include/src1100_agv.h create mode 100644 cmvr-es/devices/agv/src1100/src/src1100_agv.cpp create mode 100644 cmvr-es/service/grpc/include/grpc_agv_service.h create mode 100644 cmvr-es/service/grpc/src/grpc_agv_service.cpp create mode 100644 protos/cmvr/api/agv_command.proto create mode 100644 protos/cmvr/api/agv_service.proto create mode 100644 protos/cmvr/msgs/agv.proto diff --git a/CMakeLists.txt b/CMakeLists.txt index e12aaf6c..0609aed3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,8 +46,10 @@ file(GLOB_RECURSE PROTO_FILES ${PROTO_IMPORT_DIR}/*.proto) set(Protobuf_PROTOC_EXECUTABLE "${CMAKE_INSTALL_PREFIX}/bin/protoc" CACHE FILEPATH "" FORCE) -set_property(TARGET gRPC::grpc_cpp_plugin - PROPERTY IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin" +set_target_properties(gRPC::grpc_cpp_plugin + PROPERTIES + IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin" + IMPORTED_LOCATION_RELEASE "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin" ) # 1) 先做 OBJECT：只负责生成/编译 pb.cc diff --git a/cmvr-es/common/types/agv/agv_types.h b/cmvr-es/common/types/agv/agv_types.h new file mode 100644 index 00000000..12f53b3f --- /dev/null +++ b/cmvr-es/common/types/agv/agv_types.h @@ -0,0 +1,251 @@ +#ifndef CMVR_ES_AGV_TYPES_H +#define CMVR_ES_AGV_TYPES_H + +#include +#include +#include +#include + +#include "common/types/geometry_types.h" + +namespace cmvr::device { + +/** + * @brief AGV 通用命令/结果错误类别。 + * + * 这些枚举描述框架层面的通用结果。厂商或控制器特有错误码应由具体 + * AGV 实现转换，或保存在该实现私有的适配参数/细节中。 + */ +enum class AgvErrorCode { + OK = 0, + NotConnected, + AlreadyConnected, + ConnectionFailed, + Timeout, + InvalidArgument, + LocalizationLost, + MapNotLoaded, + TaskRejected, + TaskFailed, + TaskCanceled, + CommandFailed, + EmergencyStopped, + Fault, + UnsupportedCommand, + UnknownError +}; + +/** + * @brief AGV 命令的标准返回值。 + */ +struct AgvResult { + AgvErrorCode code{AgvErrorCode::OK}; + std::string message{"OK"}; + + bool ok() const { return code == AgvErrorCode::OK; } + static AgvResult success() { return {AgvErrorCode::OK, "OK"}; } + static AgvResult failure(AgvErrorCode c, const std::string& msg) { return {c, msg}; } +}; + +/** + * @brief AGV 粗粒度运行模式。 + */ +enum class AgvMode { + Unknown = 0, + Disconnected, + Idle, + Manual, + Auto, + Charging, + Paused, + Stopped, + Fault, + EmergencyStop +}; + +/** + * @brief 当前跟踪的导航任务状态。 + */ +enum class AgvTaskState { + None = 0, + Waiting, + Running, + Paused, + Completed, + Failed, + Canceled +}; + +/** + * @brief 当前跟踪的导航任务类型。 + */ +enum class AgvTaskType { + None = 0, + NavigateToPose, + NavigateToStation, + FollowPath, + Dock, + Charge, + Custom +}; + +/** + * @brief AGV 车体坐标系下的平面速度。 + * + * 线速度单位为米/秒，角速度单位为弧度/秒。 + */ +struct AgvVelocity { + double vx{0.0}; + double vy{0.0}; + double wz{0.0}; +}; + +/** + * @brief 导航通用运动约束和执行选项。 + * + * 除非具体实现另有说明，数值限制为 0 表示使用设备或控制器默认值。 + */ +struct AgvMotionOptions { + double max_speed{0.0}; + double max_angular_speed{0.0}; + double max_acceleration{0.0}; + double max_angular_acceleration{0.0}; + double reach_distance{0.0}; + double reach_angle{0.0}; + double speed_ratio{1.0}; + bool asynchronous{true}; +}; + +/** + * @brief AGV 适配器可选的实现特定参数。 + * + * 该结构用于避免抽象接口绑定某一个控制器协议。具体 AGV 驱动可以按需 + * 解释操作名、特殊运动模式、设备特定标志等键值。 + */ +struct AgvAdapterParams { + std::unordered_map values; + + bool empty() const { return values.empty(); } + + std::optional getString(const std::string& key) const + { + const auto it = values.find(key); + if (it == values.end()) { + return std::nullopt; + } + return it->second; + } + + std::optional getDouble(const std::string& key) const + { + const auto value = getString(key); + if (!value) { + return std::nullopt; + } + try { + return std::stod(*value); + } catch (...) { + return std::nullopt; + } + } + + std::optional getBool(const std::string& key) const + { + const auto value = getString(key); + if (!value) { + return std::nullopt; + } + if (*value == "1" || *value == "true" || *value == "yes" || *value == "on") { + return true; + } + if (*value == "0" || *value == "false" || *value == "no" || *value == "off") { + return false; + } + return std::nullopt; + } +}; + +/** + * @brief AGV 驱动连接信息。 + * + * 简单设备可只使用 ip/port。多端口、认证信息或其他连接参数可通过 + * adapter_params 传给具体实现。 + */ +struct AgvConnectionOptions { + std::string ip; + int port{0}; + AgvAdapterParams adapter_params; +}; + +/** + * @brief 作为 AgvRuntimeState 一部分暴露的电池信息。 + */ +struct AgvBatteryState { + double percentage{0.0}; + double voltage{0.0}; + double current{0.0}; + double temperature{0.0}; + bool charging{false}; +}; + +/** + * @brief AGV 当前运行状态快照。 + * + * 这是 AGV 设备的主要状态查询对象。抽象接口中应避免派生出的便利 + * getter；调用方可直接从该快照读取字段。 + */ +struct AgvRuntimeState { + double timestamp{0.0}; + AgvMode mode{AgvMode::Unknown}; + bool connected{false}; + bool localized{false}; + bool moving{false}; + bool fault{false}; + bool emergency_stopped{false}; + math::Pose2d pose{}; + AgvVelocity velocity{}; + AgvBatteryState battery{}; + std::string current_map; + std::string current_station; + std::string last_error; +}; + +/** + * @brief AGV 抽象层可见的地图站点/路径点。 + */ +struct AgvStation { + std::string id; + std::string type; + math::Pose2d pose{}; + std::string description; +}; + +/** + * @brief 显式导航路径中的一段站点到站点路径。 + */ +struct AgvPathSegment { + std::string source_station; + std::string target_station; +}; + +/** + * @brief 当前导航任务状态。 + * + * 该状态独立于 AgvRuntimeState，因为导航任务可能处于排队、暂停、 + * 完成或失败状态，而车辆本体仍然保持连接并处于正常状态。 + */ +struct AgvNavigationStatus { + AgvTaskState state{AgvTaskState::None}; + AgvTaskType type{AgvTaskType::None}; + double progress{0.0}; + std::string message; +}; + +/** + * @brief 兼容旧 AGV 状态 API 命名的别名。 + */ +using AGVState = AgvRuntimeState; + +} // namespace cmvr::device + +#endif // CMVR_ES_AGV_TYPES_H diff --git a/cmvr-es/common/types/geometry_types.h b/cmvr-es/common/types/geometry_types.h index e1378bf6..76fa0054 100644 --- a/cmvr-es/common/types/geometry_types.h +++ b/cmvr-es/common/types/geometry_types.h @@ -49,7 +49,7 @@ namespace cmvr::math { typedef struct { double x; //* unit: m double y; - double theta; + double theta; //* unit: rad } Pose2d; } diff --git a/cmvr-es/config/devices/agv/agv.pb.txt b/cmvr-es/config/devices/agv/agv.pb.txt index 45881d7a..44b94ba7 100644 --- a/cmvr-es/config/devices/agv/agv.pb.txt +++ b/cmvr-es/config/devices/agv/agv.pb.txt @@ -6,4 +6,19 @@ agv { port: 8080 } } + + agvs { + id: "agv_src1100" + src1100_agv { + ip: "192.168.192.5" + enable: false + port_status: 19204 + port_control: 19205 + port_nav: 19206 + port_config: 19207 + port_other: 19210 + port_push: 19301 + recv_timeout_ms: 1000 + } + } } diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 6b05c688..ad9c4633 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -65,4 +65,11 @@ device_manager { config_file: "devices/agv/agv.pb.txt" enable: false } + + devices { + id: "agv_src1100" + type: DEVICE_TYPE_AGV + config_file: "devices/agv/agv.pb.txt" + enable: false + } } diff --git a/cmvr-es/devices/agv/CMakeLists.txt b/cmvr-es/devices/agv/CMakeLists.txt index d3963c05..a0ca27e4 100644 --- a/cmvr-es/devices/agv/CMakeLists.txt +++ b/cmvr-es/devices/agv/CMakeLists.txt @@ -1,4 +1,5 @@ add_subdirectory(my_agv) +add_subdirectory(src1100) add_library(agv INTERFACE) @@ -7,6 +8,7 @@ target_include_directories(agv INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(agv INTERFACE cmvr_es::device::my_agv + cmvr_es::device::src1100_agv cmvr_es::proto ) diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 788ae2a1..3811283e 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -6,32 +6,203 @@ #define CMVR_ES_ABSTRACT_AGV_H #pragma once +#include +#include + +#include "common/types/agv/agv_types.h" #include "devices/abstract_device.h" -namespace cmvr::device{ - class AbstractAGV: public AbstractDevice { - public: - AbstractAGV() = default; - ~AbstractAGV() override=default; +namespace cmvr::device { - DeviceKind kind() const noexcept override { return DeviceKind::AGV; } - virtual bool getState(AGVState &state) { return true; } +/** + * @brief AGV/移动底盘设备抽象基类。 + * + * 该接口只描述通用 AGV 能力。控制器特有的请求/响应字段应放在具体 + * 驱动类中；当公共 API 需要扩展点时，可通过 AgvAdapterParams 传递。 + */ +class AbstractAGV : public AbstractDevice { +public: + AbstractAGV() = default; + ~AbstractAGV() override = default; - // navigation - virtual bool eStop() { return true; } - virtual bool goHome() { return true; } - virtual bool moveto(math::Pose2d &location, double speed_ratio) { return true; } - virtual bool setVelocity(math::Vec3 linear, math::Vec3 angular) { return true; } + DeviceKind kind() const noexcept override { return DeviceKind::AGV; } - // map - virtual bool initMap(float resolution, int width, int height) { return true; } - virtual bool updateMap() { return true; } - virtual bool saveMap(const std::string& file_path) { return true; } - virtual bool loadMap(const std::string& file_path) { return true; } + /** + * @brief 获取 AGV 运行状态快照。 + */ + virtual AgvRuntimeState runtimeState() const { return {}; } - protected: - AGVState state_; - }; -} + /** + * @brief 获取当前导航任务状态。 + */ + virtual AgvNavigationStatus navigationStatus() const { return {}; } -#endif //CMVR_ES_ABSTRACT_AGV_H + /** + * @brief 建立与 AGV 的通信连接。 + */ + virtual AgvResult connect(const AgvConnectionOptions& options = {}) + { + (void)options; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "connect not implemented"); + } + + /** + * @brief 关闭与 AGV 的通信连接。 + */ + virtual AgvResult disconnect() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "disconnect not implemented"); + } + + /** + * @brief 触发 AGV 急停行为。 + */ + virtual AgvResult emergencyStop() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "emergencyStop not implemented"); + } + + /** + * @brief 清除可恢复的 AGV 故障或告警。 + */ + virtual AgvResult clearFault() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "clearFault not implemented"); + } + + /** + * @brief 发起到世界/地图位姿的导航任务。 + */ + virtual AgvResult navigateToPose( + const math::Pose2d& pose, + const AgvMotionOptions& options = {}, + const AgvAdapterParams& adapter_params = AgvAdapterParams{}) + { + (void)pose; + (void)options; + (void)adapter_params; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "navigateToPose not implemented"); + } + + /** + * @brief 发起到指定地图站点的导航任务。 + */ + virtual AgvResult navigateToStation( + const std::string& station_id, + const AgvMotionOptions& options = {}, + const AgvAdapterParams& adapter_params = AgvAdapterParams{}) + { + (void)station_id; + (void)options; + (void)adapter_params; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "navigateToStation not implemented"); + } + + /** + * @brief 发起显式站点到站点路径导航任务。 + */ + virtual AgvResult followPath(const std::vector& path) + { + (void)path; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented"); + } + + /** + * @brief 暂停当前导航任务，如果设备支持。 + */ + virtual AgvResult pauseNavigation() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "pauseNavigation not implemented"); + } + + /** + * @brief 恢复已暂停的导航任务，如果设备支持。 + */ + virtual AgvResult resumeNavigation() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "resumeNavigation not implemented"); + } + + /** + * @brief 取消当前导航任务，如果设备支持。 + */ + virtual AgvResult cancelNavigation() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "cancelNavigation not implemented"); + } + + /** + * @brief 向 AGV 下发低层速度控制指令。 + * + * 该接口不同于导航命令。具体实现应明确速度控制在导航过程中是中断 + * 导航、与导航共存，还是被拒绝执行。 + */ + virtual AgvResult setVelocity(const AgvVelocity& velocity) + { + (void)velocity; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "setVelocity not implemented"); + } + + /** + * @brief 通过下发零速度停止低层速度控制。 + * + * 该接口不表示取消正在执行的导航任务；取消导航请使用 + * cancelNavigation()。 + */ + virtual AgvResult stopVelocityControl() + { + return setVelocity(AgvVelocity{}); + } + + /** + * @brief 查询 AGV 可用地图名称列表。 + */ + virtual AgvResult listMaps(std::vector& maps) const + { + (void)maps; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "listMaps not implemented"); + } + + /** + * @brief 查询当前活动地图中的站点列表。 + */ + virtual AgvResult listStations(std::vector& stations) const + { + (void)stations; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "listStations not implemented"); + } + + /** + * @brief 切换当前活动地图。 + */ + virtual AgvResult switchMap(const std::string& map_name) + { + (void)map_name; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "switchMap not implemented"); + } + + /** + * @brief 按名称上传或替换地图。 + */ + virtual AgvResult uploadMap(const std::string& map_name, const std::string& content) + { + (void)map_name; + (void)content; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "uploadMap not implemented"); + } + + /** + * @brief 按名称下载地图内容。 + */ + virtual AgvResult downloadMap(const std::string& map_name, std::string& content) const + { + (void)map_name; + (void)content; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "downloadMap not implemented"); + } + +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_ABSTRACT_AGV_H diff --git a/cmvr-es/devices/agv/agv_factory.h b/cmvr-es/devices/agv/agv_factory.h index b866face..79b2f0a3 100644 --- a/cmvr-es/devices/agv/agv_factory.h +++ b/cmvr-es/devices/agv/agv_factory.h @@ -7,6 +7,7 @@ #include "common/base/logging/logger.h" #include "devices/agv/abstract_agv.h" #include "devices/agv/my_agv/include/my_agv.h" +#include "devices/agv/src1100/include/src1100_agv.h" namespace cmvr::device { @@ -30,6 +31,16 @@ public: backend.set_id(cfg.id()); return std::make_shared(backend); } + case config::AGVDeviceConfig::kSrc1100Agv: + { + if (!cfg.src1100_agv().id().empty() && cfg.src1100_agv().id() != cfg.id()) { + CMVR_LOG(ERROR) << "[AGVFactory]: AGV id does not match backend id: " << cfg.id(); + return nullptr; + } + auto backend = cfg.src1100_agv(); + backend.set_id(cfg.id()); + return std::make_shared(backend); + } case config::AGVDeviceConfig::BACKEND_NOT_SET: default: diff --git a/cmvr-es/devices/agv/my_agv/include/my_agv.h b/cmvr-es/devices/agv/my_agv/include/my_agv.h index 8f0a788a..f8f69f9f 100644 --- a/cmvr-es/devices/agv/my_agv/include/my_agv.h +++ b/cmvr-es/devices/agv/my_agv/include/my_agv.h @@ -20,15 +20,13 @@ public: bool stop() override; bool update() override; - bool getState(AGVState& state) override; - bool eStop() override; - bool goHome() override; - bool moveto(math::Pose2d& location, double speed_ratio) override; - bool setVelocity(math::Vec3 linear, math::Vec3 angular) override; - bool initMap(float resolution, int width, int height) override; - bool updateMap() override; - bool saveMap(const std::string& file_path) override; - bool loadMap(const std::string& file_path) override; + AgvRuntimeState runtimeState() const override; + AgvResult emergencyStop() override; + AgvResult navigateToPose( + const math::Pose2d& pose, + const AgvMotionOptions& options = {}, + const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override; + AgvResult setVelocity(const AgvVelocity& velocity) override; private: config::MyAgvConfig config_; diff --git a/cmvr-es/devices/agv/my_agv/src/my_agv.cpp b/cmvr-es/devices/agv/my_agv/src/my_agv.cpp index a1c6ea77..dd9b6b7a 100644 --- a/cmvr-es/devices/agv/my_agv/src/my_agv.cpp +++ b/cmvr-es/devices/agv/my_agv/src/my_agv.cpp @@ -27,49 +27,27 @@ bool MyAgv::update() return true; } -bool MyAgv::getState(AGVState&) +AgvRuntimeState MyAgv::runtimeState() const { - return true; + return {}; } -bool MyAgv::eStop() +AgvResult MyAgv::emergencyStop() { - return true; + return AgvResult::success(); } -bool MyAgv::goHome() +AgvResult MyAgv::navigateToPose( + const math::Pose2d&, + const AgvMotionOptions&, + const AgvAdapterParams&) { - return true; + return AgvResult::success(); } -bool MyAgv::moveto(math::Pose2d&, double) +AgvResult MyAgv::setVelocity(const AgvVelocity&) { - return true; -} - -bool MyAgv::setVelocity(math::Vec3, math::Vec3) -{ - return true; -} - -bool MyAgv::initMap(float, int, int) -{ - return true; -} - -bool MyAgv::updateMap() -{ - return true; -} - -bool MyAgv::saveMap(const std::string&) -{ - return true; -} - -bool MyAgv::loadMap(const std::string&) -{ - return true; + return AgvResult::success(); } } // namespace cmvr::device diff --git a/cmvr-es/devices/agv/src1100/CMakeLists.txt b/cmvr-es/devices/agv/src1100/CMakeLists.txt new file mode 100644 index 00000000..6ad1f3fe --- /dev/null +++ b/cmvr-es/devices/agv/src1100/CMakeLists.txt @@ -0,0 +1,12 @@ +add_library(src1100_agv SHARED src/src1100_agv.cpp) + +target_include_directories(src1100_agv PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + +target_link_libraries(src1100_agv + PUBLIC + cmvr_es::proto + jsoncpp +) + +add_library(cmvr_es::device::src1100_agv ALIAS src1100_agv) +install(TARGETS src1100_agv LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h new file mode 100644 index 00000000..4e52af69 --- /dev/null +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -0,0 +1,104 @@ +#ifndef CMVR_ES_SRC1100_AGV_H +#define CMVR_ES_SRC1100_AGV_H + +#include +#include +#include + +#include + +#include "cmvr/config/agv_config/agv_config.pb.h" +#include "devices/agv/abstract_agv.h" + +namespace cmvr::device { + +class Src1100Agv final : public AbstractAGV { +public: + explicit Src1100Agv(const config::Src1100AgvConfig& cfg); + ~Src1100Agv() override; + + std::string typeName() const override { return "Src1100Agv"; } + + bool init() override; + bool start() override; + bool stop() override; + bool update() override; + + AgvRuntimeState runtimeState() const override; + AgvNavigationStatus navigationStatus() const override; + + AgvResult connect(const AgvConnectionOptions& options = {}) override; + AgvResult disconnect() override; + AgvResult emergencyStop() override; + AgvResult clearFault() override; + + AgvResult navigateToPose( + const math::Pose2d& pose, + const AgvMotionOptions& options = {}, + const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override; + AgvResult navigateToStation( + const std::string& station_id, + const AgvMotionOptions& options = {}, + const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override; + AgvResult followPath(const std::vector& path) override; + AgvResult pauseNavigation() override; + AgvResult resumeNavigation() override; + AgvResult cancelNavigation() override; + + AgvResult setVelocity(const AgvVelocity& velocity) override; + + AgvResult listMaps(std::vector& maps) const override; + AgvResult listStations(std::vector& stations) const override; + AgvResult switchMap(const std::string& map_name) override; + AgvResult uploadMap(const std::string& map_name, const std::string& content) override; + AgvResult downloadMap(const std::string& map_name, std::string& content) const override; + +private: + struct Ports { + int status{19204}; + int control{19205}; + int navigation{19206}; + int config{19207}; + int other{19210}; + int push{19301}; + }; + + AgvResult connectSocket_(int& sock, int port); + void closeSocket_(int& sock) const; + bool connected_() const; + + AgvResult sendCommand_(int sock, + std::uint16_t command, + const Json::Value& payload, + Json::Value* response) const; + AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const; + + static std::vector buildFrame_(std::uint16_t command, const std::string& payload); + static std::string toJsonString_(const Json::Value& value); + static bool parseJson_(const std::string& input, Json::Value& output, std::string& error); + static std::string extractJson_(const std::string& raw); + static int optionalInt_(const AgvAdapterParams& params, const std::string& key, int fallback); + static double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback); + static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options); + static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params); + static AgvResult resultFromResponse_(const Json::Value& response); + + config::Src1100AgvConfig config_; + std::string ip_; + bool enable_{true}; + int recv_timeout_ms_{1000}; + Ports ports_; + + mutable std::mutex mutex_; + int sock_status_{-1}; + int sock_control_{-1}; + int sock_navigation_{-1}; + int sock_config_{-1}; + int sock_other_{-1}; + int sock_push_{-1}; + std::string last_error_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_SRC1100_AGV_H diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp new file mode 100644 index 00000000..65c07fc5 --- /dev/null +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -0,0 +1,619 @@ +#include "devices/agv/src1100/include/src1100_agv.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::device { + +namespace { + +constexpr std::uint16_t kRobotStatusLoc = 1004; +constexpr std::uint16_t kRobotStatusBattery = 1007; +constexpr std::uint16_t kRobotStatusTask = 1020; +constexpr std::uint16_t kRobotStatusMap = 1300; +constexpr std::uint16_t kRobotStatusStation = 1301; +constexpr std::uint16_t kRobotControlStop = 2000; +constexpr std::uint16_t kRobotControlMotion = 2010; +constexpr std::uint16_t kRobotControlLoadMap = 2022; +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 kRobotTaskGoTargetList = 3066; +constexpr std::uint16_t kRobotConfigUploadMap = 4010; +constexpr std::uint16_t kRobotConfigDownloadMap = 4011; + +std::string systemError() +{ + return std::strerror(errno); +} + +Json::Value& jsonMember(Json::Value& value, const char* key) +{ + return *value.demand(key, key + std::strlen(key)); +} + +Json::Value& jsonMember(Json::Value& value, const std::string& key) +{ + return *value.demand(key.data(), key.data() + key.size()); +} + +const Json::Value* jsonFind(const Json::Value& value, const char* key) +{ + return value.find(key, key + std::strlen(key)); +} + +Json::Value jsonGet(const Json::Value& value, const char* key, const Json::Value& fallback) +{ + const auto* found = jsonFind(value, key); + return found ? *found : fallback; +} + +AgvMode modeFromTaskState(const int state) +{ + switch (state) { + case 2: + return AgvMode::Auto; + case 3: + return AgvMode::Paused; + case 5: + return AgvMode::Fault; + case 6: + return AgvMode::Stopped; + default: + return AgvMode::Idle; + } +} + +AgvTaskState toTaskState(const int value) +{ + switch (value) { + case 1: + return AgvTaskState::Waiting; + case 2: + return AgvTaskState::Running; + case 3: + return AgvTaskState::Paused; + case 4: + return AgvTaskState::Completed; + case 5: + return AgvTaskState::Failed; + case 6: + return AgvTaskState::Canceled; + case 0: + default: + return AgvTaskState::None; + } +} + +AgvTaskType toTaskType(const int value) +{ + switch (value) { + case 1: + return AgvTaskType::NavigateToPose; + case 2: + return AgvTaskType::NavigateToStation; + case 3: + return AgvTaskType::FollowPath; + case 100: + return AgvTaskType::Custom; + default: + return AgvTaskType::None; + } +} + +} // namespace + +Src1100Agv::Src1100Agv(const config::Src1100AgvConfig& cfg) + : config_(cfg), + ip_(cfg.ip()), + enable_(cfg.enable()), + recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000) +{ + id_ = cfg.id(); + if (cfg.port_status() > 0) ports_.status = cfg.port_status(); + if (cfg.port_control() > 0) ports_.control = cfg.port_control(); + if (cfg.port_nav() > 0) ports_.navigation = cfg.port_nav(); + if (cfg.port_config() > 0) ports_.config = cfg.port_config(); + if (cfg.port_other() > 0) ports_.other = cfg.port_other(); + if (cfg.port_push() > 0) ports_.push = cfg.port_push(); +} + +Src1100Agv::~Src1100Agv() +{ + (void)disconnect(); +} + +bool Src1100Agv::init() +{ + return !id_.empty() && !ip_.empty(); +} + +bool Src1100Agv::start() +{ + if (!enable_) { + return true; + } + return connect().ok(); +} + +bool Src1100Agv::stop() +{ + return disconnect().ok(); +} + +bool Src1100Agv::update() +{ + return true; +} + +AgvRuntimeState Src1100Agv::runtimeState() const +{ + AgvRuntimeState state; + state.connected = connected_(); + state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected; + state.last_error = last_error_; + + Json::Value loc; + if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) { + state.pose.x = jsonGet(loc, "x", 0.0).asDouble(); + state.pose.y = jsonGet(loc, "y", 0.0).asDouble(); + state.pose.theta = jsonGet(loc, "angle", 0.0).asDouble(); + state.localized = jsonGet(loc, "confidence", 0.0).asDouble() > 0.0; + state.current_station = jsonGet(loc, "current_station", "").asString(); + } + + Json::Value battery; + if (sendCommand_(sock_status_, kRobotStatusBattery, Json::Value(Json::objectValue), &battery).ok()) { + state.battery.percentage = jsonGet(battery, "battery_level", 0.0).asDouble(); + state.battery.temperature = jsonGet(battery, "battery_temp", 0.0).asDouble(); + state.battery.charging = jsonGet(battery, "charging", false).asBool(); + state.battery.voltage = jsonGet(battery, "voltage", 0.0).asDouble(); + state.battery.current = jsonGet(battery, "current", 0.0).asDouble(); + } + + Json::Value map; + if (sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &map).ok()) { + state.current_map = jsonGet(map, "current_map", "").asString(); + } + + const auto nav = navigationStatus(); + state.moving = nav.state == AgvTaskState::Running; + state.fault = nav.state == AgvTaskState::Failed; + state.mode = state.fault ? AgvMode::Fault : modeFromTaskState(static_cast(nav.state)); + return state; +} + +AgvNavigationStatus Src1100Agv::navigationStatus() const +{ + AgvNavigationStatus status; + Json::Value payload(Json::objectValue); + jsonMember(payload, "simple") = false; + + Json::Value response; + const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response); + if (!result.ok()) { + status.state = AgvTaskState::Failed; + status.message = result.message; + return status; + } + + status.state = toTaskState(jsonGet(response, "task_status", 0).asInt()); + status.type = toTaskType(jsonGet(response, "task_type", 0).asInt()); + status.message = jsonGet(response, "move_status_info", jsonGet(response, "err_msg", "")).asString(); + if (const auto* task_status_package = jsonFind(response, "task_status_package")) { + status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble(); + } + return status; +} + +AgvResult Src1100Agv::connect(const AgvConnectionOptions& options) +{ + std::lock_guard lock(mutex_); + if (!options.ip.empty()) { + ip_ = options.ip; + } + if (options.port > 0) { + ports_.control = options.port; + } + ports_.status = optionalInt_(options.adapter_params, "port_status", ports_.status); + ports_.control = optionalInt_(options.adapter_params, "port_control", ports_.control); + ports_.navigation = optionalInt_(options.adapter_params, "port_nav", ports_.navigation); + ports_.config = optionalInt_(options.adapter_params, "port_config", ports_.config); + ports_.other = optionalInt_(options.adapter_params, "port_other", ports_.other); + ports_.push = optionalInt_(options.adapter_params, "port_push", ports_.push); + + closeSocket_(sock_status_); + closeSocket_(sock_control_); + closeSocket_(sock_navigation_); + closeSocket_(sock_config_); + closeSocket_(sock_other_); + closeSocket_(sock_push_); + + if (ip_.empty()) { + return AgvResult::failure(AgvErrorCode::InvalidArgument, "SRC1100 AGV ip is empty"); + } + + if (auto result = connectSocket_(sock_status_, ports_.status); !result.ok()) return result; + if (auto result = connectSocket_(sock_control_, ports_.control); !result.ok()) return result; + if (auto result = connectSocket_(sock_navigation_, ports_.navigation); !result.ok()) return result; + if (auto result = connectSocket_(sock_config_, ports_.config); !result.ok()) return result; + return AgvResult::success(); +} + +AgvResult Src1100Agv::disconnect() +{ + std::lock_guard lock(mutex_); + closeSocket_(sock_status_); + closeSocket_(sock_control_); + closeSocket_(sock_navigation_); + closeSocket_(sock_config_); + closeSocket_(sock_other_); + closeSocket_(sock_push_); + return AgvResult::success(); +} + +AgvResult Src1100Agv::emergencyStop() +{ + return cancelNavigation(); +} + +AgvResult Src1100Agv::clearFault() +{ + return AgvResult::success(); +} + +AgvResult Src1100Agv::navigateToPose( + const math::Pose2d& pose, + const AgvMotionOptions& options, + const AgvAdapterParams& adapter_params) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); + jsonMember(payload, "id") = adapter_params.getString("target_id").value_or(""); + jsonMember(payload, "skill_name") = adapter_params.getString("skill_name").value_or("GotoSpecifiedPose"); + auto& free_go = jsonMember(payload, "freeGo"); + jsonMember(free_go, "x") = pose.x; + jsonMember(free_go, "y") = pose.y; + jsonMember(free_go, "theta") = pose.theta; + applyMotionOptions_(payload, options); + applyAdapterParams_(payload, adapter_params); + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::navigateToStation( + const std::string& station_id, + const AgvMotionOptions& options, + const AgvAdapterParams& adapter_params) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); + jsonMember(payload, "id") = station_id; + applyMotionOptions_(payload, options); + applyAdapterParams_(payload, adapter_params); + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::followPath(const std::vector& path) +{ + Json::Value payload(Json::objectValue); + Json::Value tasks(Json::arrayValue); + int index = 0; + for (const auto& segment : path) { + Json::Value task(Json::objectValue); + jsonMember(task, "task_id") = id_ + "_path_" + std::to_string(index++); + jsonMember(task, "source_id") = segment.source_station; + jsonMember(task, "id") = segment.target_station; + tasks.append(task); + } + jsonMember(payload, "move_task_list") = tasks; + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::pauseNavigation() +{ + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskPause, Json::Value(Json::objectValue), &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::resumeNavigation() +{ + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskResume, Json::Value(Json::objectValue), &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::cancelNavigation() +{ + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskCancel, Json::Value(Json::objectValue), &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "vx") = velocity.vx; + jsonMember(payload, "vy") = velocity.vy; + jsonMember(payload, "w") = velocity.wz; + jsonMember(payload, "duration") = -1; + Json::Value response; + auto result = sendCommand_(sock_control_, kRobotControlMotion, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::listMaps(std::vector& maps) const +{ + Json::Value response; + auto result = sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &response); + if (!result.ok()) return result; + maps.clear(); + if (const auto* values = jsonFind(response, "maps"); values && values->isArray()) { + for (const auto& value : *values) { + maps.push_back(value.asString()); + } + } + return resultFromResponse_(response); +} + +AgvResult Src1100Agv::listStations(std::vector& stations) const +{ + Json::Value response; + auto result = sendCommand_(sock_status_, kRobotStatusStation, Json::Value(Json::objectValue), &response); + if (!result.ok()) return result; + stations.clear(); + if (const auto* values = jsonFind(response, "stations"); values && values->isArray()) { + for (const auto& value : *values) { + AgvStation station; + station.id = jsonGet(value, "id", "").asString(); + station.type = jsonGet(value, "type", "").asString(); + station.pose.x = jsonGet(value, "x", 0.0).asDouble(); + station.pose.y = jsonGet(value, "y", 0.0).asDouble(); + station.pose.theta = jsonGet(value, "r", 0.0).asDouble(); + station.description = jsonGet(value, "desc", "").asString(); + stations.push_back(station); + } + } + return resultFromResponse_(response); +} + +AgvResult Src1100Agv::switchMap(const std::string& map_name) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "map_name") = map_name; + Json::Value response; + auto result = sendCommand_(sock_control_, kRobotControlLoadMap, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::uploadMap(const std::string& map_name, const std::string& content) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "map_name") = map_name; + jsonMember(payload, "map_content") = content; + Json::Value response; + auto result = sendCommand_(sock_config_, kRobotConfigUploadMap, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::downloadMap(const std::string& map_name, std::string& content) const +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "map_name") = map_name; + Json::Value response; + auto result = sendCommand_(sock_config_, kRobotConfigDownloadMap, payload, &response); + if (!result.ok()) return result; + content = jsonGet(response, "map_content", jsonGet(response, "content", "")).asString(); + return resultFromResponse_(response); +} + +AgvResult Src1100Agv::connectSocket_(int& sock, const int port) +{ + sock = ::socket(AF_INET, SOCK_STREAM, 0); + if (sock < 0) { + last_error_ = "create socket failed: " + systemError(); + return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_); + } + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_port = htons(static_cast(port)); + if (::inet_pton(AF_INET, ip_.c_str(), &address.sin_addr) <= 0) { + closeSocket_(sock); + last_error_ = "invalid SRC1100 ip: " + ip_; + return AgvResult::failure(AgvErrorCode::InvalidArgument, last_error_); + } + + if (::connect(sock, reinterpret_cast(&address), sizeof(address)) < 0) { + closeSocket_(sock); + last_error_ = "connect SRC1100 port " + std::to_string(port) + " failed: " + systemError(); + return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_); + } + + timeval timeout{}; + timeout.tv_sec = recv_timeout_ms_ / 1000; + timeout.tv_usec = (recv_timeout_ms_ % 1000) * 1000; + ::setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + return AgvResult::success(); +} + +void Src1100Agv::closeSocket_(int& sock) const +{ + if (sock >= 0) { + ::close(sock); + sock = -1; + } +} + +bool Src1100Agv::connected_() const +{ + return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0; +} + +AgvResult Src1100Agv::sendCommand_( + const int sock, + const std::uint16_t command, + const Json::Value& payload, + Json::Value* response) const +{ + std::lock_guard lock(mutex_); + if (sock < 0) { + return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 socket not connected"); + } + + const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload); + const auto frame = buildFrame_(command, payload_text); + if (::send(sock, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast(frame.size())) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send command failed: " + systemError()); + } + + std::string raw; + std::vector buffer(65536); + while (true) { + const ssize_t count = ::recv(sock, buffer.data(), buffer.size(), 0); + if (count <= 0) { + break; + } + raw.append(buffer.data(), static_cast(count)); + if (raw.find('}') != std::string::npos) { + break; + } + } + + if (raw.empty()) { + return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 receive timeout or empty response"); + } + + const std::string json_text = extractJson_(raw); + if (json_text.empty()) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 response does not contain JSON"); + } + + Json::Value parsed; + std::string error; + if (!parseJson_(json_text, parsed, error)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, error); + } + + if (response) { + *response = std::move(parsed); + } + return AgvResult::success(); +} + +AgvResult Src1100Agv::sendCommandNoResponse_( + const int sock, + const std::uint16_t command, + const Json::Value& payload) const +{ + return sendCommand_(sock, command, payload, nullptr); +} + +std::vector Src1100Agv::buildFrame_( + const std::uint16_t command, + const std::string& payload) +{ + std::vector frame(16 + payload.size(), 0); + frame[0] = 0x5A; + frame[1] = 0x01; + frame[2] = 0x00; + frame[3] = 0x01; + const auto length = static_cast(payload.size()); + frame[4] = static_cast((length >> 24U) & 0xFFU); + frame[5] = static_cast((length >> 16U) & 0xFFU); + frame[6] = static_cast((length >> 8U) & 0xFFU); + frame[7] = static_cast(length & 0xFFU); + frame[8] = static_cast((command >> 8U) & 0xFFU); + frame[9] = static_cast(command & 0xFFU); + std::copy(payload.begin(), payload.end(), frame.begin() + 16); + return frame; +} + +std::string Src1100Agv::toJsonString_(const Json::Value& value) +{ + Json::StreamWriterBuilder builder; + builder["indentation"] = ""; + return Json::writeString(builder, value); +} + +bool Src1100Agv::parseJson_(const std::string& input, Json::Value& output, std::string& error) +{ + Json::CharReaderBuilder builder; + std::unique_ptr reader(builder.newCharReader()); + return reader->parse(input.data(), input.data() + input.size(), &output, &error); +} + +std::string Src1100Agv::extractJson_(const std::string& raw) +{ + const auto begin = raw.find('{'); + const auto end = raw.rfind('}'); + if (begin == std::string::npos || end == std::string::npos || end < begin) { + return {}; + } + return raw.substr(begin, end - begin + 1); +} + +int Src1100Agv::optionalInt_(const AgvAdapterParams& params, const std::string& key, const int fallback) +{ + const auto value = params.getDouble(key); + return value ? static_cast(*value) : fallback; +} + +double Src1100Agv::optionalDouble_(const AgvAdapterParams& params, const std::string& key, const double fallback) +{ + const auto value = params.getDouble(key); + return value ? *value : fallback; +} + +void Src1100Agv::applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options) +{ + if (options.max_speed > 0.0) jsonMember(payload, "max_speed") = options.max_speed; + if (options.max_angular_speed > 0.0) jsonMember(payload, "max_wspeed") = options.max_angular_speed; + if (options.max_acceleration > 0.0) jsonMember(payload, "max_acc") = options.max_acceleration; + if (options.max_angular_acceleration > 0.0) jsonMember(payload, "max_wacc") = options.max_angular_acceleration; + if (options.reach_distance > 0.0) jsonMember(payload, "reach_dist") = options.reach_distance; + if (options.reach_angle > 0.0) jsonMember(payload, "reach_angle") = options.reach_angle; +} + +void Src1100Agv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params) +{ + for (const auto& [key, value] : params.values) { + if (key.rfind("port_", 0) == 0) { + continue; + } + jsonMember(payload, key) = value; + } + jsonMember(payload, "jack_height") = optionalDouble_( + params, + "jack_height", + jsonGet(payload, "jack_height", 0.0).asDouble()); +} + +AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response) +{ + const int ret_code = jsonGet(response, "ret_code", 0).asInt(); + const std::string message = jsonGet(response, "err_msg", "").asString(); + if (ret_code == 0) { + return AgvResult::success(); + } + return AgvResult::failure(AgvErrorCode::CommandFailed, + message.empty() ? "SRC1100 command failed: " + std::to_string(ret_code) : message); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/state_define.h b/cmvr-es/devices/state_define.h index 11b60ad7..c506748a 100644 --- a/cmvr-es/devices/state_define.h +++ b/cmvr-es/devices/state_define.h @@ -8,10 +8,11 @@ #include #include #include -#include -#include +#include #include +#include +#include "common/types/agv/agv_types.h" #include "common/types/geometry_types.h" @@ -34,11 +35,6 @@ namespace cmvr::device{ UNKNOWN }; - // ------------------------------------- AGV ------------------------------------- - typedef struct{ - - } AGVState; - // ------------------------------------- robot ------------------------------------- typedef enum { FORWARD, BACKWARD, diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index a33da009..70f9cd95 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(service grpc/src/grpc_head_service.cpp grpc/src/grpc_dexhand_service.cpp grpc/src/grpc_arm_service.cpp + grpc/src/grpc_agv_service.cpp grpc/src/grpc_hlc_service.cpp ../task/grpc_server_task/src/grpc_server_task.cpp ) diff --git a/cmvr-es/service/grpc/include/grpc_agv_service.h b/cmvr-es/service/grpc/include/grpc_agv_service.h new file mode 100644 index 00000000..8158a710 --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_agv_service.h @@ -0,0 +1,79 @@ +#ifndef CMVR_ES_GRPC_AGV_SERVICE_H +#define CMVR_ES_GRPC_AGV_SERVICE_H + +#include "cmvr/api/agv_service.grpc.pb.h" +#include "devices/agv/abstract_agv.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { + +class gRPCAgvServiceImpl final : public api::AgvService::Service { +public: + gRPCAgvServiceImpl(); + ~gRPCAgvServiceImpl() override = default; + + grpc::Status getRuntimeState(grpc::ServerContext* context, + const api::AgvRuntimeStateCommand_Request* request, + api::AgvRuntimeStateCommand_Feedback* response) override; + grpc::Status getNavigationStatus(grpc::ServerContext* context, + const api::AgvNavigationStatusCommand_Request* request, + api::AgvNavigationStatusCommand_Feedback* response) override; + grpc::Status connect(grpc::ServerContext* context, + const api::AgvConnectCommand_Request* request, + api::AgvConnectCommand_Feedback* response) override; + grpc::Status disconnect(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status emergencyStop(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status clearFault(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status navigateToPose(grpc::ServerContext* context, + const api::AgvNavigateToPoseCommand_Request* request, + api::AgvNavigateToPoseCommand_Feedback* response) override; + grpc::Status navigateToStation(grpc::ServerContext* context, + const api::AgvNavigateToStationCommand_Request* request, + api::AgvNavigateToStationCommand_Feedback* response) override; + grpc::Status followPath(grpc::ServerContext* context, + const api::AgvFollowPathCommand_Request* request, + api::AgvFollowPathCommand_Feedback* response) override; + grpc::Status pauseNavigation(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status resumeNavigation(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status cancelNavigation(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status setVelocity(grpc::ServerContext* context, + const api::AgvSetVelocityCommand_Request* request, + api::AgvSetVelocityCommand_Feedback* response) override; + grpc::Status stopVelocityControl(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status listMaps(grpc::ServerContext* context, + const api::AgvListMapsCommand_Request* request, + api::AgvListMapsCommand_Feedback* response) override; + grpc::Status listStations(grpc::ServerContext* context, + const api::AgvListStationsCommand_Request* request, + api::AgvListStationsCommand_Feedback* response) override; + grpc::Status switchMap(grpc::ServerContext* context, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) override; + grpc::Status uploadMap(grpc::ServerContext* context, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) override; + grpc::Status downloadMap(grpc::ServerContext* context, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) override; + +private: + device::DeviceManager& dmgr_; +}; + +} // namespace cmvr::service + +#endif // CMVR_ES_GRPC_AGV_SERVICE_H diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp new file mode 100644 index 00000000..a8c16945 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -0,0 +1,523 @@ +#include "service/grpc/include/grpc_agv_service.h" + +#include +#include +#include + +#include + +using google::protobuf::util::TimeUtil; + +namespace cmvr::service { + +namespace { + +void fillFeedback(api::CommandHeader_Feedback* feedback, + const bool success, + const std::string& message = {}) +{ + feedback->set_success(success); + feedback->set_error_message(message); + *feedback->mutable_timestamp() = TimeUtil::GetCurrentTime(); +} + +grpc::Status resultToStatus(const device::AgvResult& result) +{ + if (result.ok()) { + return grpc::Status::OK; + } + return grpc::Status(grpc::StatusCode::INTERNAL, result.message); +} + +template +grpc::Status setResponseResult(Response* response, const device::AgvResult& result) +{ + fillFeedback(response->mutable_header(), result.ok(), result.ok() ? "" : result.message); + return resultToStatus(result); +} + +grpc::Status setResponseResult(api::CommandHeader_Feedback* response, const device::AgvResult& result) +{ + fillFeedback(response, result.ok(), result.ok() ? "" : result.message); + return resultToStatus(result); +} + +template +grpc::Status setDeviceNotFound(Response* response, const std::string& device_id) +{ + const std::string message = "AGV device not found: " + device_id; + fillFeedback(response->mutable_header(), false, message); + return grpc::Status(grpc::StatusCode::NOT_FOUND, message); +} + +grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std::string& device_id) +{ + const std::string message = "AGV device not found: " + device_id; + fillFeedback(response, false, message); + return grpc::Status(grpc::StatusCode::NOT_FOUND, message); +} + +device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src) +{ + device::AgvAdapterParams dst; + for (const auto& [key, value] : src.values()) { + dst.values.emplace(key, value); + } + return dst; +} + +device::AgvConnectionOptions toConnectionOptions(const api::AgvConnectCommand_Request& src) +{ + device::AgvConnectionOptions dst; + dst.ip = src.ip(); + dst.port = src.port(); + dst.adapter_params = toAdapterParams(src.adapter_params()); + return dst; +} + +device::AgvMotionOptions toMotionOptions(const msgs::AgvMotionOptions& src) +{ + device::AgvMotionOptions dst; + dst.max_speed = src.max_speed(); + dst.max_angular_speed = src.max_angular_speed(); + dst.max_acceleration = src.max_acceleration(); + dst.max_angular_acceleration = src.max_angular_acceleration(); + dst.reach_distance = src.reach_distance(); + dst.reach_angle = src.reach_angle(); + dst.speed_ratio = src.speed_ratio() > 0.0 ? src.speed_ratio() : 1.0; + dst.asynchronous = src.asynchronous(); + return dst; +} + +device::AgvVelocity toVelocity(const msgs::AgvVelocity& src) +{ + return {src.vx(), src.vy(), src.wz()}; +} + +device::AgvPathSegment toPathSegment(const msgs::AgvPathSegment& src) +{ + device::AgvPathSegment dst; + dst.source_station = src.source_station(); + dst.target_station = src.target_station(); + return dst; +} + +math::Pose2d toPose2d(const msgs::AgvPose2d& src) +{ + return {src.x(), src.y(), src.theta()}; +} + +void fillPose2d(msgs::AgvPose2d* dst, const math::Pose2d& src) +{ + dst->set_x(src.x); + dst->set_y(src.y); + dst->set_theta(src.theta); +} + +void fillVelocity(msgs::AgvVelocity* dst, const device::AgvVelocity& src) +{ + dst->set_vx(src.vx); + dst->set_vy(src.vy); + dst->set_wz(src.wz); +} + +void fillBattery(msgs::AgvBatteryState* dst, const device::AgvBatteryState& src) +{ + dst->set_percentage(src.percentage); + dst->set_voltage(src.voltage); + dst->set_current(src.current); + dst->set_temperature(src.temperature); + dst->set_charging(src.charging); +} + +void fillRuntimeState(msgs::AgvRuntimeState* dst, const device::AgvRuntimeState& src) +{ + dst->set_timestamp(src.timestamp); + dst->set_mode(static_cast(src.mode)); + dst->set_connected(src.connected); + dst->set_localized(src.localized); + dst->set_moving(src.moving); + dst->set_fault(src.fault); + dst->set_emergency_stopped(src.emergency_stopped); + fillPose2d(dst->mutable_pose(), src.pose); + fillVelocity(dst->mutable_velocity(), src.velocity); + fillBattery(dst->mutable_battery(), src.battery); + dst->set_current_map(src.current_map); + dst->set_current_station(src.current_station); + dst->set_last_error(src.last_error); +} + +void fillNavigationStatus(msgs::AgvNavigationStatus* dst, const device::AgvNavigationStatus& src) +{ + dst->set_state(static_cast(src.state)); + dst->set_type(static_cast(src.type)); + dst->set_progress(src.progress); + dst->set_message(src.message); +} + +void fillStation(msgs::AgvStation* dst, const device::AgvStation& src) +{ + dst->set_id(src.id); + dst->set_type(src.type); + fillPose2d(dst->mutable_pose(), src.pose); + dst->set_description(src.description); +} + +} // namespace + +gRPCAgvServiceImpl::gRPCAgvServiceImpl() + : dmgr_(device::DeviceManager::getInstance()) +{ +} + +grpc::Status gRPCAgvServiceImpl::getRuntimeState(grpc::ServerContext*, + const api::AgvRuntimeStateCommand_Request* request, + api::AgvRuntimeStateCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + fillRuntimeState(response->mutable_state(), agv->runtimeState()); + fillFeedback(response->mutable_header(), true); + return grpc::Status::OK; + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::getNavigationStatus(grpc::ServerContext*, + const api::AgvNavigationStatusCommand_Request* request, + api::AgvNavigationStatusCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + fillNavigationStatus(response->mutable_status(), agv->navigationStatus()); + fillFeedback(response->mutable_header(), true); + return grpc::Status::OK; + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::connect(grpc::ServerContext*, + const api::AgvConnectCommand_Request* request, + api::AgvConnectCommand_Feedback* response) +{ + try { + 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->connect(toConnectionOptions(*request))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::disconnect(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->disconnect()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::emergencyStop(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->emergencyStop()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->clearFault()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*, + const api::AgvNavigateToPoseCommand_Request* request, + api::AgvNavigateToPoseCommand_Feedback* response) +{ + try { + 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->navigateToPose( + toPose2d(request->pose()), + toMotionOptions(request->options()), + toAdapterParams(request->adapter_params()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*, + const api::AgvNavigateToStationCommand_Request* request, + api::AgvNavigateToStationCommand_Feedback* response) +{ + try { + 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->navigateToStation( + request->station_id(), + toMotionOptions(request->options()), + toAdapterParams(request->adapter_params()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext*, + const api::AgvFollowPathCommand_Request* request, + api::AgvFollowPathCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + std::vector path; + path.reserve(static_cast(request->path_size())); + for (const auto& segment : request->path()) { + path.push_back(toPathSegment(segment)); + } + return setResponseResult(response, agv->followPath(path)); + } 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) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->pauseNavigation()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->resumeNavigation()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->cancelNavigation()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*, + const api::AgvSetVelocityCommand_Request* request, + api::AgvSetVelocityCommand_Feedback* response) +{ + try { + 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->setVelocity(toVelocity(request->velocity()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->stopVelocityControl()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::listMaps(grpc::ServerContext*, + const api::AgvListMapsCommand_Request* request, + api::AgvListMapsCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + std::vector maps; + const auto result = agv->listMaps(maps); + if (result.ok()) { + for (const auto& map : maps) { + response->add_maps(map); + } + } + return setResponseResult(response, result); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::listStations(grpc::ServerContext*, + const api::AgvListStationsCommand_Request* request, + api::AgvListStationsCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + std::vector stations; + const auto result = agv->listStations(stations); + if (result.ok()) { + for (const auto& station : stations) { + fillStation(response->add_stations(), station); + } + } + return setResponseResult(response, result); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) +{ + try { + 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->switchMap(request->map_name())); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) +{ + try { + 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->uploadMap(request->map_name(), request->content())); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::downloadMap(grpc::ServerContext*, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + std::string content; + const auto result = agv->downloadMap(request->map_name(), content); + if (result.ok()) { + response->set_content(content); + } + return setResponseResult(response, result); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +} // namespace cmvr::service diff --git a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h index b865e8f1..055e2eed 100644 --- a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h +++ b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h @@ -55,6 +55,7 @@ private: std::unique_ptr dexhand_service_; std::unique_ptr biohand_service_; std::unique_ptr arm_service_; + std::unique_ptr agv_service_; std::unique_ptr hlc_service_; }; diff --git a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp index 4bab32b9..dd48753a 100644 --- a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp +++ b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp @@ -8,6 +8,7 @@ #include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "common/base/logging/logger.h" #include "common/config/config_files.h" +#include "service/grpc/include/grpc_agv_service.h" #include "service/grpc/include/grpc_arm_service.h" #include "service/grpc/include/grpc_camera_service.h" #include "service/grpc/include/grpc_dexhand_service.h" @@ -101,6 +102,7 @@ bool GrpcServerTask::start() dexhand_service_ = std::make_unique(); biohand_service_ = std::make_unique(); arm_service_ = std::make_unique(); + agv_service_ = std::make_unique(); hlc_service_ = std::make_unique(); grpc::ServerBuilder builder; @@ -112,6 +114,7 @@ bool GrpcServerTask::start() builder.RegisterService(dexhand_service_.get()); builder.RegisterService(biohand_service_.get()); builder.RegisterService(arm_service_.get()); + builder.RegisterService(agv_service_.get()); builder.RegisterService(hlc_service_.get()); server_ = builder.BuildAndStart(); @@ -247,6 +250,7 @@ void GrpcServerTask::waitLoop() void GrpcServerTask::clearServices() { hlc_service_.reset(); + agv_service_.reset(); arm_service_.reset(); biohand_service_.reset(); dexhand_service_.reset(); diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto new file mode 100644 index 00000000..48a40f16 --- /dev/null +++ b/protos/cmvr/api/agv_command.proto @@ -0,0 +1,114 @@ +syntax = "proto3"; + +package cmvr.api; + +import "cmvr/api/common.proto"; +import "cmvr/msgs/agv.proto"; + +message AgvRuntimeStateCommand { + message Request { + CommandHeader.Request header = 1; + } + message Feedback { + CommandHeader.Feedback header = 1; + cmvr.msgs.AgvRuntimeState state = 2; + } +} + +message AgvNavigationStatusCommand { + message Request { + CommandHeader.Request header = 1; + } + message Feedback { + CommandHeader.Feedback header = 1; + cmvr.msgs.AgvNavigationStatus status = 2; + } +} + +message AgvConnectCommand { + message Request { + CommandHeader.Request header = 1; + string ip = 2; + int32 port = 3; + cmvr.msgs.AgvAdapterParams adapter_params = 4; + } + message Feedback { + CommandHeader.Feedback header = 1; + } +} + +message AgvNavigateToPoseCommand { + message Request { + CommandHeader.Request header = 1; + cmvr.msgs.AgvPose2d pose = 2; + cmvr.msgs.AgvMotionOptions options = 3; + cmvr.msgs.AgvAdapterParams adapter_params = 4; + } + message Feedback { + CommandHeader.Feedback header = 1; + } +} + +message AgvNavigateToStationCommand { + message Request { + CommandHeader.Request header = 1; + string station_id = 2; + cmvr.msgs.AgvMotionOptions options = 3; + cmvr.msgs.AgvAdapterParams adapter_params = 4; + } + message Feedback { + CommandHeader.Feedback header = 1; + } +} + +message AgvFollowPathCommand { + message Request { + CommandHeader.Request header = 1; + repeated cmvr.msgs.AgvPathSegment path = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + } +} + +message AgvSetVelocityCommand { + message Request { + CommandHeader.Request header = 1; + cmvr.msgs.AgvVelocity velocity = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + } +} + +message AgvListMapsCommand { + message Request { + CommandHeader.Request header = 1; + } + message Feedback { + CommandHeader.Feedback header = 1; + repeated string maps = 2; + } +} + +message AgvListStationsCommand { + message Request { + CommandHeader.Request header = 1; + } + message Feedback { + CommandHeader.Feedback header = 1; + repeated cmvr.msgs.AgvStation stations = 2; + } +} + +message AgvMapCommand { + message Request { + CommandHeader.Request header = 1; + string map_name = 2; + string content = 3; + } + message Feedback { + CommandHeader.Feedback header = 1; + string content = 2; + } +} diff --git a/protos/cmvr/api/agv_service.proto b/protos/cmvr/api/agv_service.proto new file mode 100644 index 00000000..55d485de --- /dev/null +++ b/protos/cmvr/api/agv_service.proto @@ -0,0 +1,28 @@ +syntax = "proto3"; + +package cmvr.api; + +import "cmvr/api/common.proto"; +import "cmvr/api/agv_command.proto"; + +service AgvService { + rpc getRuntimeState(AgvRuntimeStateCommand.Request) returns (AgvRuntimeStateCommand.Feedback); + rpc getNavigationStatus(AgvNavigationStatusCommand.Request) returns (AgvNavigationStatusCommand.Feedback); + rpc connect(AgvConnectCommand.Request) returns (AgvConnectCommand.Feedback); + rpc disconnect(CommandHeader.Request) returns (CommandHeader.Feedback); + rpc emergencyStop(CommandHeader.Request) returns (CommandHeader.Feedback); + rpc clearFault(CommandHeader.Request) returns (CommandHeader.Feedback); + rpc navigateToPose(AgvNavigateToPoseCommand.Request) returns (AgvNavigateToPoseCommand.Feedback); + rpc navigateToStation(AgvNavigateToStationCommand.Request) returns (AgvNavigateToStationCommand.Feedback); + rpc followPath(AgvFollowPathCommand.Request) returns (AgvFollowPathCommand.Feedback); + rpc pauseNavigation(CommandHeader.Request) returns (CommandHeader.Feedback); + rpc resumeNavigation(CommandHeader.Request) returns (CommandHeader.Feedback); + rpc cancelNavigation(CommandHeader.Request) returns (CommandHeader.Feedback); + rpc setVelocity(AgvSetVelocityCommand.Request) returns (AgvSetVelocityCommand.Feedback); + rpc stopVelocityControl(CommandHeader.Request) returns (CommandHeader.Feedback); + rpc listMaps(AgvListMapsCommand.Request) returns (AgvListMapsCommand.Feedback); + rpc listStations(AgvListStationsCommand.Request) returns (AgvListStationsCommand.Feedback); + rpc switchMap(AgvMapCommand.Request) returns (AgvMapCommand.Feedback); + rpc uploadMap(AgvMapCommand.Request) returns (AgvMapCommand.Feedback); + rpc downloadMap(AgvMapCommand.Request) returns (AgvMapCommand.Feedback); +} diff --git a/protos/cmvr/config/agv_config/agv_config.proto b/protos/cmvr/config/agv_config/agv_config.proto index 004e2155..fdea6bbd 100644 --- a/protos/cmvr/config/agv_config/agv_config.proto +++ b/protos/cmvr/config/agv_config/agv_config.proto @@ -7,11 +7,26 @@ message MyAgvConfig { int32 port = 3; } +message Src1100AgvConfig { + string id = 1; + string ip = 2; + bool enable = 3; + int32 port_status = 4; + int32 port_control = 5; + int32 port_nav = 6; + int32 port_config = 7; + int32 port_other = 8; + int32 port_push = 9; + int32 connect_timeout_ms = 10; + int32 recv_timeout_ms = 11; +} + message AGVDeviceConfig { string id = 1; oneof backend { MyAgvConfig my_agv = 10; + Src1100AgvConfig src1100_agv = 11; } } diff --git a/protos/cmvr/msgs/agv.proto b/protos/cmvr/msgs/agv.proto new file mode 100644 index 00000000..cc5c1dde --- /dev/null +++ b/protos/cmvr/msgs/agv.proto @@ -0,0 +1,73 @@ +syntax = "proto3"; + +package cmvr.msgs; + +message AgvPose2d { + double x = 1; + double y = 2; + double theta = 3; +} + +message AgvVelocity { + double vx = 1; + double vy = 2; + double wz = 3; +} + +message AgvBatteryState { + double percentage = 1; + double voltage = 2; + double current = 3; + double temperature = 4; + bool charging = 5; +} + +message AgvMotionOptions { + double max_speed = 1; + double max_angular_speed = 2; + double max_acceleration = 3; + double max_angular_acceleration = 4; + double reach_distance = 5; + double reach_angle = 6; + double speed_ratio = 7; + bool asynchronous = 8; +} + +message AgvAdapterParams { + map values = 1; +} + +message AgvRuntimeState { + double timestamp = 1; + int32 mode = 2; + bool connected = 3; + bool localized = 4; + bool moving = 5; + bool fault = 6; + bool emergency_stopped = 7; + AgvPose2d pose = 8; + AgvVelocity velocity = 9; + AgvBatteryState battery = 10; + string current_map = 11; + string current_station = 12; + string last_error = 13; +} + +message AgvStation { + string id = 1; + string type = 2; + AgvPose2d pose = 3; + string description = 4; +} + +message AgvPathSegment { + string source_station = 1; + string target_station = 2; +} + +message AgvNavigationStatus { + int32 state = 1; + int32 type = 2; + double progress = 3; + string message = 4; +}