实现仙工控制器SRC1100的集成，未测试

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
+set_target_properties(gRPC::grpc_cpp_plugin
-        PROPERTY IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/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

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 <optional>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#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<std::string, std::string> values;
+
+    bool empty() const { return values.empty(); }
+
+    std::optional<std::string> getString(const std::string& key) const
+    {
+        const auto it = values.find(key);
+        if (it == values.end()) {
+            return std::nullopt;
+        }
+        return it->second;
+    }
+
+    std::optional<double> 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<bool> 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

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

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
+    }
+  }
 }

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
+  }
 }

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
 )
 

cmvr-es/devices/agv/abstract_agv.h

@@ -6,32 +6,203 @@
 #define CMVR_ES_ABSTRACT_AGV_H
 #pragma once
 
+#include <string>
+#include <vector>
+
+#include "common/types/agv/agv_types.h"
 #include "devices/abstract_device.h"
 
 namespace cmvr::device {
+
+/**
+ * @brief AGV/移动底盘设备抽象基类。
+ *
+ * 该接口只描述通用 AGV 能力。控制器特有的请求/响应字段应放在具体
+ * 驱动类中；当公共 API 需要扩展点时，可通过 AgvAdapterParams 传递。
+ */
 class AbstractAGV : public AbstractDevice {
 public:
     AbstractAGV() = default;
     ~AbstractAGV() override = default;
 
     DeviceKind kind() const noexcept override { return DeviceKind::AGV; }
-        virtual bool getState(AGVState &state) { return true; }
 
-        // navigation
+    /**
-        virtual bool eStop() { return true; }
+     * @brief 获取 AGV 运行状态快照。
-        virtual bool goHome() { return true; }
+     */
-        virtual bool moveto(math::Pose2d &location, double speed_ratio) { return true; }
+    virtual AgvRuntimeState runtimeState() const { return {}; }
-        virtual bool setVelocity(math::Vec3 linear, math::Vec3 angular) { return true; }
 
-        // map
+    /**
-        virtual bool initMap(float resolution, int width, int height) { return true; }
+     * @brief 获取当前导航任务状态。
-        virtual bool updateMap() { return true; }
+     */
-        virtual bool saveMap(const std::string& file_path) { return true; }
+    virtual AgvNavigationStatus navigationStatus() const { return {}; }
-        virtual bool loadMap(const std::string& file_path) { return true; }
 
-    protected:
+    /**
-        AGVState state_;
+     * @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<AgvPathSegment>& 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<std::string>& maps) const
+    {
+        (void)maps;
+        return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "listMaps not implemented");
+    }
+    
+    /**
+     * @brief 查询当前活动地图中的站点列表。
+     */
+    virtual AgvResult listStations(std::vector<AgvStation>& 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

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<MyAgv>(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<Src1100Agv>(backend);
+        }
 
         case config::AGVDeviceConfig::BACKEND_NOT_SET:
         default:

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;
+    AgvRuntimeState runtimeState() const override;
-    bool eStop() override;
+    AgvResult emergencyStop() override;
-    bool goHome() override;
+    AgvResult navigateToPose(
-    bool moveto(math::Pose2d& location, double speed_ratio) override;
+        const math::Pose2d& pose,
-    bool setVelocity(math::Vec3 linear, math::Vec3 angular) override;
+        const AgvMotionOptions& options = {},
-    bool initMap(float resolution, int width, int height) override;
+        const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
-    bool updateMap() override;
+    AgvResult setVelocity(const AgvVelocity& velocity) override;
-    bool saveMap(const std::string& file_path) override;
-    bool loadMap(const std::string& file_path) override;
 
 private:
     config::MyAgvConfig config_;

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;
+    return AgvResult::success();
-}
-
-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;
 }
 
 } // namespace cmvr::device

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)

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 <mutex>
+#include <string>
+#include <vector>
+
+#include <json/json.h>
+
+#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<AgvPathSegment>& path) override;
+    AgvResult pauseNavigation() override;
+    AgvResult resumeNavigation() override;
+    AgvResult cancelNavigation() override;
+
+    AgvResult setVelocity(const AgvVelocity& velocity) override;
+
+    AgvResult listMaps(std::vector<std::string>& maps) const override;
+    AgvResult listStations(std::vector<AgvStation>& 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<std::uint8_t> 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

cmvr-es/devices/agv/src1100/src/src1100_agv.cpp

@@ -0,0 +1,619 @@
+#include "devices/agv/src1100/include/src1100_agv.h"
+
+#include <algorithm>
+#include <arpa/inet.h>
+#include <cerrno>
+#include <cstdint>
+#include <cstring>
+#include <memory>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <unistd.h>
+
+#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<int>(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<std::mutex> 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<std::mutex> 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<AgvPathSegment>& 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<std::string>& 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<AgvStation>& 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<std::uint16_t>(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<sockaddr*>(&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<std::mutex> 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<ssize_t>(frame.size())) {
+        return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send command failed: " + systemError());
+    }
+
+    std::string raw;
+    std::vector<char> 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<std::size_t>(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<std::uint8_t> Src1100Agv::buildFrame_(
+    const std::uint16_t command,
+    const std::string& payload)
+{
+    std::vector<std::uint8_t> frame(16 + payload.size(), 0);
+    frame[0] = 0x5A;
+    frame[1] = 0x01;
+    frame[2] = 0x00;
+    frame[3] = 0x01;
+    const auto length = static_cast<std::uint32_t>(payload.size());
+    frame[4] = static_cast<std::uint8_t>((length >> 24U) & 0xFFU);
+    frame[5] = static_cast<std::uint8_t>((length >> 16U) & 0xFFU);
+    frame[6] = static_cast<std::uint8_t>((length >> 8U) & 0xFFU);
+    frame[7] = static_cast<std::uint8_t>(length & 0xFFU);
+    frame[8] = static_cast<std::uint8_t>((command >> 8U) & 0xFFU);
+    frame[9] = static_cast<std::uint8_t>(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<Json::CharReader> 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<int>(*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

cmvr-es/devices/state_define.h

@@ -8,10 +8,11 @@
 #include <atomic>
 #include <cmath>
 #include <iostream>
-#include <vector>
+#include <string>
-#include <unordered_map>
 #include <set>
+#include <vector>
 
+#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,

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
 )

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

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

@@ -0,0 +1,523 @@
+#include "service/grpc/include/grpc_agv_service.h"
+
+#include <exception>
+#include <string>
+#include <vector>
+
+#include <google/protobuf/util/time_util.h>
+
+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 <typename Response>
+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 <typename Response>
+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<int>(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<int>(src.state));
+    dst->set_type(static_cast<int>(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::AbstractAGV>(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::AbstractAGV>(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::AbstractAGV>(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<device::AbstractAGV>(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<device::AbstractAGV>(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<device::AbstractAGV>(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::AbstractAGV>(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::AbstractAGV>(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::AbstractAGV>(device_id);
+        if (!agv) {
+            return setDeviceNotFound(response, device_id);
+        }
+        std::vector<device::AgvPathSegment> path;
+        path.reserve(static_cast<std::size_t>(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<device::AbstractAGV>(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<device::AbstractAGV>(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<device::AbstractAGV>(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::AbstractAGV>(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<device::AbstractAGV>(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::AbstractAGV>(device_id);
+        if (!agv) {
+            return setDeviceNotFound(response, device_id);
+        }
+        std::vector<std::string> 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::AbstractAGV>(device_id);
+        if (!agv) {
+            return setDeviceNotFound(response, device_id);
+        }
+        std::vector<device::AgvStation> 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::AbstractAGV>(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::AbstractAGV>(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::AbstractAGV>(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

cmvr-es/task/grpc_server_task/include/grpc_server_task.h

@@ -55,6 +55,7 @@ private:
     std::unique_ptr<grpc::Service> dexhand_service_;
     std::unique_ptr<grpc::Service> biohand_service_;
     std::unique_ptr<grpc::Service> arm_service_;
+    std::unique_ptr<grpc::Service> agv_service_;
     std::unique_ptr<grpc::Service> hlc_service_;
 };
 

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<service::gRPCDexHandServiceImpl>();
     biohand_service_ = std::make_unique<service::gRPCMBioHeadServiceImpl>();
     arm_service_ = std::make_unique<service::gRPCArmServiceImpl>();
+    agv_service_ = std::make_unique<service::gRPCAgvServiceImpl>();
     hlc_service_ = std::make_unique<service::gRPCHlcServiceImpl>();
 
     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();

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;
+  }
+}

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

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

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<string, string> 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;
+}