diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e100d3a..77cc732c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -122,6 +122,8 @@ target_link_libraries(cmvr_es PRIVATE cmvr_es::device::humanoid_robot cmvr_es::common cmvr_es::applications + cmvr_es::device::agv_src1100 + ) install(TARGETS cmvr_es RUNTIME DESTINATION bin) diff --git a/cmvr-es/common/config/agv_config/agv_config.pb.txt b/cmvr-es/common/config/agv_config/agv_config.pb.txt new file mode 100644 index 00000000..f62d19f2 --- /dev/null +++ b/cmvr-es/common/config/agv_config/agv_config.pb.txt @@ -0,0 +1,30 @@ +src1100_agvs { + id: "agv_src1100" + ip: "192.168.192.5" + port_status: 19301 + port_control: 19301 + port_nav: 19301 + port_config: 19301 + port_other: 19301 + port_push: 19301 + + + + enable: true +} + + +src1100_agvs { + id: "agv_src2200" + ip: "192.168.192.5" + port_status: 19204 + port_control: 19205 + port_nav: 19206 + port_config: 19207 + port_other: 19210 + port_push: 19301 + + + + enable: true +} diff --git a/cmvr-es/common/utils/config_helper/include/config_helper.h b/cmvr-es/common/utils/config_helper/include/config_helper.h index 445af379..e7ae3404 100644 --- a/cmvr-es/common/utils/config_helper/include/config_helper.h +++ b/cmvr-es/common/utils/config_helper/include/config_helper.h @@ -10,6 +10,7 @@ #include "cmvr/config/microphone_config/microphone_config.pb.h" #include "cmvr/config/speaker_config/speaker_conifg.pb.h" #include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h" +#include "cmvr/config/agv_config/agv_config.grpc.pb.h" #include "cmvr/config/biohead_config/biohead_config.pb.h" #include "cmvr/config/robot_config/robot_config.pb.h" #include "cmvr/config/server_config/server_config.pb.h" @@ -85,6 +86,12 @@ namespace cmvr return SET_CONFIG(config, touch_screen_app_config_file); } + static bool getAgvConfig(config::AGVConfig& config) + { + return GET_CONFIG(agv_config_file, config); + } + + static bool getHeadConfig(config::HeadConfig& config) { return GET_CONFIG(biohead_config_file, config); diff --git a/cmvr-es/common/utils/config_helper/include/config_setting.h b/cmvr-es/common/utils/config_helper/include/config_setting.h index caedf533..82c22b5f 100644 --- a/cmvr-es/common/utils/config_helper/include/config_setting.h +++ b/cmvr-es/common/utils/config_helper/include/config_setting.h @@ -6,6 +6,8 @@ DECLARE_string(dexhand_config_file); DECLARE_string(microphone_config_file); DECLARE_string(speaker_config_file); DECLARE_string(touch_screen_app_config_file); +DECLARE_string(agv_config_file); + DECLARE_string(biohead_config_file); DECLARE_string(robot_config_file); DECLARE_string(server_config_file); diff --git a/cmvr-es/common/utils/config_helper/src/config_setting.cpp b/cmvr-es/common/utils/config_helper/src/config_setting.cpp index 528819f5..041b4885 100644 --- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp +++ b/cmvr-es/common/utils/config_helper/src/config_setting.cpp @@ -89,6 +89,10 @@ DEFINE_string(touch_screen_app_config_file, basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt", "The configuration file for TouchScreenApp"); +DEFINE_string(agv_config_file, + basePath() + "agv_config/agv_config.pb.txt", + "AGV config file"); + DEFINE_string(biohead_config_file, basePath() + "biohead_config/biohead_config.pb.txt", "The configuration file for BioHead"); diff --git a/cmvr-es/device_manager/CMakeLists.txt b/cmvr-es/device_manager/CMakeLists.txt index 0b97b456..d74d86c3 100644 --- a/cmvr-es/device_manager/CMakeLists.txt +++ b/cmvr-es/device_manager/CMakeLists.txt @@ -15,6 +15,7 @@ target_link_libraries(device_manager PRIVATE cmvr_es::device::rh56dftp_dexhand cmvr_es::device::px_6ax_gen3 cmvr::device::head_esp32 + cmvr_es::device::agv_src1100 osqp cmvr_es::device::humanoid_robot # cmvr_es::device::aubo_robot diff --git a/cmvr-es/device_manager/include/device_factory.h b/cmvr-es/device_manager/include/device_factory.h index 1fcce739..b46e3495 100644 --- a/cmvr-es/device_manager/include/device_factory.h +++ b/cmvr-es/device_manager/include/device_factory.h @@ -28,7 +28,7 @@ namespace cmvr::device { template std::shared_ptr createFromConfig(const ConfigType& cfg); private: - std::shared_ptr create_agv_(const XmlNode& cfg); + std::shared_ptr create_agv_(const XmlNode& cfg); std::shared_ptr create_battery_(const XmlNode& cfg); std::shared_ptr create_camera_(const XmlNode& cfg); std::shared_ptr create_dexhand_(const XmlNode& cfg); diff --git a/cmvr-es/device_manager/include/device_manager.h b/cmvr-es/device_manager/include/device_manager.h index ee782e5d..8f54823b 100644 --- a/cmvr-es/device_manager/include/device_manager.h +++ b/cmvr-es/device_manager/include/device_manager.h @@ -16,7 +16,7 @@ namespace cmvr::device { using DeviceVariant = std::variant< - std::shared_ptr, + std::shared_ptr, std::shared_ptr, std::shared_ptr, std::shared_ptr, diff --git a/cmvr-es/device_manager/src/device_factory.cpp b/cmvr-es/device_manager/src/device_factory.cpp index 2c136ac5..84262f35 100644 --- a/cmvr-es/device_manager/src/device_factory.cpp +++ b/cmvr-es/device_manager/src/device_factory.cpp @@ -15,12 +15,13 @@ //#include "robot/ti5_robot/ti5_robot.h" #include "cmvr/api/system_command.pb.h" #include "data_center/include/motors_info.h" +#include "devices/agv/agv_src1100/include/agv_src1100.h" //#include "devices/robot/aubo_robot/include/aubo_robot.h" using namespace std; using namespace cmvr::device; -template std::shared_ptr DeviceFactory::create(const XmlNode&); +template std::shared_ptr DeviceFactory::create(const XmlNode&); template std::shared_ptr DeviceFactory::create(const XmlNode&); template std::shared_ptr DeviceFactory::create(const XmlNode&); template std::shared_ptr DeviceFactory::create(const XmlNode&); @@ -32,7 +33,7 @@ template std::shared_ptr DeviceFactory::create template std::shared_ptr DeviceFactory::create(const XmlNode& cfg) { - if constexpr (std::is_same_v) { + if constexpr (std::is_same_v) { return create_agv_(cfg); } else if constexpr (std::is_same_v) { return create_battery_(cfg); @@ -57,7 +58,7 @@ std::shared_ptr DeviceFactory::create(const XmlNode& cfg) { } } -std::shared_ptr DeviceFactory::create_agv_(const XmlNode& cfg) { +std::shared_ptr DeviceFactory::create_agv_(const XmlNode& cfg) { return nullptr; } @@ -184,6 +185,7 @@ template std::shared_ptr DeviceFactory::createFromConfig DeviceFactory::createFromConfig(const config::PX6AXGen3& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::FFMpegMicroPhoneConfig& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::FFMpegSpeakerConfig& cfg); +template std::shared_ptr DeviceFactory::createFromConfig(const config::AGVsrc1100Config& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::BioHeadConfig& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::HumanRobotConfig& cfg); template std::shared_ptr DeviceFactory::createFromConfig(const config::EthercatRobotConfig& cfg); @@ -280,6 +282,18 @@ std::shared_ptr DeviceFactory::createFromConfig(const ConfigType& cf throw runtime_error("[DeviceFactory]: Unsupported microphone device type"); } } + else if constexpr (std::is_same_v) + { + if constexpr (std::is_same_v) + { + return std::make_shared(cfg); + } + else + { + LOG(ERROR) << "[DeviceFactory]: Unsupported agv device type "; + throw runtime_error("[DeviceFactory]: Unsupported agv device type"); + } + } else { LOG(ERROR) << "[DeviceFactory]: Unsupported device type"; throw runtime_error("[DeviceFactory]: Unsupported device type"); @@ -289,4 +303,3 @@ std::shared_ptr DeviceFactory::createFromConfig(const ConfigType& cf return nullptr; } } - diff --git a/cmvr-es/device_manager/src/device_manager.cpp b/cmvr-es/device_manager/src/device_manager.cpp index 67c06de0..581a0aa6 100644 --- a/cmvr-es/device_manager/src/device_manager.cpp +++ b/cmvr-es/device_manager/src/device_manager.cpp @@ -9,7 +9,7 @@ using namespace std; using namespace cmvr::device; using namespace cmvr::device; -template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); +template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); @@ -152,7 +152,7 @@ void DeviceManager::getDeviceList(std::list> string dev_type; std::visit([&](const auto& ptr) { using T = std::decay_t; - if constexpr (std::is_same_v>) { + if constexpr (std::is_same_v>) { dev_type = "AGV"; } else if constexpr (std::is_same_v>) { dev_type = "Battery"; @@ -223,12 +223,12 @@ void DeviceManager::init_devices_() { LOG(ERROR) << "[DeviceManager]: Duplicate AGV Device ID" << id; throw runtime_error("[DeviceManager]: Duplicate AGV Device ID" + id); } - auto device = dev_factory_->create(node); + auto device = dev_factory_->create(node); if (device == nullptr) { throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id); } devices_[id] = device; - std::get>(devices_[id])->init(); + std::get>(devices_[id])->init(); LOG(INFO) << "[DeviceManager]: Init AGV " << id << " Success"; } @@ -312,24 +312,6 @@ void DeviceManager::init_devices_() { LOG(INFO) << "[DeviceManager]: Init Robot " << id << " Success"; } - // EYOU的先用新版的 - config::RobotConfig robot_config; - ConfigHelper::getRobotConfig(robot_config); - for (int i = 0; i < robot_config.ethercat_robots().size(); i++) { - auto robot = robot_config.ethercat_robots(i); - if (!robot.enable()) - continue; - auto id = robot.id(); - if (devices_.count(id)) { - LOG(ERROR) << "[DeviceManager]: Duplicate Robot Device ID" << id; - throw runtime_error("[DeviceManager]: Duplicate Robot Device ID" + id); - } - std::cout << "DexHand[" << i << "]: " << id << std::endl; - auto device = dev_factory_->createFromConfig(robot); - devices_[id] = device; - std::get>(devices_[id])->init(); - } - config::MicroPhoneConfig micro_phone_config; ConfigHelper::getMicroPhonesConfig(micro_phone_config); for (int i = 0; i < micro_phone_config.ffmpeg_microphones().size(); i++) { @@ -380,6 +362,23 @@ void DeviceManager::init_devices_() { devices_[id] = device; std::get>(devices_[id])->init(); } + config::AGVConfig agv_config; + ConfigHelper::getAgvConfig(agv_config); + + for (int i = 0; i < agv_config.src1100_agvs().size(); i++) { + auto agv = agv_config.src1100_agvs(i); + if (!agv.enable()) + continue; + auto id = agv.id(); + if (devices_.count(id)) { + LOG(ERROR) << "[DeviceManager]: Duplicate agv Device ID" << id; + throw runtime_error("[DeviceManager]: Duplicate agv Device ID" + id); + } + std::cout << "AgvSrc1100[" << i << "]: " << id << std::endl; + auto device = dev_factory_->createFromConfig(agv); + devices_[id] = device; + std::get>(devices_[id])->init(); + } } catch (const exception& e) { LOG(ERROR) << e.what(); diff --git a/cmvr-es/devices/CMakeLists.txt b/cmvr-es/devices/CMakeLists.txt index 41c64db3..21191941 100644 --- a/cmvr-es/devices/CMakeLists.txt +++ b/cmvr-es/devices/CMakeLists.txt @@ -6,3 +6,4 @@ add_subdirectory(biohead) add_subdirectory(robot) add_subdirectory(canbus) add_subdirectory(motor) +add_subdirectory(agv) diff --git a/cmvr-es/devices/agv/CMakeLists.txt b/cmvr-es/devices/agv/CMakeLists.txt new file mode 100644 index 00000000..0d8b4bb9 --- /dev/null +++ b/cmvr-es/devices/agv/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(agv_src1100) + diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index a838fa89..da3c1ec9 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -1,36 +1,438 @@ // -// Created by xtkuang on 2025/5/8. +// Created by xtkuang on 2025/5/6. // - #ifndef CMVR_ES_ABSTRACT_AGV_H #define CMVR_ES_ABSTRACT_AGV_H #pragma once -#include "../abstract_device.h" +#include +#include +#include +#include +#include +#include +#include +#include -namespace cmvr::device{ - class AbstractAGV: public AbstractDevice { - public: - explicit AbstractAGV(const XmlNode &config): AbstractDevice(config) {}; - ~AbstractAGV() override=default; +#include "devices/abstract_device.h" - virtual void getState(AGVState &state) {} +// 注意：这里只include，不使用 +#include "cmvr/config/agv_config/agv_config.pb.h" - // navigation - virtual void eStop() {} - virtual void goHome() {} - virtual void moveto(math::Pose2d &location, double speed_ratio) {} - virtual void setVelocity(math::Vec3 linear, math::Vec3 angular) {} +namespace cmvr::device { - // map - virtual void initMap(float resolution, int width, int height) {} - virtual void updateMap() {} - virtual void saveMap(const std::string& file_path) {} - virtual void loadMap(const std::string& file_path) {} - - protected: - AGVState state_; +class AbstractAgv : public AbstractDevice { +public: + enum class Status { + CREATED, + INITIALIZED, + RUNNING, + PAUSED, + STOPPED, + FAULT }; -} + //查询机器人信息 + struct AgvStatusInfo { + std::string id; + std::string vehicle_id; + std::string robot_note; + std::string version; + std::string model; + std::string dsp_version; + std::string gyro_version; + std::string map_version; + std::string model_version; + std::string netprotocol_version; + std::string modbus_version; + std::string current_map; + std::string current_map_md5; + std::string model_md5; + std::string ssid; + int rssi = 0; + std::string ap_addr; + std::string current_ip; + std::string mac; + std::string echoid_type; + std::string echoid; + int ret_code = 0; + std::string err_msg; -#endif //CMVR_ES_ABSTRACT_AGV_H \ No newline at end of file + bool isValid() const { + return !id.empty() && ret_code == 0; + } + }; + + + // 电池状态信息 + struct BatteryStatus + { + double battery_level = 0.0; + double battery_temp = 0.0; + bool charging = false; + double voltage = 0.0; + double current = 0.0; + double max_charge_voltage = -1.0; + double max_charge_current = -1.0; + bool manual_charge = false; + bool auto_charge = false; + int32_t battery_cycle = 0; + std::string battery_user_data; + std::string extra; + int32_t ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + + // 机器人位置信息 + struct RobotLocation + { + double x = 0.0; + double y = 0.0; + double angle = 0.0; + double confidence = 0.0; + std::string current_station; + std::string last_station; + int32_t loc_method = 0; + int32_t ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + // 地图下载结果 + struct DownloadMapResult + { + std::string map_name; + std::string map_content; + int32_t ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + + + // 单地图文件信息 + struct MapFileInfo + { + std::string name; + std::string modified; + int64_t size = 0; + }; + + // 地图整体状态 + struct MapStatus + { + std::string current_map; + std::string current_map_md5; + std::vector maps; + std::vector map_files_info; + int32_t ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + + // 上传地图结果 + struct UploadMapResult + { + int32_t ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + // 抢占控制权返回结果 + struct LockResult + { + int32_t ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + + // 当前控制权信息 + struct CurrentLockStatus + { + bool locked = false; + std::string ip; + int32_t port = 0; + uint8_t type = 0; + std::string nick_name; + int64_t time_t = 0; + std::string desc; + int32_t ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + + //==================== 新增：运动控制结构体 ==================== + // 开环运动下发参数结构体 + struct MotionCtrlReq + { + double vx = 0.0; + double vy = 0.0; + double w = 0.0; + double steer = 0.0; + double real_steer = 0.0; + int64_t duration = -1; // -1 代表缺省，使用设备默认时长 + }; + + // 运动控制返回结果结构体 + struct MotionCtrlRes + { + int ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + + // 切换地图入参 + struct LoadMapReq + { + std::string map_name; + }; + + // 切换地图返回结果 + struct LoadMapRes + { + int ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + // 查询地图载入状态返回 + struct QueryLoadMapStatusRes + { + int32_t loadmap_status = 0; //0失败 1成功 2载入中 + int ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + + // 单个站点信息 + struct StationItem + { + std::string id; + std::string type; + double x = 0.0; + double y = 0.0; + double r = 0.0; + std::string desc; + std::string executor; + std::string prepoint; + std::string recfile; + bool spin = false; + bool use_down_pgv = false; + }; + + // 查询站点列表返回结果 + struct QueryStationRes + { + std::vector stations; + int ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + + // 单段导航任务 + struct MoveTaskItem + { + std::string task_id; + std::string source_id; + std::string id; + std::string operation; + double jack_height = 0.0; + }; + + // 指定路径导航入参 + struct GoTargetListReq + { + std::vector move_task_list; + }; + + // 指定路径导航返回 + struct GoTargetListRes + { + int ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + + // 导航料箱信息 + struct NavContainerItem + { + std::string container_name; + std::string desc; + std::string goods_id; + bool has_goods = false; + }; + + // 1020 查询当前导航入参 + struct RobotStatusTaskReq + { + bool simple = false; + }; + + // 1020 查询当前导航出参 + struct RobotStatusTaskRes + { + int task_status = 0; + int task_type = 0; + std::string target_id; + std::vector target_point; + std::vector finished_path; + std::vector unfinished_path; + std::string move_status_info; + std::vector containers; + + int ret_code = 0; + std::string create_on; + std::string err_msg; + }; + + + + + + + using AgvState = AgvStatusInfo; + +public: + AbstractAgv() = default; + ~AbstractAgv() override = default; + + AbstractAgv(const AbstractAgv&) = delete; + AbstractAgv& operator=(const AbstractAgv&) = delete; + AbstractAgv(AbstractAgv&&) = default; + AbstractAgv& operator=(AbstractAgv&&) = default; + + // 纯虚接口 + virtual Status state() const = 0; + virtual std::string lastError() const = 0; + + + void start() override = 0; + void stop() override = 0; + void update() override = 0; + + virtual void getState(AgvState& state) { + state = AgvState{}; + getStatusInfo(state); + } + + virtual void getStatusInfo(AgvStatusInfo& info) = 0; + + // 查询电池状态 + virtual void getBatteryStatus(BatteryStatus& info, bool simple = false) = 0; + + // 查询机器人位置 + virtual void getRobotLocation(RobotLocation& info) = 0; + + // 下载地图 + virtual void downloadMap(DownloadMapResult& info, const std::string& map_name) = 0; + + + // 查询地图状态 + virtual void getMapStatus(MapStatus& info) = 0; + + // 上传地图 + virtual void uploadMap(UploadMapResult& info, const std::string& map_json) = 0; + + /** + * @brief 抢占机器人控制权 + * @param info 出参，抢占结果 + * @param nick_name 入参，抢占者名称 + */ + virtual void lockRobotControl(LockResult& info, const std::string& nick_name) = 0; + + + + /** + * @brief 下发开环速度运动指令 命令码2010(0x07DA)，使用控制端口sock_control_ + * @param res 出参 运动指令返回结果 + * @param req 入参 vx/vy/w/steer/real_steer/duration运动参数 + * @note 下发后会强制中断当前自动导航任务；多舵轮设备仅vx/vy/w生效 + */ + virtual void robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) = 0; + + /** + * @brief 切换载入地图 命令码2022(0x07E6)，控制端口sock_control_ + * @param res 出参 执行结果 + * @param req 入参 目标地图名称 + */ + virtual void robotLoadMap(LoadMapRes& res, const LoadMapReq& req) = 0; + + + + /** + * @brief 1020(0x03FC) robot_status_task_req 查询当前导航状态 状态端口 sock_status_ + * @param res 输出导航全量状态 + * @param req simple=true 仅返回task_status + */ + virtual void robotStatusTask(RobotStatusTaskRes& res, const RobotStatusTaskReq& req) = 0; + + + + virtual void move(double vx, double vy, double vw) { + throw std::logic_error("move not implemented"); + } + + virtual void stopMove() { + throw std::logic_error("stopMove not implemented"); + } + + /** + * @brief 查询当前机器人控制权持有者 + * @param info 出参 控制权信息 + */ + virtual void getCurrentLockStatus(CurrentLockStatus& info) = 0; + + /** + * @brief 查询地图载入状态 命令码1022(0x03FE)，状态端口sock_status_ + * @param res 出参 地图加载状态结果 + * @note loadmap_status=2载入中禁止重定位 + */ + virtual void queryLoadMapStatus(QueryLoadMapStatusRes& res) = 0; + + + /** + * @brief 查询当前地图全部站点信息 命令码1301(0x0515)，状态端口sock_status_ + * @param res 出参 站点数组与执行结果 + */ + virtual void queryStationList(QueryStationRes& res) = 0; + + + /** + * @brief 指定路径导航 命令码3066(0x0BFA)，控制端口sock_control_ + * @param res 出参 下发任务结果 + * @param req 入参 多段站点导航序列 + * @note task_id、source_id、id 必填；任务串行追加执行 + */ + virtual void robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) = 0; + + + + + + +protected: + std::string id_; + std::string ip_; + // ======================== + // 多端口配置（对应 proto） + // ======================== + int port_status_{0}; // 19204 状态查询 + int port_control_{0}; // 19205 控制API + int port_nav_{0}; // 19206 导航API + int port_config_{0}; // 19207 配置API + int port_other_{0}; // 19210 其他API + int port_push_{0}; // 19301 推送 + bool enable_{false}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_ABSTRACT_AGV_H \ No newline at end of file diff --git a/cmvr-es/devices/agv/agv_src1100/CMakeLists.txt b/cmvr-es/devices/agv/agv_src1100/CMakeLists.txt new file mode 100644 index 00000000..f18e7907 --- /dev/null +++ b/cmvr-es/devices/agv/agv_src1100/CMakeLists.txt @@ -0,0 +1,20 @@ +add_library(agv_src1100 SHARED src/agv_src1100.cpp) + +target_include_directories(agv_src1100 PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${JSONCPP_INCLUDE_DIRS} +) + +add_library(cmvr_es::device::agv_src1100 ALIAS agv_src1100) + +# 关键修改：PRIVATE -> PUBLIC +target_link_libraries(agv_src1100 PUBLIC + jsoncpp + cmvr_es::common + cmvr_es::proto + glog + pthread +) + +install(TARGETS agv_src1100 LIBRARY DESTINATION lib) \ No newline at end of file diff --git a/cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h b/cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h new file mode 100644 index 00000000..847dcfed --- /dev/null +++ b/cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h @@ -0,0 +1,88 @@ +// +// Created by linbo on 2025/6/20. +// +#ifndef AGV_SRC1100_H +#define AGV_SRC1100_H +#pragma once + +#include "./agv/abstract_agv.h" +#include +#include +#include +#include "nlohmann/json.hpp" +using json = nlohmann::json; +namespace cmvr::device { + + class AgvSrc1100 final : public AbstractAgv { + public: + explicit AgvSrc1100(const XmlNode& cfg); + explicit AgvSrc1100(const config::AGVsrc1100Config& cfg); + ~AgvSrc1100() override; + + Status state() const override; + std::string lastError() const override; + + void start() override; + void stop() override; + void update() override; + + void getStatusInfo(AgvStatusInfo& info) override; + void getBatteryStatus(BatteryStatus& info, bool simple = false) override; + void getRobotLocation(RobotLocation& info) override; + void downloadMap(DownloadMapResult& info, const std::string& map_name) override; + void getMapStatus(MapStatus& info) override; + void uploadMap(UploadMapResult& info, const std::string& map_json) override; + void lockRobotControl(LockResult& info, const std::string& nick_name) override; + void getCurrentLockStatus(CurrentLockStatus& info) override; + + void robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) override; + + void robotLoadMap(LoadMapRes& res, const LoadMapReq& req) override; + + void queryLoadMapStatus(QueryLoadMapStatusRes& res) override; + void queryStationList(QueryStationRes& res) override; + + void robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) override; + + + void robotStatusTask(RobotStatusTaskRes& res, const RobotStatusTaskReq& req) override; + + + + + + + + private: + void asyncConnect(int& sock, int port); + bool sendAndRecv(int sock, const uint8_t* header); + void flushSocket(int sock); + void setSocketTimeout(int sock, int timeout_ms); + + + + // 辅助函数：将JSON字符串解析为Json::Value，同时做错误处理 + bool parseJson(const std::string& json_str, json& root, std::string& err_msg); + + mutable std::mutex mutex_; + std::atomic status_{Status::CREATED}; + std::string last_error_; + std::string current_json_; + + + + // 多socket + int sock_status_ = -1; + int sock_control_ = -1; + int sock_nav_ = -1; + int sock_config_ = -1; + int sock_other_ = -1; + int sock_push_ = -1; + + std::vector connect_threads_; + mutable std::mutex connect_mutex_; + }; + +} // namespace cmvr::device + +#endif // AGV_SRC1100_H \ No newline at end of file diff --git a/cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp b/cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp new file mode 100644 index 00000000..f657eb1a --- /dev/null +++ b/cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp @@ -0,0 +1,1830 @@ +#include "../include/agv_src1100.h" +#include +#include +#include +#include +#include +#include +#include +#include +// #include "json/json.h" +#include "nlohmann/json.hpp" +using json = nlohmann::json; +namespace cmvr::device { + +// ====================================================================== +// 构造函数（多端口版本） +// ====================================================================== + + +AgvSrc1100::AgvSrc1100(const config::AGVsrc1100Config& cfg) : AbstractAgv() { + LOG(INFO) << "[AgvSrc1100 Create] this_addr=" << this + << " cfg_id=" << cfg.id() + << " cfg_ip=" << cfg.ip() + << " cfg_port_config=" << cfg.port_config(); + id_ = cfg.id(); + ip_ = cfg.ip(); + enable_ = cfg.enable(); + + port_status_ = cfg.port_status(); + port_control_ = cfg.port_control(); + port_nav_ = cfg.port_nav(); + port_config_ = cfg.port_config(); + port_other_ = cfg.port_other(); + port_push_ = cfg.port_push(); + + if (enable_) { + connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_status_), port_status_); + connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_control_), port_control_); + connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_nav_), port_nav_); + connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_config_), port_config_); + connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_other_), port_other_); + connect_threads_.emplace_back(&AgvSrc1100::asyncConnect, this, std::ref(sock_push_), port_push_); + } +} + +AgvSrc1100::~AgvSrc1100() { + stop(); + for (auto& t : connect_threads_) { + if (t.joinable()) t.join(); + } +} + +AbstractAgv::Status AgvSrc1100::state() const { + return status_; +} + +std::string AgvSrc1100::lastError() const { + std::lock_guard lock(mutex_); + return last_error_; +} + +// ====================================================================== +// 通用异步连接 +// ====================================================================== +void AgvSrc1100::asyncConnect(int& sock, int port) { + int s = socket(AF_INET, SOCK_STREAM, 0); + if (s < 0) { + std::lock_guard lock(mutex_); + last_error_ = "socket create failed"; + status_ = Status::FAULT; + return; + } + + struct sockaddr_in addr{}; + addr.sin_family = AF_INET; + addr.sin_port = htons(port); + inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr); + + if (connect(s, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + close(s); + std::lock_guard lock(mutex_); + last_error_ = "connect failed port:" + std::to_string(port); + status_ = Status::FAULT; + return; + } + + { + std::lock_guard lock(mutex_); + sock = s; + status_ = Status::INITIALIZED; + } +} + +void AgvSrc1100::start() { + status_ = Status::RUNNING; +} + +void AgvSrc1100::stop() { + auto closeSocket = [](int& s) { + if (s >= 0) { close(s); s = -1; } + }; + closeSocket(sock_status_); + closeSocket(sock_control_); + closeSocket(sock_nav_); + closeSocket(sock_config_); + closeSocket(sock_other_); + closeSocket(sock_push_); + status_ = Status::STOPPED; +} + +void AgvSrc1100::update() {} + +void AgvSrc1100::flushSocket(int sock) { + uint8_t tmp[128]; + ssize_t rlen; + while ((rlen = recv(sock, tmp, sizeof(tmp), MSG_DONTWAIT)) > 0); +} + +void AgvSrc1100::setSocketTimeout(int sock, int timeout_ms) { + struct timeval tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); +} + +// ====================================================================== +// 增强版收发：确保完整接收所有数据帧 +// ====================================================================== + bool AgvSrc1100::sendAndRecv(int sock, const uint8_t* header) { + std::lock_guard lock(mutex_); + + if (sock < 0) { + last_error_ = "socket not connected"; + return false; + } + + flushSocket(sock); + + // 发送16字节请求包头 + if (send(sock, header, 16, MSG_NOSIGNAL) != 16) { + last_error_ = "send header failed"; + return false; + } + + // 整体接收超时 1000ms + setSocketTimeout(sock, 100); + + const int RECV_BUF_LEN = 16384; + uint8_t recvBuf[RECV_BUF_LEN] = {0}; + int totalRecv = 0; + + // 循环接收，直到读空 or 缓冲区满 + while (totalRecv < RECV_BUF_LEN) + { + int ret = recv(sock, recvBuf + totalRecv, RECV_BUF_LEN - totalRecv, 0); + if (ret <= 0) break; // 读空/超时，退出 + + totalRecv += ret; + } + + setSocketTimeout(sock, 0); + + if (totalRecv <= 0) { + last_error_ = "no data received"; + return false; + } + + std::string rawData(reinterpret_cast(recvBuf), totalRecv); + size_t jsonStart = rawData.find('{'); + size_t jsonEnd = rawData.rfind('}'); + + // 兜底：如果没找到结尾 }，手动补全，修复截断报错 + if (jsonStart == std::string::npos) + { + last_error_ = "can not find '{'"; + return false; + } + if (jsonEnd == std::string::npos) + { + // 找不到 }，截取到末尾并补 } + current_json_ = rawData.substr(jsonStart) + "}"; + } + else + { + current_json_ = rawData.substr(jsonStart, jsonEnd - jsonStart + 1); + } + + + fflush(stdout); + + last_error_ = "ok"; + return true; +} + +// ====================================================================== +// 辅助JSON解析函数 +// ====================================================================== + bool AgvSrc1100::parseJson(const std::string& json_str, json& root, std::string& err_msg) +{ + root.clear(); + try + { + root = json::parse(json_str); + } + catch (json::parse_error& e) + { + err_msg = "JSON parse error: " + std::string(e.what()); + return false; + } + return true; +} + +// =========================================================================== +// 接口1：查询设备信息 → 19204 命令码 1000 (0x03E8) +// =========================================================================== + void AgvSrc1100::getStatusInfo(AgvStatusInfo& info) { + int sock = sock_status_; + if (sock < 0) { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, 0x00, 0x00, + 0x03, 0xE8, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00 + }; + + if (!sendAndRecv(sock, send_buf)) { + info.ret_code = -2; + info.err_msg = last_error_; + return; + } + + std::string json_str; + { + std::lock_guard lk(mutex_); + json_str = current_json_; + } + + // 兜底修复JSON闭合 + if (!json_str.empty()) + { + if (json_str.front() != '{') json_str = "{" + json_str; + if (json_str.back() != '}') json_str += "}"; + } + + if (json_str.empty()) { + info.ret_code = -4; + info.err_msg = "empty json data"; + return; + } + + json root; + std::string err; + if (!parseJson(json_str, root, err)) { + info.ret_code = -3; + info.err_msg = err; + return; + } + + info.id = root.value("id", ""); + info.vehicle_id = root.value("vehicle_id", ""); + info.version = root.value("version", ""); + info.model = root.value("model", ""); + info.dsp_version = root.value("dsp_version", ""); + info.current_ip = root.value("current_ip", ""); + info.mac = root.value("MAC", ""); + info.rssi = root.value("rssi", 0); + info.ret_code = root.value("ret_code", 0); + info.err_msg = root.value("err_msg", ""); +} + + +// =========================================================================== +// 接口：查询机器人电池状态 → 命令码 1007 (0x03EF) +// =========================================================================== +void AgvSrc1100::getBatteryStatus(BatteryStatus& info, bool simple) +{ + int sock = sock_status_; + if (sock < 0) + { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 步骤1：构造16字节请求帧 + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, 0x00, 0x00, + 0x03, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00 + }; + + if (!sendAndRecv(sock, send_buf)) + { + info.ret_code = -2; + info.err_msg = last_error_; + return; + } + + // 步骤2：数据拷贝 + JSON 兜底 + 解析 + std::string json_str; + { + std::lock_guard lk(mutex_); + json_str = current_json_; + } + + if (!json_str.empty()) + { + if (json_str.front() != '{') json_str = "{" + json_str; + if (json_str.back() != '}') json_str += "}"; + } + + if (json_str.empty()) + { + info.ret_code = -4; + info.err_msg = "empty json data"; + return; + } + + json root; + std::string err; + if (!parseJson(json_str, root, err)) + { + info.ret_code = -3; + info.err_msg = err; + return; + } + + // 步骤3：字段赋值 + info.battery_level = root.value("battery_level", 0.0); + info.battery_temp = root.value("battery_temp", 0.0); + info.charging = root.value("charging", false); + info.voltage = root.value("voltage", 0.0); + info.current = root.value("current", 0.0); + + info.max_charge_voltage = root.value("max_charge_voltage", -1.0); + info.max_charge_current = root.value("max_charge_current", -1.0); + + info.manual_charge = root.value("manual_charge", false); + info.auto_charge = root.value("auto_charge", false); + info.battery_cycle = root.value("battery_cycle", 0); + + info.battery_user_data = root.value("battery_user_data", ""); + info.extra = root.value("extra", ""); + + info.ret_code = root.value("ret_code", 0); + info.create_on = root.value("create_on", ""); + info.err_msg = root.value("err_msg", ""); +} + + + // =========================================================================== + // 接口：查询机器人位置 → 命令码 1004 (0x03EC) + // =========================================================================== + void AgvSrc1100::getRobotLocation(RobotLocation& info) +{ + int sock = sock_status_; + if (sock < 0) + { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 步骤1：构造16字节请求帧 + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, 0x00, 0x00, + 0x03, 0xEC, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00 + }; + + if (!sendAndRecv(sock, send_buf)) + { + info.ret_code = -2; + info.err_msg = last_error_; + return; + } + + // 步骤2：数据拷贝 + JSON兜底 + 解析 + std::string json_str; + { + std::lock_guard lk(mutex_); + json_str = current_json_; + } + + if (!json_str.empty()) + { + if (json_str.front() != '{') json_str = "{" + json_str; + if (json_str.back() != '}') json_str += "}"; + } + + if (json_str.empty()) + { + info.ret_code = -4; + info.err_msg = "empty json data"; + return; + } + + json root; + std::string err; + if (!parseJson(json_str, root, err)) + { + info.ret_code = -3; + info.err_msg = err; + return; + } + + // 步骤3：字段赋值 + info.x = root.value("x", 0.0); + info.y = root.value("y", 0.0); + info.angle = root.value("angle", 0.0); + info.confidence = root.value("confidence", 0.0); + info.current_station = root.value("current_station", ""); + info.last_station = root.value("last_station", ""); + info.loc_method = root.value("loc_method", 0); + info.ret_code = root.value("ret_code", 0); + info.create_on = root.value("create_on", ""); + info.err_msg = root.value("err_msg", ""); +} + + + // =========================================================================== +// 接口：下载地图 命令码 4011(0x0FAB) +// =========================================================================== +void AgvSrc1100::downloadMap(DownloadMapResult& info, const std::string& map_name) +{ + int sock = sock_config_; + if (sock < 0) + { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 组装请求JSON + std::string req_json = R"({"map_name":")" + map_name + R"("})"; + uint8_t payload_len = static_cast(req_json.size()); + + // 严格对齐协议帧头 + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x00, + 0x00, 0x00, 0x00, payload_len, + 0x0F, 0xAB, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + + flushSocket(sock); + // 发送帧头 + if (send(sock, send_buf, 16, MSG_NOSIGNAL) != 16) + { + info.ret_code = -2; + info.err_msg = "send frame header failed"; + return; + } + // 发送JSON负载 + if (send(sock, req_json.data(), req_json.size(), MSG_NOSIGNAL) != (ssize_t)req_json.size()) + { + info.ret_code = -2; + info.err_msg = "send payload failed"; + return; + } + + // 增大缓冲区 + 循环接收，确保大数据收完整 + const int RECV_BUF_LEN = 65536; + std::vector recvBuf(RECV_BUF_LEN); + std::string rawData; + int ret = 0; + + setSocketTimeout(sock, 500); // 延长超时适配大地图 + while ((ret = recv(sock, recvBuf.data(), RECV_BUF_LEN, 0)) > 0) + { + rawData.append(reinterpret_cast(recvBuf.data()), ret); + } + setSocketTimeout(sock, 0); + + if (rawData.empty()) + { + info.ret_code = -2; + info.err_msg = "no response data"; + return; + } + + // 截取JSON区间 + size_t jsonStart = rawData.find('{'); + size_t jsonEnd = rawData.rfind('}'); + std::string json_str; + + if (jsonStart == std::string::npos) + { + info.ret_code = -4; + info.err_msg = "json not found"; + return; + } + if (jsonEnd == std::string::npos) + { + // 兜底补全闭合符 + json_str = rawData.substr(jsonStart) + "}"; + } + else + { + json_str = rawData.substr(jsonStart, jsonEnd - jsonStart + 1); + } + + // JSON解析 + json root; + std::string err; + if (!parseJson(json_str, root, err)) + { + info.ret_code = -3; + info.err_msg = err; + return; + } + + // 字段赋值 + info.map_name = map_name; + info.ret_code = root.value("ret_code", 0); + info.create_on = root.value("create_on", ""); + info.err_msg = root.value("err_msg", ""); + info.map_content = (0 == info.ret_code) ? json_str : ""; +} + + + + // =========================================================================== +// 接口：查询地图状态 → 命令码 1300 (0x0514) +// =========================================================================== +void AgvSrc1100::getMapStatus(MapStatus& info) +{ + int sock = sock_status_; + if (sock < 0) + { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 步骤1：构造16字节请求帧 + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, 0x00, 0x00, + 0x05, 0x14, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00 + }; + + if (!sendAndRecv(sock, send_buf)) + { + info.ret_code = -2; + info.err_msg = last_error_; + return; + } + + // 步骤2：数据拷贝 + JSON兜底 + 解析 + std::string json_str; + { + std::lock_guard lk(mutex_); + json_str = current_json_; + } + + if (!json_str.empty()) + { + if (json_str.front() != '{') json_str = "{" + json_str; + if (json_str.back() != '}') json_str += "}"; + } + + if (json_str.empty()) + { + info.ret_code = -4; + info.err_msg = "empty json data"; + return; + } + + json root; + std::string err; + if (!parseJson(json_str, root, err)) + { + info.ret_code = -3; + info.err_msg = err; + return; + } + + // 步骤3：字段赋值 + info.current_map = root.value("current_map", ""); + info.current_map_md5 = root.value("current_map_md5", ""); + info.ret_code = root.value("ret_code", 0); + info.create_on = root.value("create_on", ""); + info.err_msg = root.value("err_msg", ""); + + // 解析地图名数组 + if (root.contains("maps") && root["maps"].is_array()) + { + for (auto& item : root["maps"]) + { + info.maps.push_back(item.get()); + } + } + + // 解析地图文件详情数组 + if (root.contains("map_files_info") && root["map_files_info"].is_array()) + { + for (auto& item : root["map_files_info"]) + { + MapFileInfo fileInfo; + fileInfo.name = item.value("name", ""); + fileInfo.modified = item.value("modified", ""); + fileInfo.size = item.value("size", 0LL); + info.map_files_info.push_back(fileInfo); + } + } +} + + +// =========================================================================== +// 接口：上传地图 → 命令码 4010 (0x0FAA) +// =========================================================================== +void AgvSrc1100::uploadMap(UploadMapResult& info, const std::string& map_json) +{ + int sock = sock_config_; + if (sock < 0) + { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 清空socket残留数据 + flushSocket(sock); + // 延长IO超时 1s + setSocketTimeout(sock, 1000); + + uint16_t payload_len = static_cast(map_json.size()); + // 帧头：第7、8字节 = 负载长度(大端)，修正单字节溢出问题 + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x00, + 0x00, 0x00, + static_cast((payload_len >> 8) & 0xFF), // 高字节 + static_cast(payload_len & 0xFF), // 低字节 + 0x0F, 0xAA, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + + // 发送16字节帧头，循环重试 + int totalSent = 0; + int retries = 3; + while (totalSent < 16 && retries-- > 0) + { + int ret = send(sock, send_buf + totalSent, 16 - totalSent, 0); + if (ret <= 0) + { + usleep(10000); + continue; + } + totalSent += ret; + } + if (totalSent != 16) + { + setSocketTimeout(sock, 0); + info.ret_code = -2; + info.err_msg = "send frame header failed"; + return; + } + + // 分片发送大JSON数据，4096字节一片 + const int SLICE_SIZE = 4096; + size_t offset = 0; + size_t totalSize = map_json.size(); + const char* data = map_json.c_str(); + + while (offset < totalSize) + { + int sendLen = std::min((size_t)SLICE_SIZE, totalSize - offset); + int ret = send(sock, data + offset, sendLen, 0); + if (ret <= 0) + { + setSocketTimeout(sock, 0); + info.ret_code = -2; + info.err_msg = "send map payload failed"; + return; + } + offset += ret; + usleep(500); + } + + // 接收响应 + const int RECV_BUF_LEN = 65536; + std::vector recvBuf(RECV_BUF_LEN); + std::string rawData; + int recvRet = 0; + while ((recvRet = recv(sock, recvBuf.data(), RECV_BUF_LEN, 0)) > 0) + { + rawData.append(reinterpret_cast(recvBuf.data()), recvRet); + } + setSocketTimeout(sock, 0); + + if (rawData.empty()) + { + info.ret_code = -2; + info.err_msg = "no response data"; + return; + } + + // 提取JSON + size_t jsonStart = rawData.find('{'); + size_t jsonEnd = rawData.rfind('}'); + std::string json_str; + if (jsonStart == std::string::npos) + { + info.ret_code = -4; + info.err_msg = "json not found"; + return; + } + if (jsonEnd == std::string::npos) + { + json_str = rawData.substr(jsonStart) + "}"; + } + else + { + json_str = rawData.substr(jsonStart, jsonEnd - jsonStart + 1); + } + + // 解析JSON + json root; + std::string err; + if (!parseJson(json_str, root, err)) + { + info.ret_code = -3; + info.err_msg = "json parse error: " + err; + return; + } + + info.ret_code = root.value("ret_code", 0); + info.create_on = root.value("create_on", ""); + info.err_msg = root.value("err_msg", ""); +} + + // =========================================================================== +// 接口：抢占机器人控制权 命令码 4005 (0x0FA5) +// =========================================================================== +/** + * @brief 下发抢占控制权指令到机器人配置端口 + * @param info 输出抢占结果结构体 + * @param nick_name 抢占方标识名称 + */ +void AgvSrc1100::lockRobotControl(LockResult& info, const std::string& nick_name) +{ + // 使用配置端口发送控制指令 + int sock = sock_config_; + if (sock < 0) + { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 组装请求JSON：{"nick_name":"xxx"} + std::string req_json = R"({"nick_name":")" + nick_name + R"("})"; + // 负载长度双字节大端存储，兼容长字符串 + uint16_t payload_len = static_cast(req_json.size()); + + // 16字节协议帧头，严格对齐设备报文格式 + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, + static_cast((payload_len >> 8) & 0xFF), // 长度高字节 + static_cast(payload_len & 0xFF), // 长度低字节 + 0x0F, 0xA5, // 指令编号 0x0FA5=4005 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + + // 清空socket残留脏数据，防止粘包干扰 + flushSocket(sock); + // 设置IO超时1000ms，适配网络延迟 + setSocketTimeout(sock, 1000); + + // 循环发送16字节帧头，最多重试3次 + int total_sent = 0; + int retry = 3; + while (total_sent < 16 && retry > 0) + { + int ret = send(sock, send_buf + total_sent, 16 - total_sent, 0); + if (ret <= 0) + { + retry--; + usleep(10000); + continue; + } + total_sent += ret; + } + // 帧头发送失败直接返回错误 + if (total_sent != 16) + { + setSocketTimeout(sock, 0); + info.ret_code = -2; + info.err_msg = "send frame header failed"; + return; + } + + // 分片发送JSON负载，防止缓冲区溢出 + const int slice_size = 4096; + size_t offset = 0; + size_t total_size = req_json.size(); + const char* data = req_json.c_str(); + while (offset < total_size) + { + int send_len = std::min((size_t)slice_size, total_size - offset); + int ret = send(sock, data + offset, send_len, 0); + if (ret <= 0) + { + setSocketTimeout(sock, 0); + info.ret_code = -2; + info.err_msg = "send payload failed"; + return; + } + offset += ret; + usleep(500); + } + + // 循环读取完整响应数据 + const int recv_buf_len = 65536; + std::vector recv_buf(recv_buf_len); + std::string raw_data; + int recv_ret = 0; + while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, 0)) > 0) + { + raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); + } + setSocketTimeout(sock, 0); + + // 无响应数据报错 + if (raw_data.empty()) + { + info.ret_code = -2; + info.err_msg = "no response data"; + return; + } + + // 截取完整JSON区间 + size_t json_start = raw_data.find('{'); + size_t json_end = raw_data.rfind('}'); + std::string json_str; + if (json_start == std::string::npos) + { + info.ret_code = -4; + info.err_msg = "json not found in response"; + return; + } + if (json_end == std::string::npos) + { + json_str = raw_data.substr(json_start) + "}"; + } + else + { + json_str = raw_data.substr(json_start, json_end - json_start + 1); + } + + // JSON解析 + json root; + std::string err; + if (!parseJson(json_str, root, err)) + { + info.ret_code = -3; + info.err_msg = "json parse error: " + err; + return; + } + + // 赋值返回字段 + info.ret_code = root.value("ret_code", 0); + info.create_on = root.value("create_on", ""); + info.err_msg = root.value("err_msg", ""); +} + + +// =========================================================================== +// 接口：查询当前控制权所有者 命令码 1060 (0x0424) +// =========================================================================== +/** + * @brief 查询机器人当前抢占控制权信息 + * @param info 输出控制权详情结构体 + */ +void AgvSrc1100::getCurrentLockStatus(CurrentLockStatus& info) +{ + int sock = sock_status_; + if (sock < 0) + { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 16字节帧头，无JSON负载，负载长度0 + uint16_t payload_len = 0; + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, + static_cast((payload_len >> 8) & 0xFF), + static_cast(payload_len & 0xFF), + 0x04, 0x24, // 命令码 0x0424 = 1060 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + + flushSocket(sock); + setSocketTimeout(sock, 1000); + + // 循环发送帧头 + int total_sent = 0; + int retry = 3; + while (total_sent < 16 && retry > 0) + { + int ret = send(sock, send_buf + total_sent, 16 - total_sent, 0); + if (ret <= 0) + { + retry--; + usleep(10000); + continue; + } + total_sent += ret; + } + if (total_sent != 16) + { + setSocketTimeout(sock, 0); + info.ret_code = -2; + info.err_msg = "send frame header failed"; + return; + } + + // 接收完整响应 + const int RECV_BUF_LEN = 65536; + std::vector recv_buf(RECV_BUF_LEN); + std::string raw_data; + int recv_ret = 0; + while ((recv_ret = recv(sock, recv_buf.data(), RECV_BUF_LEN, 0)) > 0) + { + raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); + } + setSocketTimeout(sock, 0); + + if (raw_data.empty()) + { + info.ret_code = -2; + info.err_msg = "no response data"; + return; + } + + // 截取JSON + size_t json_start = raw_data.find('{'); + size_t json_end = raw_data.rfind('}'); + std::string json_str; + if (json_start == std::string::npos) + { + info.ret_code = -4; + info.err_msg = "json not found in response"; + return; + } + if (json_end == std::string::npos) + { + json_str = raw_data.substr(json_start) + "}"; + } + else + { + json_str = raw_data.substr(json_start, json_end - json_start + 1); + } + + // JSON解析 + json root; + std::string err; + if (!parseJson(json_str, root, err)) + { + info.ret_code = -3; + info.err_msg = "json parse error: " + err; + return; + } + + // 字段赋值 + info.locked = root.value("locked", false); + info.ip = root.value("ip", ""); + info.port = root.value("port", 0); + info.type = static_cast(root.value("type", 0)); + info.nick_name = root.value("nick_name", ""); + info.time_t = root.value("time_t", 0LL); + info.desc = root.value("desc", ""); + info.ret_code = root.value("ret_code", 0); + info.create_on = root.value("create_on", ""); + info.err_msg = root.value("err_msg", ""); +} + + + // =========================================================================== +// 接口：开环速度运动控制 命令码 2010 (0x07DA) +// 通信端口：sock_control_ 19205控制端口 +// 下发指令强制取消正在执行的导航任务 +// 多舵轮设备仅解析vx/vy/w；steer、real_steer仅单舵轮设备生效 +// =========================================================================== +void AgvSrc1100::robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) +{ + int sock = sock_control_; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "socket not connected"; + return; + } + + json send_json; + if (req.vx != 0.0) send_json["vx"] = req.vx; + if (req.vy != 0.0) send_json["vy"] = req.vy; + if (req.w != 0.0) send_json["w"] = req.w; + if (req.steer != 0.0) send_json["steer"] = req.steer; + if (req.real_steer != 0.0) send_json["real_steer"] = req.real_steer; + if (req.duration >= 0) send_json["duration"] = req.duration; + + std::string payload = send_json.dump(); + uint16_t payload_len = static_cast(payload.size()); + + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, + static_cast((payload_len >> 8) & 0xFF), + static_cast(payload_len & 0xFF), + 0x07, 0xDA, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + + flushSocket(sock); + setSocketTimeout(sock, 1000); + + int total_sent = 0; + int retry_cnt = 3; + while (total_sent < 16 && retry_cnt > 0) + { + int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); + if (ret <= 0) + { + retry_cnt--; + usleep(10000); + continue; + } + total_sent += ret; + } + if (total_sent != 16) + { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send frame header failed"; + return; + } + + const int slice_size = 4096; + size_t offset = 0; + size_t total_size = payload.size(); + const char* data_ptr = payload.data(); + while (offset < total_size) + { + int send_len = std::min((size_t)slice_size, total_size - offset); + int ret = send(sock, data_ptr + offset, send_len, MSG_NOSIGNAL); + if (ret <= 0) + { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send payload failed"; + return; + } + offset += ret; + usleep(500); + } + + const int recv_buf_len = 65536; + std::vector recv_buf(recv_buf_len); + std::string raw_data; + int recv_ret = 0; + while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, 0)) > 0) + { + raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); + } + setSocketTimeout(sock, 0); + + if (raw_data.empty()) + { + res.ret_code = -2; + res.err_msg = "no response data"; + return; + } + + size_t json_start = raw_data.find('{'); + size_t json_end = raw_data.rfind('}'); + std::string json_str; + if (json_start == std::string::npos) + { + res.ret_code = -4; + res.err_msg = "json not found in response"; + return; + } + if (json_end == std::string::npos) + { + json_str = raw_data.substr(json_start) + "}"; + } + else + { + json_str = raw_data.substr(json_start, json_end - json_start + 1); + } + + json root; + std::string parse_err; + if (!parseJson(json_str, root, parse_err)) + { + res.ret_code = -3; + res.err_msg = "json parse error: " + parse_err; + return; + } + + res.ret_code = root.value("ret_code", 0); + res.create_on = root.value("create_on", ""); + res.err_msg = root.value("err_msg", ""); +} + + + // =========================================================================== +// 接口：切换载入地图 命令码 2022 (0x07E6) +// 通信端口：sock_control_ 19205控制端口 +// 功能：下发地图名切换设备当前地图，合法名称更新current_map +// =========================================================================== +void AgvSrc1100::robotLoadMap(LoadMapRes& res, const LoadMapReq& req) +{ + + int sock = sock_control_; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "socket not connected"; + return; + } + + // 组装请求JSON {"map_name":"xxx"} + json send_json; + send_json["map_name"] = req.map_name; + std::string payload = send_json.dump(); + uint16_t payload_len = static_cast(payload.size()); + + // 16字节协议帧头 0x5A 01 00 01 + 长度大端 + 0x07E6 + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, + static_cast((payload_len >> 8) & 0xFF), + static_cast(payload_len & 0xFF), + 0x07, 0xE6, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + + flushSocket(sock); + setSocketTimeout(sock, 1000); + + // 重试发送帧头3次 + int total_sent = 0; + int retry_cnt = 3; + while (total_sent < 16 && retry_cnt > 0) + { + int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); + if (ret <= 0) + { + retry_cnt--; + usleep(10000); + continue; + } + total_sent += ret; + } + if (total_sent != 16) + { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send frame header failed"; + return; + } + + // 分片发送JSON负载 + const int slice_size = 4096; + size_t offset = 0; + size_t total_size = payload.size(); + const char* data_ptr = payload.data(); + while (offset < total_size) + { + int send_len = std::min((size_t)slice_size, total_size - offset); + int ret = send(sock, data_ptr + offset, send_len, MSG_NOSIGNAL); + if (ret <= 0) + { + setSocketTimeout(sock, 3000); + res.ret_code = -2; + res.err_msg = "send payload failed"; + return; + } + offset += ret; + usleep(500); + } + + // 接收响应报文 + const int recv_buf_len = 65536; + std::vector recv_buf(recv_buf_len); + std::string raw_data; + int recv_ret = 0; + while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, 0)) > 0) + { + raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); + } + setSocketTimeout(sock, 0); + + if (raw_data.empty()) + { + res.ret_code = -2; + res.err_msg = "no response data"; + return; + } + + // 截取JSON，兜底补全闭合符 + size_t json_start = raw_data.find('{'); + size_t json_end = raw_data.rfind('}'); + std::string json_str; + if (json_start == std::string::npos) + { + res.ret_code = -4; + res.err_msg = "json not found in response"; + return; + } + if (json_end == std::string::npos) + { + json_str = raw_data.substr(json_start) + "}"; + } + else + { + json_str = raw_data.substr(json_start, json_end - json_start + 1); + } + + // 解析JSON + json root; + std::string parse_err; + if (!parseJson(json_str, root, parse_err)) + { + res.ret_code = -3; + res.err_msg = "json parse error: " + parse_err; + return; + } + + res.ret_code = root.value("ret_code", 0); + res.create_on = root.value("create_on", ""); + res.err_msg = root.value("err_msg", ""); + LOG(INFO) << "recv raw_data len=" << raw_data.size() << " content=" << raw_data; + +} + + // =========================================================================== +// 接口：查询地图载入状态 命令码 1022 (0x03FE) +// 通信端口：sock_status_ 19204状态端口 +// loadmap_status 0=失败 1=成功 2=载入中；载入中禁止重定位 +// =========================================================================== +void AgvSrc1100::queryLoadMapStatus(QueryLoadMapStatusRes& res) +{ + int sock = sock_status_; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "socket not connected"; + return; + } + + // 无负载，负载长度0 + uint16_t payload_len = 0; + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, + static_cast((payload_len >> 8) & 0xFF), + static_cast(payload_len & 0xFF), + 0x03, 0xFE, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + + flushSocket(sock); + setSocketTimeout(sock, 1000); + + int total_sent = 0; + int retry_cnt = 3; + while (total_sent < 16 && retry_cnt > 0) + { + int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); + if (ret <= 0) + { + retry_cnt--; + usleep(10000); + continue; + } + total_sent += ret; + } + if (total_sent != 16) + { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send frame header failed"; + return; + } + + const int recv_buf_len = 65536; + std::vector recv_buf(recv_buf_len); + std::string raw_data; + int recv_ret = 0; + while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, 0)) > 0) + { + raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); + } + setSocketTimeout(sock, 0); + + if (raw_data.empty()) + { + res.ret_code = -2; + res.err_msg = "no response data"; + return; + } + + size_t json_start = raw_data.find('{'); + size_t json_end = raw_data.rfind('}'); + std::string json_str; + if (json_start == std::string::npos) + { + res.ret_code = -4; + res.err_msg = "json not found in response"; + return; + } + if (json_end == std::string::npos) + { + json_str = raw_data.substr(json_start) + "}"; + } + else + { + json_str = raw_data.substr(json_start, json_end - json_start + 1); + } + + json root; + std::string parse_err; + if (!parseJson(json_str, root, parse_err)) + { + res.ret_code = -3; + res.err_msg = "json parse error: " + parse_err; + return; + } + + res.loadmap_status = root.value("loadmap_status", 0); + res.ret_code = root.value("ret_code", 0); + res.create_on = root.value("create_on", ""); + res.err_msg = root.value("err_msg", ""); +} + + + // =========================================================================== +// 接口：查询当前地图站点列表 命令码 1301 (0x0515) +// 通信端口：sock_status_ 19204状态端口 +// 功能：获取当前加载地图所有LocationMark/ChargePoint/ActionPoint站点 +// =========================================================================== +void AgvSrc1100::queryStationList(QueryStationRes& res) +{ + int sock = sock_status_; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "socket not connected"; + return; + } + + uint16_t payload_len = 0; + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, + static_cast((payload_len >> 8) & 0xFF), + static_cast(payload_len & 0xFF), + 0x05, 0x15, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + + flushSocket(sock); + setSocketTimeout(sock, 1000); + + int total_sent = 0; + int retry_cnt = 3; + while (total_sent < 16 && retry_cnt > 0) + { + int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); + if (ret <= 0) + { + retry_cnt--; + usleep(10000); + continue; + } + total_sent += ret; + } + if (total_sent != 16) + { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send frame header failed"; + return; + } + + const int recv_buf_len = 65536; + std::vector recv_buf(recv_buf_len); + std::string raw_data; + int recv_ret = 0; + while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, 0)) > 0) + { + raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); + } + setSocketTimeout(sock, 0); + + if (raw_data.empty()) + { + res.ret_code = -2; + res.err_msg = "no response data"; + return; + } + + size_t json_start = raw_data.find('{'); + size_t json_end = raw_data.rfind('}'); + std::string json_str; + if (json_start == std::string::npos) + { + res.ret_code = -4; + res.err_msg = "json not found in response"; + return; + } + if (json_end == std::string::npos) + { + json_str = raw_data.substr(json_start) + "}"; + } + else + { + json_str = raw_data.substr(json_start, json_end - json_start + 1); + } + + json root; + std::string parse_err; + if (!parseJson(json_str, root, parse_err)) + { + res.ret_code = -3; + res.err_msg = "json parse error: " + parse_err; + return; + } + + res.ret_code = root.value("ret_code", 0); + res.create_on = root.value("create_on", ""); + res.err_msg = root.value("err_msg", ""); + + // 解析站点数组 + // 解析站点数组 + if (root.contains("stations") && root["stations"].is_array()) + { + for (auto& item : root["stations"]) + { + StationItem st; + st.id = item.value("id", ""); + st.type = item.value("type", ""); + st.x = item.value("x", 0.0); + st.y = item.value("y", 0.0); + st.r = item.value("r", 0.0); + st.desc = item.value("desc", ""); + st.executor = item.value("executor", ""); + st.prepoint = item.value("prepoint", ""); + st.recfile = item.value("recfile", ""); + + // 兼容bool/字符串"true"/"false" + if (item.contains("spin")) + { + if (item["spin"].is_boolean()) + st.spin = item["spin"].get(); + else if (item["spin"].is_string()) + st.spin = (item["spin"].get() == "true"); + } + if (item.contains("use_down_pgv")) + { + if (item["use_down_pgv"].is_boolean()) + st.use_down_pgv = item["use_down_pgv"].get(); + else if (item["use_down_pgv"].is_string()) + st.use_down_pgv = (item["use_down_pgv"].get() == "true"); + } + + res.stations.push_back(st); + } + } +} + + +// =========================================================================== +// 接口：指定路径导航 命令码 3066 (0x0BFA) +// 通信端口：sock_control_ 19205控制端口 +// 功能：下发多段连续站点导航任务序列，任务串行排队执行 +// 必填字段：task_id / source_id / id +// =========================================================================== +void AgvSrc1100::robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) +{ + int sock = sock_nav_; // ✅ 修正：使用 sock_nav_ 而非 port_nav_ + LOG(INFO) << "robotGoTargetList sock fd=" << sock; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "nav socket invalid"; // 更新错误信息 + return; + } + + // 组装请求JSON + json send_json; + json arr = json::array(); + for (const auto& task : req.move_task_list) + { + json obj; + obj["task_id"] = task.task_id; + obj["source_id"] = task.source_id; + obj["id"] = task.id; + if (!task.operation.empty()) + { + obj["operation"] = task.operation; + obj["jack_height"] = task.jack_height; + } + arr.push_back(obj); + } + send_json["move_task_list"] = arr; + std::string payload = send_json.dump(); + uint16_t payload_len = static_cast(payload.size()); + LOG(INFO) << "nav payload length:" << payload_len; + + // 静态帧头缓冲区 + static uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, + 0,0, + 0x0B, 0xFA, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + send_buf[6] = static_cast((payload_len >> 8) & 0xFF); + send_buf[7] = static_cast(payload_len & 0xFF); + + flushSocket(sock); + setSocketTimeout(sock, 3000); + + // 发送16字节帧头 + int total_sent = 0; + int retry_cnt = 5; + while (total_sent < 16 && retry_cnt > 0) + { + int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); + if (ret <= 0) + { + LOG(ERROR) << "send header failed, ret=" << ret + << ", errno=" << errno + << " (" << strerror(errno) << ")"; + retry_cnt--; + usleep(20000); + continue; // 不再检查 sock_control_，因为 sock 已在开头验证 + } + total_sent += ret; + } + if (total_sent != 16) + { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send frame header failed"; + return; + } + + // 分片发送，缩小分片大小至1024 + const int slice_size = 1024; + size_t offset = 0; + size_t total_size = payload.size(); + const char* data_ptr = payload.data(); + while (offset < total_size) + { + int send_len = std::min((size_t)slice_size, total_size - offset); + int ret = send(sock, data_ptr + offset, send_len, MSG_NOSIGNAL); + if (ret <= 0) + { + LOG(ERROR) << "send payload failed, errno=" << errno << " (" << strerror(errno) << ")"; + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send payload failed"; + return; + } + offset += ret; + } + + // 接收并解析响应 + const int recv_buf_len = 65536; + std::vector recv_buf(recv_buf_len); + std::string raw_data; + int recv_ret = 0; + while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, MSG_NOSIGNAL)) > 0) + { + raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); + } + setSocketTimeout(sock, 0); + + if (raw_data.empty()) + { + res.ret_code = -2; + res.err_msg = "no response data"; + return; + } + + size_t json_start = raw_data.find('{'); + if (json_start == std::string::npos) + { + res.ret_code = -4; + res.err_msg = "json not found in response"; + return; + } + std::string json_str = raw_data.substr(json_start); + if (json_str.back() != '}') + { + json_str += "}"; + } + + json root; + std::string parse_err; + try + { + root = json::parse(json_str); + } + catch (json::parse_error& e) + { + res.ret_code = -3; + res.err_msg = "json parse error: " + std::string(e.what()); + return; + } + + res.ret_code = root.contains("ret_code") ? root["ret_code"].get() : 0; + res.create_on = root.contains("create_on") ? root["create_on"].get() : ""; + res.err_msg = root.contains("err_msg") ? root["err_msg"].get() : ""; +} + + + //=========================================================================== +// 1020 robot_status_task_req 0x03FC 查询当前导航状态 sock_status_ 19204 +// 应答编号 11020 0x2B0C +//=========================================================================== +void AgvSrc1100::robotStatusTask(RobotStatusTaskRes& res, const RobotStatusTaskReq& req) +{ + int sock = sock_status_; + LOG(INFO) << "robotStatusTask sock fd=" << sock; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "status port socket invalid"; + return; + } + + // 组装请求JSON + json send_json; + if (req.simple) + { + send_json["simple"] = true; + } + std::string payload = send_json.dump(); + uint16_t payload_len = static_cast(payload.size()); + + // 帧头 0x03FC = 1020 + static uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, + 0,0, + 0x03, 0xFC, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }; + send_buf[6] = static_cast((payload_len >> 8) & 0xFF); + send_buf[7] = static_cast(payload_len & 0xFF); + + flushSocket(sock); + setSocketTimeout(sock, 3000); + + // 发送帧头 5次重试 + int total_sent = 0; + int retry_cnt = 5; + while (total_sent < 16 && retry_cnt > 0) + { + int ret = send(sock, send_buf + total_sent, 16 - total_sent, MSG_NOSIGNAL); + if (ret <= 0) + { + retry_cnt--; + usleep(20000); + if (sock_status_ < 0) + { + setSocketTimeout(sock, 0); + res.ret_code = -1; + res.err_msg = "socket broken send header"; + return; + } + continue; + } + total_sent += ret; + } + if (total_sent != 16) + { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send frame header failed"; + return; + } + + // 分片发送payload + const int slice_size = 1024; + size_t offset = 0; + size_t total_size = payload.size(); + const char* data_ptr = payload.data(); + while (offset < total_size) + { + int send_len = std::min((size_t)slice_size, total_size - offset); + int ret = send(sock, data_ptr + offset, send_len, MSG_NOSIGNAL); + if (ret <= 0) + { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send payload failed"; + return; + } + offset += ret; + } + + // 接收应答 + const int recv_buf_len = 65536; + std::vector recv_buf(recv_buf_len); + std::string raw_data; + int recv_ret = 0; + while ((recv_ret = recv(sock, recv_buf.data(), recv_buf_len, MSG_NOSIGNAL)) > 0) + { + raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); + } + setSocketTimeout(sock, 0); + + if (raw_data.empty()) + { + res.ret_code = -2; + res.err_msg = "no response data"; + return; + } + + // 截取纯JSON + size_t json_start = raw_data.find('{'); + if (json_start == std::string::npos) + { + res.ret_code = -4; + res.err_msg = "json not found in response"; + return; + } + std::string json_str = raw_data.substr(json_start); + if (json_str.back() != '}') + { + json_str += "}"; + } + + // JSON解析捕获异常 + json root; + try + { + root = json::parse(json_str); + } + catch (json::parse_error& e) + { + res.ret_code = -3; + res.err_msg = "json parse error: " + std::string(e.what()); + return; + } + + // 顶层公共字段 + res.ret_code = root.contains("ret_code") ? root["ret_code"].get() : 0; + res.create_on = root.contains("create_on") ? root["create_on"].get() : ""; + res.err_msg = root.contains("err_msg") ? root["err_msg"].get() : ""; + + // 导航基础状态 + res.task_status = root.contains("task_status") ? root["task_status"].get() : 0; + res.task_type = root.contains("task_type") ? root["task_type"].get() : 0; + res.target_id = root.contains("target_id") ? root["target_id"].get() : ""; + res.move_status_info = root.contains("move_status_info") ? root["move_status_info"].get() : ""; + + // target_point x,y,r + if (root.contains("target_point") && root["target_point"].is_array()) + { + for (auto& val : root["target_point"]) + { + res.target_point.push_back(val.get()); + } + } + + // 已走过站点 + if (root.contains("finished_path") && root["finished_path"].is_array()) + { + for (auto& s : root["finished_path"]) + { + res.finished_path.push_back(s.get()); + } + } + // 未走站点 + if (root.contains("unfinished_path") && root["unfinished_path"].is_array()) + { + for (auto& s : root["unfinished_path"]) + { + res.unfinished_path.push_back(s.get()); + } + } + + // 料箱数组 + if (root.contains("containers") && root["containers"].is_array()) + { + for (auto& item : root["containers"]) + { + NavContainerItem c; + c.container_name = item.contains("container_name") ? item["container_name"].get() : ""; + c.desc = item.contains("desc") ? item["desc"].get() : ""; + c.goods_id = item.contains("goods_id") ? item["goods_id"].get() : ""; + if (item.contains("has_goods")) + { + if (item["has_goods"].is_boolean()) + c.has_goods = item["has_goods"].get(); + else if (item["has_goods"].is_string()) + c.has_goods = (item["has_goods"].get() == "true"); + } + res.containers.push_back(c); + } + } +} + + + +} // namespace cmvr::device + + + diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 4ef50063..98ea4199 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -9,6 +9,7 @@ add_library(service grpc/src/grpc_humanoid_robot_service.cpp grpc/src/grpc_hlc_service.cpp grpc/src/server_runner.cpp + grpc/src/grpc_agv_service.cpp ) target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) 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..d620c474 --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_agv_service.h @@ -0,0 +1,131 @@ +// +// Created by xtkuang on 2025/6/1. +// + +#ifndef GRPC_AGV_SERVICE_H +#define GRPC_AGV_SERVICE_H + +#include "cmvr/api/agv_service.grpc.pb.h" +#include "utils/base/include/grpc_utils.h" +#include "device_manager/include/device_manager.h" + +namespace cmvr::service { + + class gRPCAGVServiceImpl final : public api::AgvService::Service { + public: + gRPCAGVServiceImpl(); + ~gRPCAGVServiceImpl() override = default; + + // AGV服务接口 + grpc::Status GetStatusInfo(grpc::ServerContext* context, + const api::GetAgvStatusInfoCommand_Request* request, + api::GetAgvStatusInfoCommand_Feedback* response) override; + + grpc::Status GetBatteryStatus(grpc::ServerContext* context, + const api::RobotStatusBatteryCommand_Request* request, + api::RobotStatusBatteryCommand_Feedback* response) override; + + grpc::Status GetRobotLocation(grpc::ServerContext* context, + const api::RobotStatusLocCommand_Request* request, + api::RobotStatusLocCommand_Feedback* response) override; + + grpc::Status RobotConfigDownloadMap(grpc::ServerContext* context, + const api::RobotConfigDownloadMapCommand_Request* request, + api::RobotConfigDownloadMapCommand_Feedback* response) override; + + grpc::Status GetMapStatus(grpc::ServerContext* context, + const api::RobotStatusMapCommand_Request* request, + api::RobotStatusMapCommand_Feedback* response) override; + + grpc::Status RobotConfigUploadMap(grpc::ServerContext* context, + const api::RobotConfigUploadMapCommand_Request* request, + api::RobotConfigUploadMapCommand_Feedback* response) override; + /** + * @brief gRPC接口：抢占机器人控制权 + */ + grpc::Status RobotConfigLock(grpc::ServerContext* context, + const api::RobotConfigLockCommand_Request* request, + api::RobotConfigLockCommand_Feedback* response) override; + + + + /** + * @brief gRPC接口 查询当前控制权持有者 + */ + grpc::Status GetCurrentLockStatus(grpc::ServerContext* context, + const api::RobotStatusCurrentLockCommand_Request* request, + api::RobotStatusCurrentLockCommand_Feedback* response) override; + + /** + * @brief AGV开环速度运动控制 + * @param context grpc上下文 + * @param request 请求体，header携带device_id区分多AGV + * @param response 运动指令执行结果 + */ + grpc::Status RobotMotionControl(grpc::ServerContext* context, + const api::RobotMotionControlCommand_Request* request, + api::RobotMotionControlCommand_Feedback* response) override; + + + /** + * @brief 切换载入地图 + * @param context grpc上下文 + * @param request 请求体，携带目标地图名称 + * @param response 切换地图执行结果 + */ + grpc::Status RobotLoadMap( + grpc::ServerContext* context, + const api::RobotLoadMapCommand_Request* request, + api::RobotLoadMapCommand_Feedback* response) override; + + /** + * @brief 查询地图载入状态 + * @param context grpc上下文 + * @param request 请求头 + * @param response 地图加载状态结果 + */ + grpc::Status QueryLoadMapStatus( + grpc::ServerContext* context, + const api::RobotQueryLoadMapStatusCommand_Request* request, + api::RobotQueryLoadMapStatusCommand_Feedback* response) override; + + /** + * @brief 查询当前地图所有站点信息 + * @param context grpc上下文 + * @param request 请求头 + * @param response 站点列表结果 + */ + grpc::Status QueryStationList( + grpc::ServerContext* context, + const api::QueryStationListCommand_Request* request, + api::QueryStationListCommand_Feedback* response) override; + + + /** + * @brief 指定路径连续站点导航 + * @param context grpc上下文 + * @param request 导航任务列表请求 + * @param response 下发任务返回结果 + */ + grpc::Status RobotGoTargetList( + grpc::ServerContext* context, + const api::RobotGoTargetListCommand_Request* request, + api::RobotGoTargetListCommand_Feedback* response) override; + + + grpc::Status RobotStatusTask( + grpc::ServerContext* context, + const api::RobotStatusTaskCommand_Request* request, + api::RobotStatusTaskCommand_Feedback* response) override; + + + + + + + private: + device::DeviceManager& dmgr_; + }; +} + +#endif //GRPC_AGV_SERVICE_H \ No newline at end of file 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..8b7fd741 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -0,0 +1,708 @@ +// +// Created by xtkuang on 2025/6/1. +// + +#include "./service/grpc/include/grpc_agv_service.h" + +#include + +#include "devices/agv/agv_src1100/include/agv_src1100.h" + +#include "devices/agv/abstract_agv.h" + +namespace cmvr::service { + + gRPCAGVServiceImpl::gRPCAGVServiceImpl() : dmgr_(device::DeviceManager::getInstance()) { + } + + grpc::Status gRPCAGVServiceImpl::GetStatusInfo( + grpc::ServerContext* context, + const api::GetAgvStatusInfoCommand_Request* request, + api::GetAgvStatusInfoCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (GetStatusInfo): id=" << dev_id; + const auto agv_device = dmgr_.getDevice(dev_id); + + device::AbstractAgv::AgvStatusInfo device_status; + agv_device->getStatusInfo(device_status); + + auto* proto_status = response->mutable_status(); + proto_status->set_id(device_status.id); + proto_status->set_vehicle_id(device_status.vehicle_id); + proto_status->set_version(device_status.version); + proto_status->set_model(device_status.model); + proto_status->set_dsp_version(device_status.dsp_version); + proto_status->set_current_ip(device_status.current_ip); + proto_status->set_mac(device_status.mac); + proto_status->set_rssi(device_status.rssi); + proto_status->set_ret_code(device_status.ret_code); + proto_status->set_err_msg(device_status.err_msg); + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "GetStatusInfo device_id:" << dev_id << " vehicle_id: " << device_status.vehicle_id; + return grpc::Status::OK; + } + catch (std::exception &e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + +grpc::Status gRPCAGVServiceImpl::GetBatteryStatus( +grpc::ServerContext* context, +const api::RobotStatusBatteryCommand_Request* request, +api::RobotStatusBatteryCommand_Feedback* response) +{ + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (GetBatteryStatus): id=" << dev_id; + const auto agv_device = dmgr_.getDevice(dev_id); + + bool simple = false; + if (request->has_data()) + { + simple = request->data().simple(); + } + + device::AbstractAgv::BatteryStatus bat_info; + agv_device->getBatteryStatus(bat_info, simple); + + auto* proto_bat = response->mutable_status(); + proto_bat->set_battery_level(bat_info.battery_level); + proto_bat->set_battery_temp(bat_info.battery_temp); + proto_bat->set_charging(bat_info.charging); + proto_bat->set_voltage(bat_info.voltage); + proto_bat->set_current(bat_info.current); + proto_bat->set_max_charge_voltage(bat_info.max_charge_voltage); + proto_bat->set_max_charge_current(bat_info.max_charge_current); + proto_bat->set_manual_charge(bat_info.manual_charge); + proto_bat->set_auto_charge(bat_info.auto_charge); + proto_bat->set_battery_cycle(bat_info.battery_cycle); + proto_bat->set_battery_user_data(bat_info.battery_user_data); + proto_bat->set_extra(bat_info.extra); + proto_bat->set_ret_code(bat_info.ret_code); + proto_bat->set_create_on(bat_info.create_on); + proto_bat->set_err_msg(bat_info.err_msg); + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "GetBatteryStatus device_id:" << dev_id << " level: " << bat_info.battery_level; + return grpc::Status::OK; + } + catch (std::exception &e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } +} + + + grpc::Status gRPCAGVServiceImpl::GetRobotLocation( + grpc::ServerContext* context, + const api::RobotStatusLocCommand_Request* request, + api::RobotStatusLocCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (GetRobotLocation): id=" << dev_id; + const auto agv_device = dmgr_.getDevice(dev_id); + + device::AbstractAgv::RobotLocation loc_info; + agv_device->getRobotLocation(loc_info); + + auto* proto_loc = response->mutable_status(); + proto_loc->set_x(loc_info.x); + proto_loc->set_y(loc_info.y); + proto_loc->set_angle(loc_info.angle); + proto_loc->set_confidence(loc_info.confidence); + proto_loc->set_current_station(loc_info.current_station); + proto_loc->set_last_station(loc_info.last_station); + proto_loc->set_loc_method(loc_info.loc_method); + proto_loc->set_ret_code(loc_info.ret_code); + proto_loc->set_create_on(loc_info.create_on); + proto_loc->set_err_msg(loc_info.err_msg); + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "GetRobotLocation device_id:" << dev_id << " x: " << loc_info.x << ", y: " << loc_info.y; + return grpc::Status::OK; + } + catch (std::exception &e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + + grpc::Status gRPCAGVServiceImpl::RobotConfigDownloadMap( + grpc::ServerContext* context, + const api::RobotConfigDownloadMapCommand_Request* request, + api::RobotConfigDownloadMapCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (RobotConfigDownloadMap): id=" << dev_id; + const auto agv_device = dmgr_.getDevice(dev_id); + + std::string map_name = request->data().map_name(); + device::AbstractAgv::DownloadMapResult down_info; + agv_device->downloadMap(down_info, map_name); + + auto* proto_res = response->mutable_status(); + proto_res->set_ret_code(down_info.ret_code); + proto_res->set_create_on(down_info.create_on); + proto_res->set_err_msg(down_info.err_msg); + proto_res->set_map_content(down_info.map_content); + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "DownloadMap device_id:" << dev_id << " map: " << map_name << ", ret_code: " << down_info.ret_code; + return grpc::Status::OK; + } + catch (std::exception &e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + + + grpc::Status gRPCAGVServiceImpl::GetMapStatus( + grpc::ServerContext* context, + const api::RobotStatusMapCommand_Request* request, + api::RobotStatusMapCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (GetMapStatus): id=" << dev_id; + const auto agv_device = dmgr_.getDevice(dev_id); + + device::AbstractAgv::MapStatus map_info; + agv_device->getMapStatus(map_info); + + auto* proto_map = response->mutable_status(); + proto_map->set_current_map(map_info.current_map); + proto_map->set_current_map_md5(map_info.current_map_md5); + proto_map->set_ret_code(map_info.ret_code); + proto_map->set_create_on(map_info.create_on); + proto_map->set_err_msg(map_info.err_msg); + + for (auto& name : map_info.maps) + { + proto_map->add_maps(name); + } + + for (auto& file : map_info.map_files_info) + { + auto* proto_file = proto_map->add_map_files_info(); + proto_file->set_name(file.name); + proto_file->set_modified(file.modified); + proto_file->set_size(file.size); + } + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "GetMapStatus device_id:" << dev_id << " current map: " << map_info.current_map; + return grpc::Status::OK; + } + catch (std::exception &e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + + grpc::Status gRPCAGVServiceImpl::RobotConfigUploadMap( + grpc::ServerContext* context, + const api::RobotConfigUploadMapCommand_Request* request, + api::RobotConfigUploadMapCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (RobotConfigUploadMap): id=" << dev_id; + const auto agv_device = dmgr_.getDevice(dev_id); + + std::string map_json = request->data().map_content(); + device::AbstractAgv::UploadMapResult upload_info; + agv_device->uploadMap(upload_info, map_json); + + auto* proto_res = response->mutable_status(); + proto_res->set_ret_code(upload_info.ret_code); + proto_res->set_create_on(upload_info.create_on); + proto_res->set_err_msg(upload_info.err_msg); + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "UploadMap device_id:" << dev_id << " ret_code: " << upload_info.ret_code; + return grpc::Status::OK; + } + catch (std::exception &e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + + /** + * @brief gRPC 抢占机器人控制权接口 + * @param context grpc上下文 + * @param request 前端请求参数，携带nick_name抢占者名称 + * @param response 抢占结果返回体 + */ + grpc::Status gRPCAGVServiceImpl::RobotConfigLock( + grpc::ServerContext* context, + const api::RobotConfigLockCommand_Request* request, + api::RobotConfigLockCommand_Feedback* response) + { + try { + // 动态从header拿设备ID，和查询接口统一 + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (RobotConfigLock): id=" << dev_id; + + // 根据device_id获取对应AGV实例 + const auto agv_device = dmgr_.getDevice(dev_id); + + // 读取抢占者名称 + std::string nick_name = request->data().nick_name(); + device::AbstractAgv::LockResult lock_res; + // 调用底层抢占接口 + agv_device->lockRobotControl(lock_res, nick_name); + + // 填充protobuf返回数据 + auto* proto_status = response->mutable_status(); + proto_status->set_ret_code(lock_res.ret_code); + proto_status->set_create_on(lock_res.create_on); + proto_status->set_err_msg(lock_res.err_msg); + + // 响应头标记成功 + 时间戳对齐全局规范 + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "Lock robot control, device_id:" << dev_id << ", nick:" << nick_name << ", ret_code:" << lock_res.ret_code; + return grpc::Status::OK; + } + catch (std::exception &e) + { + // 统一异常捕获，和相机/查询接口风格完全一致 + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + +/** + * @brief 查询机器人当前控制权信息gRPC接口 + */ +grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( + grpc::ServerContext* context, + const api::RobotStatusCurrentLockCommand_Request* request, + api::RobotStatusCurrentLockCommand_Feedback* response) +{ + try { + // 1. 读取请求device_id，打印日志（和相机代码保持一致） + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (GetCurrentLockStatus): id=" << dev_id; + + // 2. 按device_id获取AGV设备实例，和相机 dmgr_.getDevice(dev_id) 统一写法 + const auto agv_dev = dmgr_.getDevice(dev_id); + + // 3. 调用底层接口获取控制权信息 + device::AbstractAgv::CurrentLockStatus lock_info; + agv_dev->getCurrentLockStatus(lock_info); + + // 4. 填充protobuf返回结构体 + auto* proto_status = response->mutable_status(); + proto_status->set_locked(lock_info.locked); + proto_status->set_ip(lock_info.ip); + proto_status->set_port(lock_info.port); + proto_status->set_type(lock_info.type); + proto_status->set_nick_name(lock_info.nick_name); + proto_status->set_time_t(lock_info.time_t); + proto_status->set_desc(lock_info.desc); + proto_status->set_ret_code(lock_info.ret_code); + proto_status->set_create_on(lock_info.create_on); + proto_status->set_err_msg(lock_info.err_msg); + + // 5. 响应header标记成功 + 填充时间戳（对齐相机接口） + response->mutable_header()->set_success(true); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + + LOG(INFO) << "Query current lock status, device_id:" << dev_id + << ", locked:" << lock_info.locked << ", nick:" << lock_info.nick_name; + return grpc::Status::OK; + } + catch (std::exception &e) + { + // 异常统一捕获，失败header，填错误信息+时间戳，和相机完全对齐 + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(e.what()); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } +} + + + grpc::Status gRPCAGVServiceImpl::RobotMotionControl( + grpc::ServerContext* context, + const api::RobotMotionControlCommand_Request* request, + api::RobotMotionControlCommand_Feedback* response) + { + try { + // 从header读取device_id，区分多台AGV设备 + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (RobotMotionControl): target device_id=" << dev_id; + const auto agv_dev = dmgr_.getDevice(dev_id); + + // grpc入参转换到底层结构体 + device::AbstractAgv::MotionCtrlReq req; + auto& input_data = request->data(); + req.vx = input_data.vx(); + req.vy = input_data.vy(); + req.w = input_data.w(); + req.steer = input_data.steer(); + req.real_steer = input_data.real_steer(); + req.duration = input_data.duration(); + + // 调用底层运动控制接口 + device::AbstractAgv::MotionCtrlRes res; + agv_dev->robotMotionControl(res, req); + + // 填充grpc返回状态 + auto* resp_status = response->mutable_status(); + resp_status->set_ret_code(res.ret_code); + resp_status->set_create_on(res.create_on); + resp_status->set_err_msg(res.err_msg); + + // 统一规范header：成功标记+时间戳 + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "RobotMotionControl finish, dev_id:" << dev_id + << " vx=" << req.vx << " vy=" << req.vy << " w=" << req.w + << " ret_code=" << res.ret_code; + return grpc::Status::OK; + } + catch (std::exception& e) + { + // 全局统一异常捕获模板 + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + grpc::Status gRPCAGVServiceImpl::RobotLoadMap( + grpc::ServerContext* context, + const api::RobotLoadMapCommand_Request* request, + api::RobotLoadMapCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (RobotLoadMap): target device_id=" << dev_id; + const auto agv_dev = dmgr_.getDevice(dev_id); + + device::AbstractAgv::LoadMapReq req; + auto& input_data = request->data(); + req.map_name = input_data.map_name(); + + device::AbstractAgv::LoadMapRes res; + agv_dev->robotLoadMap(res, req); + + auto* resp_status = response->mutable_status(); + resp_status->set_ret_code(res.ret_code); + resp_status->set_create_on(res.create_on); + resp_status->set_err_msg(res.err_msg); + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + // 修复：取timestamp子字段 + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "RobotLoadMap finish, dev_id:" << dev_id + << " map_name=" << req.map_name + << " ret_code=" << res.ret_code; + return grpc::Status::OK; + } + catch (std::exception& e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + // 修复：取timestamp子字段 + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + + grpc::Status gRPCAGVServiceImpl::QueryLoadMapStatus( + grpc::ServerContext* context, + const api::RobotQueryLoadMapStatusCommand_Request* request, + api::RobotQueryLoadMapStatusCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (QueryLoadMapStatus): device_id=" << dev_id; + const auto agv_dev = dmgr_.getDevice(dev_id); + + device::AbstractAgv::QueryLoadMapStatusRes res; + agv_dev->queryLoadMapStatus(res); + + auto* resp_status = response->mutable_status(); + resp_status->set_loadmap_status(res.loadmap_status); + resp_status->set_ret_code(res.ret_code); + resp_status->set_create_on(res.create_on); + resp_status->set_err_msg(res.err_msg); + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "QueryLoadMapStatus finish dev_id:" << dev_id + << " loadmap_status=" << res.loadmap_status + << " ret_code=" << res.ret_code; + return grpc::Status::OK; + } + catch (std::exception& e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + grpc::Status gRPCAGVServiceImpl::QueryStationList( + grpc::ServerContext* context, + const api::QueryStationListCommand_Request* request, + api::QueryStationListCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (QueryStationList): device_id=" << dev_id; + const auto agv_dev = dmgr_.getDevice(dev_id); + + device::AbstractAgv::QueryStationRes res; + agv_dev->queryStationList(res); + + auto* resp_status = response->mutable_status(); + resp_status->set_ret_code(res.ret_code); + resp_status->set_create_on(res.create_on); + resp_status->set_err_msg(res.err_msg); + + // 填充站点数组 + for (auto& st : res.stations) + { + auto* pb_st = resp_status->add_stations(); + pb_st->set_id(st.id); + pb_st->set_type(st.type); + pb_st->set_x(st.x); + pb_st->set_y(st.y); + pb_st->set_r(st.r); + pb_st->set_desc(st.desc); + pb_st->set_executor(st.executor); + pb_st->set_prepoint(st.prepoint); + pb_st->set_recfile(st.recfile); + pb_st->set_spin(st.spin); + pb_st->set_use_down_pgv(st.use_down_pgv); + } + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "QueryStationList finish dev_id:" << dev_id + << " station_count=" << res.stations.size() + << " ret_code=" << res.ret_code; + return grpc::Status::OK; + } + catch (std::exception& e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + + grpc::Status gRPCAGVServiceImpl::RobotGoTargetList( + grpc::ServerContext* context, + const api::RobotGoTargetListCommand_Request* request, + api::RobotGoTargetListCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (RobotGoTargetList): target device_id=" << dev_id; + const auto agv_dev = dmgr_.getDevice(dev_id); + + device::AbstractAgv::GoTargetListReq req; + const auto& pb_list = request->data().move_task_list(); + for (const auto& pb_item : pb_list) + { + device::AbstractAgv::MoveTaskItem task; + task.task_id = pb_item.task_id(); + task.source_id = pb_item.source_id(); + task.id = pb_item.id(); + task.operation = pb_item.operation(); + task.jack_height = pb_item.jack_height(); + req.move_task_list.push_back(task); + } + + device::AbstractAgv::GoTargetListRes res; + agv_dev->robotGoTargetList(res, req); + + auto* resp_status = response->mutable_status(); + resp_status->set_ret_code(res.ret_code); + resp_status->set_create_on(res.create_on); + resp_status->set_err_msg(res.err_msg); + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "RobotGoTargetList finish dev_id:" << dev_id + << " task_count=" << req.move_task_list.size() + << " ret_code=" << res.ret_code; + return grpc::Status::OK; + } + catch (std::exception& e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + + + + grpc::Status gRPCAGVServiceImpl::RobotStatusTask( + grpc::ServerContext* context, + const api::RobotStatusTaskCommand_Request* request, + api::RobotStatusTaskCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (RobotStatusTask): id=" << dev_id; + const auto agv_dev = dmgr_.getDevice(dev_id); + + // 嵌套结构体 必须加 AbstractAgv:: + device::AbstractAgv::RobotStatusTaskReq req; + if (request->has_data()) + { + req.simple = request->data().simple(); + } + + device::AbstractAgv::RobotStatusTaskRes res; + agv_dev->robotStatusTask(res, req); + + auto* out_data = response->mutable_data(); + out_data->set_ret_code(res.ret_code); + out_data->set_create_on(res.create_on); + out_data->set_err_msg(res.err_msg); + + out_data->set_task_status(res.task_status); + out_data->set_task_type(res.task_type); + out_data->set_target_id(res.target_id); + out_data->set_move_status_info(res.move_status_info); + + for (double p : res.target_point) + out_data->add_target_point(p); + for (auto& s : res.finished_path) + out_data->add_finished_path(s); + for (auto& s : res.unfinished_path) + out_data->add_unfinished_path(s); + + for (auto& c : res.containers) + { + auto* pc = out_data->add_containers(); + pc->set_container_name(c.container_name); + pc->set_desc(c.desc); + pc->set_goods_id(c.goods_id); + pc->set_has_goods(c.has_goods); + } + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "RobotStatusTask device_id:" << dev_id << " task_status: " << res.task_status; + return grpc::Status::OK; + } + catch (std::exception &e) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message(e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + + +} \ No newline at end of file diff --git a/cmvr-es/service/grpc/src/server_runner.cpp b/cmvr-es/service/grpc/src/server_runner.cpp index ba3d0ce1..32726748 100644 --- a/cmvr-es/service/grpc/src/server_runner.cpp +++ b/cmvr-es/service/grpc/src/server_runner.cpp @@ -22,6 +22,7 @@ #include "service/grpc/include/grpc_hlc_service.h" #include "json/json.h" #include +#include "service/grpc/include/grpc_agv_service.h" using namespace cmvr::service; ServerRunner::ServerRunner() = default; @@ -118,6 +119,7 @@ void ServerRunner::threadMain() { auto biohand_service = std::make_unique(); auto humanoid_robot_service = std::make_unique(); auto hlc_service = std::make_unique(); + auto agv_service = std::make_unique(); grpc::ServerBuilder builder; builder.AddListeningPort(local_address, grpc::InsecureServerCredentials()); @@ -129,6 +131,7 @@ void ServerRunner::threadMain() { builder.RegisterService(biohand_service.get()); builder.RegisterService(humanoid_robot_service.get()); builder.RegisterService(hlc_service.get()); + builder.RegisterService(agv_service.get()); auto local_server = builder.BuildAndStart(); if (!local_server) { diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto new file mode 100644 index 00000000..d2a4fad9 --- /dev/null +++ b/protos/cmvr/api/agv_command.proto @@ -0,0 +1,424 @@ +syntax = "proto3"; + +import "cmvr/api/common.proto"; + +package cmvr.api; + +// AGV状态信息 +message AgvStatusInfo { + optional string id = 1; // AGV ID + optional string vehicle_id = 2; // 车辆ID + optional string version = 3; // 软件版本 + optional string model = 4; // 型号 + optional string dsp_version = 5; // DSP版本 + optional string current_ip = 6; // 当前IP地址 + optional string mac = 7; // MAC地址 + optional int32 rssi = 8; // 信号强度 + optional int32 ret_code = 9; // 返回码 + optional string err_msg = 10; // 错误信息 +} + +// 获取AGV状态命令 +message GetAgvStatusInfoCommand { + message Request { + CommandHeader.Request header = 1; + } + + message Feedback { + CommandHeader.Feedback header = 1; + AgvStatusInfo status = 2; + } +} + + +// ===================== 电池状态查询 ===================== +message AgvBatteryStatus { + optional double battery_level = 1; // 电池电量 [0,1] + optional double battery_temp = 2; // 电池温度 ℃ + optional bool charging = 3; // 是否正在充电 + optional double voltage = 4; // 电压 V + optional double current = 5; // 电流 A + optional double max_charge_voltage = 6; // 最大充电电压，-1=不支持 + optional double max_charge_current = 7; // 最大充电电流，-1=不支持 + optional bool manual_charge = 8; // 手动充电状态 + optional bool auto_charge = 9; // 自动充电状态 + optional int32 battery_cycle = 10; // 电池循环次数 + optional string battery_user_data = 11; // 电池自定义数据 + optional string extra = 12; // 客户自定义数据 + optional int32 ret_code = 13; // 错误码 + optional string create_on = 14; // 时间戳 + optional string err_msg = 15; // 错误信息 +} + +message RobotStatusBatteryRequestData { + optional bool simple = 1; // true:仅简单数据 false:全量数据，默认false +} + +message RobotStatusBatteryCommand { + message Request { + CommandHeader.Request header = 1; + RobotStatusBatteryRequestData data = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + AgvBatteryStatus status = 2; + } +} + + + +// ===================== 机器人位置查询 ===================== +message AgvRobotLocation { + optional double x = 1; // X坐标(m) + optional double y = 2; // Y坐标(m) + optional double angle = 3; // 姿态角(rad) + optional double confidence = 4; // 定位置信度[0,1] + optional string current_station = 5; // 当前最近站点ID + optional string last_station = 6; // 上一站点ID + optional int32 loc_method = 7; // 定位方式 + optional int32 ret_code = 8; // 错误码 + optional string create_on = 9; // 时间戳 + optional string err_msg = 10; // 错误信息 +} + +message RobotStatusLocCommand { + message Request { + CommandHeader.Request header = 1; + } + message Feedback { + CommandHeader.Feedback header = 1; + AgvRobotLocation status = 2; + } +} + + +// ===================== 地图下载 ===================== +message RobotConfigDownloadMapRequestData { + optional string map_name = 1; +} + +message AgvDownloadMapResult { + optional int32 ret_code = 1; + optional string create_on = 2; + optional string err_msg = 3; + optional string map_content = 4; +} + +message RobotConfigDownloadMapCommand { + message Request { + CommandHeader.Request header = 1; + RobotConfigDownloadMapRequestData data = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + AgvDownloadMapResult status = 2; + } +} + + +// 单条地图文件信息 +message MapFileInfo { + optional string name = 1; + optional string modified = 2; + optional int64 size = 3; +} + +// 地图查询响应 +message AgvMapStatus { + optional string current_map = 1; + optional string current_map_md5 = 2; + repeated string maps = 3; + repeated MapFileInfo map_files_info = 4; + optional int32 ret_code = 5; + optional string create_on = 6; + optional string err_msg = 7; +} + +// 查询地图状态命令 +message RobotStatusMapCommand { + message Request { + CommandHeader.Request header = 1; + } + message Feedback { + CommandHeader.Feedback header = 1; + AgvMapStatus status = 2; + } +} + + +// ===================== 上传地图 ===================== +message RobotConfigUploadMapRequestData { + optional string map_content = 1; +} + +message AgvUploadMapResult { + optional int32 ret_code = 1; + optional string create_on = 2; + optional string err_msg = 3; +} + +message RobotConfigUploadMapCommand { + message Request { + CommandHeader.Request header = 1; + RobotConfigUploadMapRequestData data = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + AgvUploadMapResult status = 2; + } +} + + +// ===================== 抢占控制权 ====================== +// 抢占控制权 请求参数 +message RobotConfigLockRequestData { + // 控制权抢占者名称，必填 + optional string nick_name = 1; +} + +// 抢占控制权 返回结果 +message AgvLockResult { + // API错误码 + optional int32 ret_code = 1; + // API上传时间戳 + optional string create_on = 2; + // 错误描述信息 + optional string err_msg = 3; +} + +// 抢占控制权 命令封装 +message RobotConfigLockCommand { + message Request { + CommandHeader.Request header = 1; + // 请求业务参数 + RobotConfigLockRequestData data = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + // 抢占控制权响应数据 + AgvLockResult status = 2; + } +} + + + +// ===================== 查询当前控制权所有者 ====================== +// 查询当前控制权所有者响应结构体 +message AgvCurrentLockStatus { + // 当前控制权是否被抢占 + optional bool locked = 1; + // 抢占方IP + optional string ip = 2; + // 抢占方端口 + optional int32 port = 3; + // 抢占者类型 0=default 2=roboshop 0xDD=srd + optional uint32 type = 4; + // 抢占者昵称 + optional string nick_name = 5; + // 抢占时间戳 单位秒 + optional int64 time_t = 6; + // 抢占方描述信息 + optional string desc = 7; + // 接口错误码 + optional int32 ret_code = 8; + // 接口时间戳 + optional string create_on = 9; + // 错误信息 + optional string err_msg = 10; +} + + + +// 查询当前控制权指令 +message RobotStatusCurrentLockCommand { + message Request { + CommandHeader.Request header = 1; + } + message Feedback { + CommandHeader.Feedback header = 1; + AgvCurrentLockStatus status = 2; + } +} + + +//==================== 开环速度运动控制 命令码：2010(0x07DA) ==================== +// 下发指令会强制取消当前正在执行的自动导航任务 +// 多舵轮设备仅解析vx/vy/w；steer、real_steer仅单舵轮设备生效 +// vx：X轴速度 m/s；vy：Y轴速度 m/s；w：旋转角速度 rad/s，逆时针为正、顺时针为负 +// steer：单舵轮档位控制 -2回零 / ±15°增减；real_steer：目标舵角rad，优先级高于steer +// duration：持续时间ms，0=永久运行；缺省读取全局参数ControlMotionDuration(默认500ms) +message RobotMotionControlRequestData { + optional double vx = 1; + optional double vy = 2; + optional double w = 3; + optional double steer = 4; + optional double real_steer = 5; + optional int64 duration = 6; +} + +message RobotMotionControlResult { + optional int32 ret_code = 1; + optional string create_on = 2; + optional string err_msg = 3; +} + +message RobotMotionControlCommand { + message Request { + CommandHeader.Request header = 1; + RobotMotionControlRequestData data = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + RobotMotionControlResult status = 2; + } +} + + +//==================== 切换载入地图 命令码：2022(0x07E6) ==================== +// 切换指定地图，合法地图会更新设备current_map字段 +message RobotLoadMapRequestData { + optional string map_name = 1; // 地图名，仅字母数字-_，不可缺省 +} + +message RobotLoadMapResult { + optional int32 ret_code = 1; + optional string create_on = 2; + optional string err_msg = 3; +} + +message RobotLoadMapCommand { + message Request { + CommandHeader.Request header = 1; + RobotLoadMapRequestData data = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + RobotLoadMapResult status = 2; + } +} + + +//==================== 查询地图载入状态 命令码：1022(0x03FE) ==================== +// loadmap_status:0失败 1成功 2载入中；LOADING状态不可重定位 +message RobotQueryLoadMapStatusResult { + optional int32 loadmap_status = 1; + optional int32 ret_code = 2; + optional string create_on = 3; + optional string err_msg = 4; +} + +message RobotQueryLoadMapStatusCommand { + message Request { + CommandHeader.Request header = 1; + } + message Feedback { + CommandHeader.Feedback header = 1; + RobotQueryLoadMapStatusResult status = 2; + } +} + + + +//==================== 查询当前地图所有站点信息 命令码：1301(0x0515) ==================== +message StationItem { + optional string id = 1; + optional string type = 2; + optional double x = 3; + optional double y = 4; + optional double r = 5; + optional string desc = 6; + optional string executor = 7; + optional string prepoint = 8; + optional string recfile = 9; + optional bool spin = 10; + optional bool use_down_pgv = 11; +} + +message QueryStationListResult { + repeated StationItem stations = 1; + optional int32 ret_code = 2; + optional string create_on = 3; + optional string err_msg = 4; +} + +message QueryStationListCommand { + message Request { + CommandHeader.Request header = 1; + } + message Feedback { + CommandHeader.Feedback header = 1; + QueryStationListResult status = 2; + } +} + +//==================== 指定路径导航 命令码：3066(0x0BFA) ==================== +// 单段导航任务单元 +message MoveTaskItem { + optional string task_id = 1; + optional string source_id = 2; + optional string id = 3; + optional string operation = 4; + optional double jack_height = 5; +} + +// 指定路径导航请求体 +message RobotGoTargetListRequestData { + repeated MoveTaskItem move_task_list = 1; +} + +// 指定路径导航返回结果 +message RobotGoTargetListResult { + optional int32 ret_code = 1; + optional string create_on = 2; + optional string err_msg = 3; +} + +message RobotGoTargetListCommand { + message Request { + CommandHeader.Request header = 1; + RobotGoTargetListRequestData data = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + RobotGoTargetListResult status = 2; + } +} + + +//==================== 查询当前导航状态 robot_status_task_req 1020(0x03FC) 应答0x2B0C ==================== +message QueryCurrentNavReqData { + optional bool simple = 1; +} + +message NavContainerItem { + optional string container_name = 1; + optional string desc = 2; + optional string goods_id = 3; + optional bool has_goods = 4; +} + +message QueryCurrentNavResData { + optional int32 task_status = 1; + optional int32 task_type = 2; + optional string target_id = 3; + repeated double target_point = 4; + repeated string finished_path = 5; + repeated string unfinished_path = 6; + optional string move_status_info = 7; + repeated NavContainerItem containers = 8; + + optional int32 ret_code = 9; + optional string create_on = 10; + optional string err_msg = 11; +} + +message RobotStatusTaskCommand { + message Request { + CommandHeader.Request header = 1; + QueryCurrentNavReqData data = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + QueryCurrentNavResData data = 2; + } +} diff --git a/protos/cmvr/api/agv_service.proto b/protos/cmvr/api/agv_service.proto new file mode 100644 index 00000000..6434904d --- /dev/null +++ b/protos/cmvr/api/agv_service.proto @@ -0,0 +1,60 @@ +syntax = "proto3"; + +import "cmvr/api/agv_command.proto"; + +package cmvr.api; + +service AgvService { + // 基本控制 + rpc GetStatusInfo(GetAgvStatusInfoCommand.Request) returns (GetAgvStatusInfoCommand.Feedback); + + rpc GetBatteryStatus(RobotStatusBatteryCommand.Request) returns (RobotStatusBatteryCommand.Feedback); + + rpc GetRobotLocation(RobotStatusLocCommand.Request) returns (RobotStatusLocCommand.Feedback); + + rpc RobotConfigDownloadMap(RobotConfigDownloadMapCommand.Request) returns (RobotConfigDownloadMapCommand.Feedback); + + + rpc GetMapStatus(RobotStatusMapCommand.Request) returns (RobotStatusMapCommand.Feedback); + + + rpc RobotConfigUploadMap(RobotConfigUploadMapCommand.Request) returns (RobotConfigUploadMapCommand.Feedback); + + // 抢占机器人控制权 + rpc RobotConfigLock(RobotConfigLockCommand.Request) returns (RobotConfigLockCommand.Feedback); + + // 查询当前控制权所有者 + rpc GetCurrentLockStatus(RobotStatusCurrentLockCommand.Request) returns (RobotStatusCurrentLockCommand.Feedback); + + // 开环速度运动控制 + rpc RobotMotionControl(RobotMotionControlCommand.Request) returns (RobotMotionControlCommand.Feedback); + + // 切换载入地图 命令码2022 + rpc RobotLoadMap(RobotLoadMapCommand.Request) returns (RobotLoadMapCommand.Feedback); + + + // 查询地图载入状态 命令码1022 + rpc QueryLoadMapStatus(RobotQueryLoadMapStatusCommand.Request) returns (RobotQueryLoadMapStatusCommand.Feedback); + + + // 查询当前地图站点列表 命令码1301 + rpc QueryStationList(QueryStationListCommand.Request) returns (QueryStationListCommand.Feedback); + + + // 指定路径导航 命令码3066 + rpc RobotGoTargetList(RobotGoTargetListCommand.Request) returns (RobotGoTargetListCommand.Feedback); + + // 查询当前导航状态 1020 robot_status_task_req + rpc RobotStatusTask(RobotStatusTaskCommand.Request) returns (RobotStatusTaskCommand.Feedback); + + + + + + + + + + +} + diff --git a/protos/cmvr/config/agv_config/agv_config.proto b/protos/cmvr/config/agv_config/agv_config.proto new file mode 100644 index 00000000..cb972f1d --- /dev/null +++ b/protos/cmvr/config/agv_config/agv_config.proto @@ -0,0 +1,20 @@ +// camera_config.proto - 所有配置都在这里 +syntax = "proto3"; +package cmvr.config; + + +message AGVsrc1100Config{ + string id = 1; + string ip = 2; + bool enable = 3; + int32 port_status = 4; // 19204 状态查询 + int32 port_control = 5; // 19205 控制API + int32 port_nav = 6; // 19206 导航API + int32 port_config = 7; // 19207 配置API + int32 port_other = 8; // 19210 其他API + int32 port_push = 9; // 19301 推送API +} + +message AGVConfig{ + repeated AGVsrc1100Config src1100_agvs = 1; +}