From b4468809daaa59d3fd1f6998a61e9e89ae248418 Mon Sep 17 00:00:00 2001 From: tankaitao <1767759995@qq.com> Date: Tue, 23 Jun 2026 16:44:03 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/agv_config/agv_config.pb.txt | 12 +- cmvr-es/devices/agv/abstract_agv.h | 891 +++- .../agv/agv_src1100/include/agv_src1100.h | 325 +- .../agv/agv_src1100/src/agv_src1100.cpp | 4405 ++++++++++------- .../service/grpc/include/grpc_agv_service.h | 333 +- cmvr-es/service/grpc/src/grpc_agv_service.cpp | 782 ++- protos/cmvr/api/agv_command.proto | 866 +++- protos/cmvr/api/agv_service.proto | 32 +- 8 files changed, 5190 insertions(+), 2456 deletions(-) diff --git a/cmvr-es/common/config/agv_config/agv_config.pb.txt b/cmvr-es/common/config/agv_config/agv_config.pb.txt index f62d19f2..e9c35fe1 100644 --- a/cmvr-es/common/config/agv_config/agv_config.pb.txt +++ b/cmvr-es/common/config/agv_config/agv_config.pb.txt @@ -1,12 +1,12 @@ 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 + port_status: 19204 + port_control: 19205 + port_nav: 19206 + port_config: 19207 + port_other: 19210 + port_push: 19302 diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index da3c1ec9..858e3973 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -1,6 +1,12 @@ // // Created by xtkuang on 2025/5/6. // +/** + * @file abstract_agv.h + * @brief AGV设备抽象基类定义，提供统一的接口规范和数据结构。 + * 所有具体AGV实现（如SRC-1100）需继承此类。 + * @note 本文件定义了大量与仙工API对应的结构体，用于状态查询、运动控制、导航任务等。 + */ #ifndef CMVR_ES_ABSTRACT_AGV_H #define CMVR_ES_ABSTRACT_AGV_H #pragma once @@ -21,416 +27,841 @@ namespace cmvr::device { +/** + * @brief AGV（自动导引车）设备的抽象基类 + * + * 继承自 AbstractDevice，定义了 AGV 设备通用的接口和行为， + * 包括状态管理、信息查询、运动控制、地图管理、导航任务等功能。 + * 所有具体 AGV 设备实现必须继承此类并实现其纯虚函数。 + */ class AbstractAgv : public AbstractDevice { public: + /** + * @brief AGV 设备运行状态枚举 + */ enum class Status { - CREATED, - INITIALIZED, - RUNNING, - PAUSED, - STOPPED, - FAULT + 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; + // ==================== 状态与信息结构体 ==================== + + /** + * @brief 机器人基本信息查询结果 + * + * 对应仙工 API 1000 (0x03E8) 的响应字段 + */ + struct AgvStatusInfo { + std::string id; ///< 设备唯一标识 + std::string vehicle_id; ///< 车辆编号（如 "agv_001"） + std::string robot_note; ///< 机器人备注（用户自定义） + std::string version; ///< 软件版本号 + std::string model; ///< 车型（如 "SRC-1100"） + std::string dsp_version; ///< DSP 固件版本 + std::string gyro_version; ///< 陀螺仪版本 + std::string map_version; ///< 地图版本 + std::string model_version; ///< 模型版本 + std::string netprotocol_version; ///< 网络协议版本 + std::string modbus_version; ///< Modbus 协议版本 + std::string current_map; ///< 当前加载的地图名称 + std::string current_map_md5; ///< 当前地图的 MD5 校验值 + std::string model_md5; ///< 模型文件 MD5 + std::string ssid; ///< 当前连接的 Wi-Fi SSID + int rssi = 0; ///< Wi-Fi 信号强度 (RSSI, 0~100) + std::string ap_addr; ///< 接入点 MAC 地址 + std::string current_ip; ///< 当前 IP 地址 + std::string mac; ///< 设备 MAC 地址 + std::string echoid_type; ///< 回音 ID 类型 + std::string echoid; ///< 回音 ID + int ret_code = 0; ///< 返回码，0 表示成功，非0 表示错误 + std::string err_msg; ///< 错误信息（失败时填充） + /** + * @brief 检查信息是否有效（ID 非空且返回码为 0） + */ bool isValid() const { return !id.empty() && ret_code == 0; } }; - - // 电池状态信息 + /** + * @brief 电池状态信息 + * + * 对应仙工 API 1007 (0x03EF) 的响应字段 + */ 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; + double battery_level = 0.0; ///< 电池电量百分比 (0~100) + double battery_temp = 0.0; ///< 电池温度（摄氏度） + bool charging = false; ///< 是否正在充电 + double voltage = 0.0; ///< 当前电压（V） + double current = 0.0; ///< 当前电流（A） + double max_charge_voltage = -1.0; ///< 最大充电电压（-1 表示未知） + double max_charge_current = -1.0; ///< 最大充电电流（-1 表示未知） + bool manual_charge = false; ///< 是否连接手动充电器（仅 SRC-2000） + bool auto_charge = false; ///< 是否连接自动充电桩（仅 SRC-2000） + int32_t battery_cycle = 0; ///< 电池循环次数（从 BMS 读取，可能不准确） + std::string battery_user_data; ///< 电池用户自定义数据（字符串） + std::string extra; ///< 额外扩展字段（预留） + int32_t ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳（ISO 8601） + std::string err_msg; ///< 错误信息 }; - - // 机器人位置信息 + /** + * @brief 机器人位置信息 + * + * 对应仙工 API 1004 (0x03EC) 的响应字段 + */ 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; + double x = 0.0; ///< 世界坐标系 X 坐标（米） + double y = 0.0; ///< 世界坐标系 Y 坐标（米） + double angle = 0.0; ///< 朝向角（弧度），逆时针为正 + double confidence = 0.0; ///< 定位置信度（0~1） + std::string current_station; ///< 当前所在站点 ID（若在站点上） + std::string last_station; ///< 上一个经过的站点 ID + int32_t loc_method = 0; ///< 定位方法（0: 自然轮廓, 1: 反光柱, 2: 二维码, 3: 里程计, ...） + int32_t ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; - // 地图下载结果 + /** + * @brief 地图下载结果 + * + * 对应仙工 API 4011 (0x0FAB) 的响应 + */ struct DownloadMapResult { - std::string map_name; - std::string map_content; - int32_t ret_code = 0; - std::string create_on; - std::string err_msg; + std::string map_name; ///< 地图名称 + std::string map_content; ///< 地图文件内容（JSON 字符串） + int32_t ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; - - - // 单地图文件信息 + /** + * @brief 单个地图文件信息 + */ struct MapFileInfo { - std::string name; - std::string modified; - int64_t size = 0; + std::string name; ///< 文件名 + std::string modified; ///< 最后修改时间 + int64_t size = 0; ///< 文件大小（字节） }; - // 地图整体状态 + /** + * @brief 地图整体状态（当前地图、所有地图列表及文件详情） + * + * 对应仙工 API 1300 (0x0514) 的响应 + */ 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; + std::string current_map; ///< 当前加载的地图名称 + std::string current_map_md5; ///< 当前地图的 MD5 + std::vector maps; ///< 所有地图名称列表 + std::vector map_files_info; ///< 每个地图文件的详细信息 + int32_t ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; - - // 上传地图结果 + /** + * @brief 上传地图结果 + * + * 对应仙工 API 4010 (0x0FAA) 的响应 + */ struct UploadMapResult { - int32_t ret_code = 0; - std::string create_on; - std::string err_msg; + int32_t ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; - // 抢占控制权返回结果 + /** + * @brief 抢占控制权返回结果 + * + * 对应仙工 API 4005 (0x0FA5) 的响应 + */ struct LockResult { - int32_t ret_code = 0; - std::string create_on; - std::string err_msg; + int32_t ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; - - // 当前控制权信息 + /** + * @brief 当前控制权信息 + * + * 对应仙工 API 1060 (0x0424) 的响应 + */ 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; + bool locked = false; ///< 是否被锁定（有控制权持有者） + std::string ip; ///< 持有者 IP 地址 + int32_t port = 0; ///< 持有者端口 + uint8_t type = 0; ///< 持有者类型（0: 未知, 1: 调度系统, 2: 示教器, ...） + std::string nick_name; ///< 持有者昵称（自定义标识） + int64_t time_t = 0; ///< 锁定时间戳（Unix 秒） + std::string desc; ///< 描述信息 + int32_t ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; + // ==================== 运动控制相关结构体 ==================== - //==================== 新增：运动控制结构体 ==================== - // 开环运动下发参数结构体 + /** + * @brief 开环运动下发参数 + * + * 对应仙工 API 2010 (0x07DA) 的请求参数 + * 注意：多舵轮设备仅 vx/vy/w 生效，steer/real_steer 仅单舵轮设备有效。 + */ 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 代表缺省，使用设备默认时长 + double vx = 0.0; ///< 机器人坐标系 X 方向线速度（m/s） + double vy = 0.0; ///< 机器人坐标系 Y 方向线速度（m/s） + double w = 0.0; ///< 角速度（rad/s），逆时针为正 + double steer = 0.0; ///< 舵轮转向角（仅单舵轮，单位 rad） + double real_steer = 0.0; ///< 实际舵轮角度（反馈，仅单舵轮） + int64_t duration = -1; ///< 运动持续时间（毫秒），-1 表示一直运动直到下次指令 }; - // 运动控制返回结果结构体 + /** + * @brief 运动控制返回结果 + */ struct MotionCtrlRes { - int ret_code = 0; - std::string create_on; - std::string err_msg; + int ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; - - // 切换地图入参 + /** + * @brief 切换地图入参 + * + * 对应仙工 API 2022 (0x07E6) 的请求 + */ struct LoadMapReq { - std::string map_name; + std::string map_name; ///< 目标地图名称（必须已存在于机器人中） }; - // 切换地图返回结果 + /** + * @brief 切换地图返回结果 + */ struct LoadMapRes { - int ret_code = 0; - std::string create_on; - std::string err_msg; + int ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; - // 查询地图载入状态返回 + /** + * @brief 查询地图载入状态返回 + * + * 对应仙工 API 1022 (0x03FE) 的响应 + */ struct QueryLoadMapStatusRes { - int32_t loadmap_status = 0; //0失败 1成功 2载入中 - int ret_code = 0; - std::string create_on; - std::string err_msg; + int32_t loadmap_status = 0; ///< 地图加载状态：0=失败, 1=成功, 2=载入中 + int ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; - - // 单个站点信息 + /** + * @brief 单个站点信息 + * + * 对应仙工 API 1301 (0x0515) 响应中的单个站点对象 + */ 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; + std::string id; ///< 站点 ID（唯一标识） + std::string type; ///< 站点类型（如 "LocationMark", "ChargePoint", "ActionPoint"） + double x = 0.0; ///< 站点 X 坐标（米） + double y = 0.0; ///< 站点 Y 坐标（米） + double r = 0.0; ///< 站点朝向角（弧度） + std::string desc; ///< 描述信息 + std::string executor; ///< 执行器信息（用于动作） + std::string prepoint; ///< 前置点 ID（若需要先到前置点） + std::string recfile; ///< 识别文件路径（用于视觉识别） + bool spin = false; ///< 是否允许自旋（原地旋转） + bool use_down_pgv = false; ///< 是否使用下视 PGV 二维码 }; - // 查询站点列表返回结果 + /** + * @brief 查询站点列表返回结果 + */ struct QueryStationRes { - std::vector stations; - int ret_code = 0; - std::string create_on; - std::string err_msg; + std::vector stations; ///< 站点列表 + int ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; - - // 单段导航任务 + /** + * @brief 单段导航任务（用于指定路径导航） + * + * 对应仙工 API 3066 (0x0BFA) 中 move_task_list 的每个元素 + */ struct MoveTaskItem { - std::string task_id; - std::string source_id; - std::string id; - std::string operation; - double jack_height = 0.0; + std::string task_id; ///< 任务 ID（必须全局唯一） + std::string source_id; ///< 起始站点 ID + std::string id; ///< 目标站点 ID + std::string operation; ///< 到达后执行的动作（如 "JackLoad", "WaitDI", 等） + double jack_height = 0.0; ///< 顶升高度（当 operation 为顶升相关时使用） }; - // 指定路径导航入参 + /** + * @brief 指定路径导航入参（多个导航段） + * + * 对应仙工 API 3066 的请求数据区 + */ struct GoTargetListReq { - std::vector move_task_list; + std::vector move_task_list; ///< 导航任务序列（必须按顺序且相邻站点间有直接路径） }; - // 指定路径导航返回 + /** + * @brief 指定路径导航返回结果 + */ struct GoTargetListRes { - int ret_code = 0; - std::string create_on; - std::string err_msg; + int ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 }; - - // 导航料箱信息 + // ==================== 导航料箱信息 ==================== + /** + * @brief 导航任务中涉及的料箱信息 + */ struct NavContainerItem { - std::string container_name; - std::string desc; - std::string goods_id; - bool has_goods = false; + std::string container_name; ///< 料箱名称 + std::string desc; ///< 描述 + std::string goods_id; ///< 绑定的货物 ID + bool has_goods = false; ///< 是否已绑定货物 }; - // 1020 查询当前导航入参 - struct RobotStatusTaskReq + // ==================== 1020 实时导航查询 ==================== + /** + * @brief 1020 实时导航查询入参 + * + * 对应仙工 API 1020 (0x03FC) + */ + struct RobotStatusTaskCurrentReq { - bool simple = false; + bool simple = false; ///< 若为 true，只返回 task_status 字段 }; - // 1020 查询当前导航出参 - struct RobotStatusTaskRes + /** + * @brief 1020 实时导航查询出参 + * + * 对应仙工 API 1020 的响应 + */ + struct RobotStatusTaskCurrentRes { - 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 task_status = 0; ///< 任务状态：0=NONE, 1=WAITING, 2=RUNNING, 3=SUSPENDED, 4=COMPLETED, 5=FAILED, 6=CANCELED + int task_type = 0; ///< 任务类型：0=无, 1=自由导航到坐标, 2=自由导航到站点, 3=路径导航到站点, 7=平动转动, 100=其他 + std::string target_id; ///< 目标站点 ID（若 task_type 为 2 或 3） + std::vector target_point; ///< 目标坐标 [x, y, r]（若 task_type 为 1） + std::vector finished_path; ///< 已经过的站点列表 + std::vector unfinished_path; ///< 尚未经过的站点列表 + std::string move_status_info; ///< 导航附加信息（如错误描述） + std::vector containers; ///< 料箱信息（若有） + int ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 + }; + // ==================== 1110 批量任务查询 ==================== + /** + * @brief 单条任务状态项 + */ + struct SingleTaskStatusItem + { + std::string task_id; ///< 任务 ID + int status = 0; ///< 状态码（同上述 task_status） + int type = 0; ///< 任务类型（同上述 task_type） + }; + + /** + * @brief 任务状态包（包含最近完成和所有未完成的任务） + * + * 对应仙工 API 1110 (0x0456) 的响应中的 task_status_package + */ + struct TaskStatusPackage + { + std::string closest_target; ///< 最近站点 ID + std::string source_name; ///< 起点站点名称 + std::string target_name; ///< 终点站点名称 + double percentage = 0.0; ///< 任务进度百分比（0~100） + double distance = 0.0; ///< 剩余距离（米） + std::string info; ///< 附加信息 + std::vector task_status_list; ///< 所有任务状态列表 + }; + + /** + * @brief 1110 批量查询任务入参 + */ + struct QueryTaskStatusPackageReq + { + std::vector task_ids; ///< 要查询的任务 ID 列表，若为空则查询所有未完成+最近一条完成 + }; + + /** + * @brief 1110 批量查询任务出参 + */ + struct QueryTaskStatusPackageRes + { + TaskStatusPackage task_status_package; ///< 任务状态包 + int ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 + }; + + // ==================== DI / DO / 音频 / PGV / 脚本等通用类型 ==================== + /** + * @brief DI 点位信息 + */ + struct DIItem + { + int id = 0; ///< DI 编号 + bool status = false; ///< 状态（true=高电平, false=低电平） + }; + /** + * @brief DO 点位信息 + */ + struct DOItem + { + int id = 0; ///< DO 编号 + bool status = false; ///< 状态（true=高电平, false=低电平） + }; + /** + * @brief 音频播放参数 + */ + struct SoundArgs + { + std::string name; ///< 音频文件名 + int loop = 0; ///< 循环次数（0=播放一次, 1=循环播放） + int stop = 0; ///< 1=停止播放 + }; + /** + * @brief WaitDI 动作参数 + */ + struct WaitDIArgs + { + std::vector DI; ///< 需要等待的 DI 条件列表 + double timeout = 0.0; ///< 超时时间（秒），0 表示无限等待 + }; + /** + * @brief SetDO 动作参数 + */ + struct SetDOArgs + { + std::vector DO; ///< 要设置的 DO 列表 + }; + /** + * @brief PGV（二维码）二次定位参数 + */ + struct PgvParam + { + bool use_pgv = false; ///< 是否使用上视 PGV + bool use_down_pgv = false; ///< 是否使用下视 PGV + double pgv_adjust_dist = 0.0; ///< 最大调整半径（米） + double pgv_adjust_cx = 0.0; ///< 二维码坐标系下的 X 偏移（米） + double pgv_adjust_cy = 0.0; ///< 二维码坐标系下的 Y 偏移（米） + double pgv_x_adjust = 0.0; ///< 仅调整 X 方向误差（米） + }; + /** + * @brief 自由导航目标点（x, y, theta） + */ + struct FreeGoPoint + { + double x = 0.0; ///< X 坐标（米） + double y = 0.0; ///< Y 坐标（米） + double theta = 0.0; ///< 朝向角（弧度） + }; + /** + * @brief 脚本参数（通用） + */ + struct ScriptArgs + { + std::map str_kv; ///< 字符串键值对 + std::map num_kv; ///< 数字键值对 + std::vector do_list; ///< DO 列表（用于 setDO 等） + std::vector di_list; ///< DI 列表（用于 waitDI 等） + }; + + // ==================== 3051 单点导航 ==================== + /** + * @brief 3051 单点导航入参 + * + * 对应仙工 API 3051 (0x0BEB) 的请求数据区 + * + * @attention + * - **仅用于单车测试/验证，严禁用于多车调度场景**，否则会出现速度不连续、不跟随路径等危险。 + * - 下发此任务会取消当前正在执行的任务（不排队）。 + * - 成功下发后会自动清除指定的 warning/error 报错码。 + * - 支持自由导航（freeGo）和基于站点的路径导航两种模式。 + */ + struct RobotGoTargetReq + { + // -------- 必填字段 -------- + std::string source_id; ///< 起始站点 ID（若为 "SELF_POSITION" 表示从当前位置出发） + std::string id; ///< 目标站点 ID（若为 "SELF_POSITION" 表示原地执行 operation） + std::string task_id; ///< 任务 ID（非强制，但建议提供） + + // -------- 可选速度/角度控制 -------- + double angle = 0.0; ///< 目标点朝向角（弧度），仅对自由导航或目标站点有方向要求时使用 + std::string method; ///< 运动方式："forward"（正走）或 "backward"（倒走） + double max_speed = 0.0; ///< 最大线速度（m/s），0 表示使用默认 + double max_wspeed = 0.0; ///< 最大角速度（rad/s） + double max_acc = 0.0; ///< 最大加速度（m/s²） + double max_wacc = 0.0; ///< 最大角加速度（rad/s²） + int64_t duration = 0; ///< 到达后等待时间（毫秒） + int32_t orientation = 0; ///< 全向车保持的角度（暂不使用） + bool spin = false; ///< 是否随动（用于托盘旋转） + int64_t delay = 0; ///< 延迟结束导航状态的时间（毫秒），缺省为 0 + int32_t start_rot_dir = 0; ///< 起步原地旋转方向：-1=顺时针, 0=就近, 1=逆时针 + int32_t end_rot_dir = 0; ///< 到点原地旋转方向（同上） + double reach_dist = 0.0; ///< 到点位置精度（米），默认由系统决定 + double reach_angle = 0.0; ///< 到点角度精度（弧度） + std::string skill_name; ///< 技能名称："Action"（动作）或 "GotoSpecifiedPose"（路径导航） + + // -------- PGV 二次定位 -------- + PgvParam pgv; ///< PGV 参数 + + // -------- 设备操作（顶升/货叉/辊筒/牵引等） -------- + std::string operation; ///< 操作类型，如 "JackLoad", "ForkUnload", "RollerLoad", "HookLoad", "WaitDI", "SetDO", "sound", "Script" 等 + double jack_height = 0.0; ///< 顶升高度（米），用于 JackLoad/JackUnload/JackHeight + double start_height = 0.0; ///< 货叉起步前举升高度（米） + double end_height = 0.0; ///< 货叉到点后举升高度（米） + double fork_mid_height = 0.0; ///< 货叉行走过程中举升高度（米） + double fork_dist = 0.0; ///< 货叉前移距离（米），用于 ForkForward + std::string direction; ///< 方向："left"/"right"/"front"/"back"，用于辊筒或货叉 + bool recognize = false; ///< 是否启用视觉识别（用于货叉/顶升加载） + std::string recfile; ///< 识别文件路径（如 "shelf/s0002.shelf"） + SoundArgs sounds_args; ///< 音频参数（当 operation="sound" 时） + WaitDIArgs wait_di_args; ///< WaitDI 参数 + SetDOArgs set_do_args; ///< SetDO 参数 + std::string script_name; ///< 脚本文件名（当 operation="Script" 时） + ScriptArgs script_args; ///< 脚本参数 + int32_t script_stage = 2; ///< 脚本执行阶段：0=导航前, 1=导航中, 2=导航后, 3=脚本控制导航 + + // -------- 原地旋转（特殊用法） -------- + double move_angle = 0.0; ///< 旋转角度（弧度），仅当 skill_name="GoByOdometer" 时使用 + double speed_w = 0.0; ///< 旋转角速度（rad/s） + int32_t loc_mode = 0; ///< 定位模式：1=激光定位, 0=里程定位 + + // -------- 自由导航（仅支持双轮差速底盘） -------- + FreeGoPoint freeGo; ///< 自由导航目标坐标（若提供此字段，则忽略 id 字段，使用坐标导航） + }; + + /** + * @brief 3051 单点导航出参 + */ + struct RobotGoTargetRes + { + int ret_code = 0; ///< 返回码，0 表示成功 + std::string create_on; ///< 数据创建时间戳 + std::string err_msg; ///< 错误信息 + }; + + //新增暂停导航响应结构体 + struct RobotTaskPauseRes { int ret_code = 0; std::string create_on; std::string err_msg; }; + // 继续导航响应结构体 + struct RobotTaskResumeRes { + int ret_code = 0; + std::string create_on; + std::string err_msg; + }; + // ==================== robot_control_stop 停止开环运动响应 ==================== + // 0x07D0 robot_control_stop 停止开环运动返回 + struct RobotControlStopRes + { + int ret_code = 0; + std::string create_on; + std::string err_msg; + }; + // 取消导航响应结构体 + struct RobotTaskCancelRes { + 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; - // 纯虚接口 + // ==================== 纯虚接口 ==================== + + /** + * @brief 获取当前设备运行状态 + * @return Status 枚举值 + */ virtual Status state() const = 0; + + /** + * @brief 获取最后一次发生的错误信息 + * @return 错误描述字符串 + */ virtual std::string lastError() const = 0; - + /** + * @brief 启动设备（建立连接、初始化资源等） + */ void start() override = 0; + + /** + * @brief 停止设备（断开连接、释放资源等） + */ void stop() override = 0; + + /** + * @brief 周期性更新设备状态（由上层循环调用，可处理心跳、重连等） + */ void update() override = 0; + /** + * @brief 获取 AGV 状态（封装了 getStatusInfo） + * @param state 输出参数，填充 AgvState 结构 + */ virtual void getState(AgvState& state) { state = AgvState{}; getStatusInfo(state); } + /** + * @brief 获取 AGV 基本信息（命令码 1000） + * @param info 输出参数，填充 AgvStatusInfo + */ virtual void getStatusInfo(AgvStatusInfo& info) = 0; - // 查询电池状态 + /** + * @brief 查询电池状态（命令码 1007） + * @param info 输出参数，填充 BatteryStatus + * @param simple 若为 true，只查询关键信息（可提高效率，具体取决于实现） + */ virtual void getBatteryStatus(BatteryStatus& info, bool simple = false) = 0; - // 查询机器人位置 + /** + * @brief 查询机器人当前位置（命令码 1004） + * @param info 输出参数，填充 RobotLocation + */ virtual void getRobotLocation(RobotLocation& info) = 0; - // 下载地图 + /** + * @brief 下载指定名称的地图（命令码 4011） + * @param info 输出参数，填充 DownloadMapResult（包含地图内容） + * @param map_name 要下载的地图名称 + */ virtual void downloadMap(DownloadMapResult& info, const std::string& map_name) = 0; - - // 查询地图状态 + /** + * @brief 查询地图状态（命令码 1300） + * @param info 输出参数，填充 MapStatus（当前地图、所有地图列表及文件信息） + */ virtual void getMapStatus(MapStatus& info) = 0; - // 上传地图 + /** + * @brief 上传地图（命令码 4010） + * @param info 输出参数，填充 UploadMapResult + * @param map_json 地图 JSON 字符串（完整的地图文件内容） + */ virtual void uploadMap(UploadMapResult& info, const std::string& map_json) = 0; /** - * @brief 抢占机器人控制权 - * @param info 出参，抢占结果 - * @param nick_name 入参，抢占者名称 + * @brief 抢占机器人控制权（命令码 4005） + * @param info 输出参数，填充 LockResult + * @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生效 + * @brief 下发开环速度运动指令（命令码 2010） + * + * 使用控制端口（19205）。下发后会强制中断当前自动导航任务。 + * 对于多舵轮设备，仅 vx/vy/w 生效。 + * + * @param res 输出参数，运动指令返回结果 + * @param req 输入参数，包含 vx/vy/w/steer/real_steer/duration */ virtual void robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) = 0; /** - * @brief 切换载入地图 命令码2022(0x07E6)，控制端口sock_control_ - * @param res 出参 执行结果 - * @param req 入参 目标地图名称 - */ + * @brief 切换载入地图（命令码 2022） + * + * 使用控制端口（19205）。目标地图必须已在机器人中存储。 + * + * @param res 输出参数，执行结果 + * @param req 输入参数，目标地图名称 + */ virtual void robotLoadMap(LoadMapRes& res, const LoadMapReq& req) = 0; - + /** + * @brief 查询当前导航状态（命令码 1020） + * + * 使用状态端口（19204）。 + * @param res 输出参数，导航全量状态 + * @param req 输入参数，simple=true 仅返回 task_status + */ + virtual void robotStatusTaskCurrent(RobotStatusTaskCurrentRes& res, const RobotStatusTaskCurrentReq& 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; - - - + * @brief 简单运动控制（设置速度） + * + * 默认实现抛出 logic_error，子类可重写。 + * @param vx X 方向速度 (m/s) + * @param vy Y 方向速度 (m/s) + * @param vw 角速度 (rad/s) + */ virtual void move(double vx, double vy, double vw) { throw std::logic_error("move not implemented"); } + /** + * @brief 停止运动 + * + * 默认实现抛出 logic_error，子类可重写。 + */ virtual void stopMove() { throw std::logic_error("stopMove not implemented"); } /** - * @brief 查询当前机器人控制权持有者 - * @param info 出参 控制权信息 + * @brief 查询当前机器人控制权持有者（命令码 1060） + * @param info 输出参数，填充 CurrentLockStatus */ virtual void getCurrentLockStatus(CurrentLockStatus& info) = 0; /** - * @brief 查询地图载入状态 命令码1022(0x03FE)，状态端口sock_status_ - * @param res 出参 地图加载状态结果 - * @note loadmap_status=2载入中禁止重定位 + * @brief 查询地图载入状态（命令码 1022） + * + * 使用状态端口（19204）。 + * @param res 输出参数，包含 loadmap_status（0=失败, 1=成功, 2=载入中） + * @note 载入中（2）时禁止执行重定位操作 */ virtual void queryLoadMapStatus(QueryLoadMapStatusRes& res) = 0; - /** - * @brief 查询当前地图全部站点信息 命令码1301(0x0515)，状态端口sock_status_ - * @param res 出参 站点数组与执行结果 + * @brief 查询当前地图全部站点信息（命令码 1301） + * + * 使用状态端口（19204）。 + * @param res 输出参数，包含站点数组与执行结果 */ virtual void queryStationList(QueryStationRes& res) = 0; - /** - * @brief 指定路径导航 命令码3066(0x0BFA)，控制端口sock_control_ - * @param res 出参 下发任务结果 - * @param req 入参 多段站点导航序列 - * @note task_id、source_id、id 必填；任务串行追加执行 + * @brief 指定路径导航（命令码 3066） + * + * 使用导航端口（19206）。任务串行追加执行。 + * + * @param res 输出参数，下发任务结果（ret_code=0 仅表示接收成功，不代表执行完成） + * @param req 输入参数，多段站点导航序列（每个任务必须包含 task_id, source_id, id） + * @attention + * - source_id 和 id 之间必须有直接相连的线路，不可以跳点。 + * - 任务会排队执行，若前一个任务失败，后续任务自动取消。 + * - 适合多车调度场景，因为路径可预知、不会自主绕路。 */ virtual void robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) = 0; + /** + * @brief 批量查询任务状态（命令码 1110） + * + * 使用状态端口（19204）。 + * @param res 输出参数，包含所有任务状态包（已完成+未完成） + * @param req 输入参数，要查询的 task_id 列表；若为空，则查询所有未完成+最近一条完成的任务 + */ + virtual void robotStatusTaskPackage(QueryTaskStatusPackageRes& res, const QueryTaskStatusPackageReq& req) = 0; + /** + * @brief 单点站点自动规划导航（命令码 3051） + * + * 使用导航端口（19206）。 + * @param res 输出参数，下发结果（ret_code=0 仅表示接收成功） + * @param req 完整导航参数（支持自由导航、固定路径导航、带设备操作等） + * @warning + * - **严禁用于多车调度场景**，仅限单车测试/任务链验证。 + * - 下发新任务会取消当前正在执行的任务（不排队）。 + * - 成功下发后会**自动清除**指定的 warning/error 报错码（详见协议文档）。 + * - 支持自由导航（freeGo），但仅限双轮差速底盘。 + */ + virtual void robotGoTarget(RobotGoTargetRes& res, const RobotGoTargetReq& req) = 0; + + + /** + * @brief 2000(0x07D0) robot_control_stop_req 停止开环速度运动指令 + * @param res 出参，执行结果 + */ + virtual void robotControlStop(RobotControlStopRes& res) = 0; + + + /** + * @brief 暂停当前导航任务（命令码 3001, 0x0BB9） + * @param res 输出参数，执行结果 + */ + virtual void robotTaskPause(RobotTaskPauseRes& res) = 0; + + + /** + * @brief 继续当前导航任务（命令码 3002, 0x0BBA） + * @param res 输出参数，执行结果 + */ + virtual void robotTaskResume(RobotTaskResumeRes& res) = 0; + + + /** + * @brief 取消当前导航任务（命令码 3003, 0x0BBB） + * @param res 输出参数，执行结果 + */ + virtual void robotTaskCancel(RobotTaskCancelRes& res) = 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}; + // ==================== 保护成员变量 ==================== + std::string id_; ///< 设备 ID（唯一标识） + std::string ip_; ///< 设备 IP 地址 + + // 多端口配置（对应 proto 定义） + int port_status_{0}; ///< 状态查询端口（默认 19204） + int port_control_{0}; ///< 控制 API 端口（默认 19205） + int port_nav_{0}; ///< 导航 API 端口（默认 19206） + int port_config_{0}; ///< 配置 API 端口（默认 19207） + int port_other_{0}; ///< 其他 API 端口（默认 19210） + int port_push_{0}; ///< 推送端口（默认 19301） + + bool enable_{false}; ///< 设备启用标志 }; } // namespace cmvr::device diff --git a/cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h b/cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h index 847dcfed..13ab8d32 100644 --- a/cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h +++ b/cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h @@ -1,6 +1,11 @@ // // Created by linbo on 2025/6/20. // +/** + * @file agv_src1100.h + * @brief 仙工智能 SRC-1100/2200 系列 AGV 控制器设备实现类的头文件。 + * 继承自 AbstractAgv，实现了所有纯虚接口，并管理多端口 TCP 连接。 + */ #ifndef AGV_SRC1100_H #define AGV_SRC1100_H #pragma once @@ -10,78 +15,292 @@ #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; +/** + * @brief 仙工智能 SRC-1100/2200 系列 AGV 控制器设备实现类 + * + * 继承自 AbstractAgv，实现了与仙工智能 SRC 系列控制器通信的具体协议。 + * 支持多端口（状态、控制、导航、配置、推送）并发连接和指令交互。 + * + * @note 该类不是线程安全的，外部调用需自行保证同一设备实例的串行访问。 + */ +class AgvSrc1100 final : public AbstractAgv { +public: + // ==================== 构造 / 析构 ==================== - Status state() const override; - std::string lastError() const override; + /** + * @brief 从 XML 配置节点构造设备实例（已弃用或未使用，保留兼容） + * @param cfg XML 配置节点，包含设备 ID、IP、端口、启用标志等 + */ + explicit AgvSrc1100(const XmlNode& cfg); - void start() override; - void stop() override; - void update() override; + /** + * @brief 从 Protobuf 配置对象构造设备实例 + * @param cfg AGVsrc1100Config 配置对象，包含设备 ID、IP、端口、启用标志等 + */ + explicit AgvSrc1100(const config::AGVsrc1100Config& cfg); - 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; + /** + * @brief 析构函数，自动停止设备并释放资源 + */ + ~AgvSrc1100() override; - void robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) override; + // ==================== 生命周期管理（通用） ==================== - void robotLoadMap(LoadMapRes& res, const LoadMapReq& req) override; + /** + * @brief 获取当前设备运行状态 + * @return Status 枚举值（CREATED / INITIALIZED / RUNNING / PAUSED / STOPPED / FAULT） + */ + Status state() const override; - void queryLoadMapStatus(QueryLoadMapStatusRes& res) override; - void queryStationList(QueryStationRes& res) override; + /** + * @brief 获取最后一次发生的错误信息 + * @return 错误描述字符串，若无错误则返回空字符串 + */ + std::string lastError() const override; - void robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) override; + /** + * @brief 启动设备，标记为运行状态 + * @note 所有 socket 连接应在构造时已完成，启动仅改变状态标志 + */ + void start() override; + + /** + * @brief 停止设备，关闭所有 TCP 连接并释放资源 + */ + void stop() override; + + /** + * @brief 周期性更新设备状态 + * @note 当前为空实现，可扩展心跳检测或重连逻辑 + */ + void update() override; + + // ==================== 端口 19204 – 机器人状态 API（允许 10 个连接） ==================== + // 功能：查询机器人各种状态信息（只读操作，不改变机器人状态） + + /** + * @brief 查询机器人基本信息（命令码 1000, robot_status_info_req） + * @param info 输出参数，填充 AgvStatusInfo + * @note 返回信息包括：版本、型号、地图名称、网络 IP、MAC、Wi-Fi 信号等 + */ + void getStatusInfo(AgvStatusInfo& info) override; + + /** + * @brief 查询电池状态（命令码 1007, robot_status_battery_req） + * @param info 输出参数，填充 BatteryStatus + * @param simple 若为 true，仅返回关键电量信息（当前实现未区分） + * @note 返回信息包括：电量百分比、温度、充放电状态、电压、电流等 + */ + void getBatteryStatus(BatteryStatus& info, bool simple = false) override; + + /** + * @brief 查询机器人当前位置（命令码 1004, robot_status_loc_req） + * @param info 输出参数，填充 RobotLocation + * @note 返回信息包括：世界坐标系 X/Y 坐标、朝向角、定位置信度、当前站点 + */ + void getRobotLocation(RobotLocation& info) override; + + /** + * @brief 查询地图状态（命令码 1300, robot_status_map_req） + * @param info 输出参数，填充 MapStatus + * @note 返回信息包括：当前载入的地图名称、所有存储的地图列表、文件详情 + */ + void getMapStatus(MapStatus& info) override; + + /** + * @brief 查询当前控制权持有者（命令码 1060, robot_status_current_lock_req） + * @param info 输出参数，填充 CurrentLockStatus + * @note 返回信息包括：是否被锁定、持有者 IP/端口/昵称、锁定时间等 + */ + void getCurrentLockStatus(CurrentLockStatus& info) override; + + /** + * @brief 查询地图载入状态（命令码 1022, robot_status_loadmap_req） + * @param res 输出参数，包含 loadmap_status + * @note loadmap_status: 0=失败, 1=成功, 2=载入中（载入中禁止重定位） + */ + void queryLoadMapStatus(QueryLoadMapStatusRes& res) override; + + /** + * @brief 查询当前地图站点列表（命令码 1301, robot_status_station_req） + * @param res 输出参数，填充 QueryStationRes，包含所有站点信息 + * @note 返回信息包括：站点 ID、类型、坐标、朝向角、属性等 + */ + void queryStationList(QueryStationRes& res) override; + + /** + * @brief 查询当前导航状态（命令码 1020, robot_status_task_req） + * @param res 输出参数，填充 RobotStatusTaskCurrentRes + * @param req 输入参数，simple=true 时只返回 task_status + * @note 返回信息包括：任务状态、任务类型、目标站点/坐标、已走/未走路径 + */ + void robotStatusTaskCurrent(RobotStatusTaskCurrentRes& res, const RobotStatusTaskCurrentReq& req) override; + + /** + * @brief 批量查询任务状态（命令码 1110, robot_status_task_status_package_req） + * @param res 输出参数，填充 QueryTaskStatusPackageRes + * @param req 输入参数，task_ids 列表；若为空则查询所有未完成 + 最近一条已完成的任务 + * @note 返回信息包括：任务列表、每个任务的 ID/状态/类型、进度百分比、剩余距离等 + */ + void robotStatusTaskPackage(QueryTaskStatusPackageRes& res, const QueryTaskStatusPackageReq& req) override; + + // ==================== 端口 19205 – 机器人控制 API（允许 5 个连接） ==================== + // 功能：下发控制指令，改变机器人运动或状态（非导航类指令） + + /** + * @brief 下发开环速度运动指令（命令码 2010, robot_control_motion_req） + * @param res 输出参数，执行结果 + * @param req 输入参数，包含 vx/vy/w/steer/real_steer/duration + * @warning 下发此指令会立即取消当前正在执行的导航任务 + * @note 多舵轮设备仅 vx/vy/w 生效，steer/real_steer 仅单舵轮设备有效 + */ + void robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) override; + + /** + * @brief 切换载入地图（命令码 2022, robot_control_loadmap_req） + * @param res 输出参数，执行结果 + * @param req 输入参数，目标地图名称 + * @note 目标地图必须已存在于机器人中，否则切换失败 + */ + void robotLoadMap(LoadMapRes& res, const LoadMapReq& req) override; + + // ==================== 端口 19206 – 机器人导航 API（允许 5 个连接） ==================== + // 功能：下发导航任务 + + /** + * @brief 指定路径导航（命令码 3066, robot_task_gotargetlist_req） + * @param res 输出参数，下发结果（ret_code=0 仅表示指令被接收，不表示执行完成） + * @param req 输入参数，包含 move_task_list（站点序列） + * @attention + * - 每个任务必须含 task_id、source_id、id 三个必填字段 + * - source_id 和 id 之间必须有直接相连的线路，不可跳点 + * - 任务会排队执行，前一个任务失败时后续任务自动取消 + * - 适合多车调度场景 + */ + void robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) override; + + /** + * @brief 单点站点自动规划导航（命令码 3051, robot_task_gotarget_req） + * @param res 输出参数，下发结果（ret_code=0 仅表示指令被接收） + * @param req 输入参数，支持自由导航（freeGo）和基于站点的路径导航两种模式 + * @warning + * - **严禁用于多车调度场景**，仅限单车测试/任务链验证 + * - 下发新任务会取消当前正在执行的任务（不排队） + * - 成功下发后会自动清除指定的 warning/error 报错码 + * - 自由导航（freeGo）仅支持双轮差速底盘 + */ + void robotGoTarget(RobotGoTargetRes& res, const RobotGoTargetReq& req) override; + + // ==================== 端口 19207 – 机器人配置 API（允许 5 个连接） ==================== + // 功能：配置类操作（地图上传/下载、控制权管理、参数修改等） + + /** + * @brief 抢占机器人控制权（命令码 4005, robot_config_lock_req） + * @param info 输出参数，填充 LockResult + * @param nick_name 抢占者昵称，用于标识调用方 + * @note 抢占成功后调用方获得独占控制权，其他方仅可查询状态 + */ + void lockRobotControl(LockResult& info, const std::string& nick_name) override; + + /** + * @brief 上传地图（命令码 4010, robot_config_uploadmap_req） + * @param info 输出参数，填充 UploadMapResult + * @param map_json 地图 JSON 字符串（完整的地图文件内容） + * @note 地图数据较大时会自动分片发送，需确保 JSON 格式正确 + */ + void uploadMap(UploadMapResult& info, const std::string& map_json) override; + + /** + * @brief 下载指定地图（命令码 4011, robot_config_downloadmap_req） + * @param info 输出参数，填充 DownloadMapResult（包含地图 JSON 内容） + * @param map_name 要下载的地图名称 + * @note 下载的地图内容以 JSON 字符串形式存储在 info.map_content 中 + */ + void downloadMap(DownloadMapResult& info, const std::string& map_name) override; + + // ==================== 端口 19210 – 其他 API（允许 5 个连接） ==================== + // 功能：外设控制（顶升/货叉/辊筒/音频/IO 等） + // 当前未实现具体方法，预留扩展 + + // ==================== 端口 19301 – 机器人推送 API（允许 10 个连接） ==================== + // 功能：接收机器人主动推送的实时状态数据 + // 相关配置方法（如 9300）可能在此，当前未实现 - void robotStatusTask(RobotStatusTaskRes& res, const RobotStatusTaskReq& req) override; + // ---------- 新增任务控制接口（3001/3002/3003）及停止运动（2000） ---------- + void robotTaskPause(RobotTaskPauseRes& res) override; + void robotTaskResume(RobotTaskResumeRes& res) override; + void robotTaskCancel(RobotTaskCancelRes& res) override; + void robotControlStop(RobotControlStopRes& res) override; +private: + // ==================== 私有通信辅助函数 ==================== + + /** + * @brief 异步连接指定端口 + * @param sock 输出参数，连接成功后存储 socket 文件描述符 + * @param port 目标端口号（19204/19205/19206/19207/19210/19301） + * @note 在独立线程中执行，连接成功后将 sock 设置为有效值 + */ + void asyncConnect(int& sock, int port); + + /** + * @brief 发送请求并接收完整响应 + * @param sock 已连接的 socket 文件描述符 + * @param header 16 字节协议帧头 + * @return true 表示收发成功，current_json_ 中存储响应 JSON；false 表示失败 + * @note 内部先发送帧头，然后循环接收直到超时，最后提取 JSON 部分 + */ + bool sendAndRecv(int sock, const uint8_t* header); + + /** + * @brief 清空 socket 接收缓冲区中的残留数据 + * @param sock 目标 socket 文件描述符 + * @note 采用非阻塞方式读取并丢弃所有可读数据，防止粘包干扰 + */ + void flushSocket(int sock); + + /** + * @brief 设置 socket 接收超时时间 + * @param sock 目标 socket 文件描述符 + * @param timeout_ms 超时时间（毫秒），0 表示取消超时 + */ + void setSocketTimeout(int sock, int timeout_ms); + + /** + * @brief 解析 JSON 字符串并做异常捕获 + * @param json_str 输入的 JSON 字符串 + * @param root 输出参数，解析后的 json 对象 + * @param err_msg 输出参数，解析失败时的错误信息 + * @return true 表示解析成功，false 表示解析失败 + */ + 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_; ///< 最近一次响应的 JSON 内容（由 sendAndRecv 填充） + // ---------- 各端口 Socket 文件描述符 ---------- + int sock_status_ = -1; ///< 19204 – 状态 API 端口 + int sock_control_ = -1; ///< 19205 – 控制 API 端口 + int sock_nav_ = -1; ///< 19206 – 导航 API 端口 + int sock_config_ = -1; ///< 19207 – 配置 API 端口 + int sock_other_ = -1; ///< 19210 – 其他 API 端口 + int sock_push_ = -1; ///< 19301 – 推送 API 端口 - 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_; - }; + std::vector connect_threads_; ///< 各端口异步连接线程 + mutable std::mutex connect_mutex_; ///< 保护 connect_threads_ 的互斥锁 +}; } // namespace cmvr::device diff --git a/cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp b/cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp index f657eb1a..558e621b 100644 --- a/cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp +++ b/cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp @@ -1,3 +1,15 @@ +// ============================================================================ +// 文件名: agv_src1100.cpp +// 描述: 仙工智能 SRC-1100/2200 系列 AGV 控制器设备实现 +// 端口分类: +// 19204 - 机器人状态 API（查询位置/电池/任务/地图/控制权等） +// 19205 - 机器人控制 API（开环运动/切换地图等） +// 19206 - 机器人导航 API（路径导航/指定路径导航等） +// 19207 - 机器人配置 API（地图上传下载/控制权抢占等） +// 19210 - 其他 API（外设控制/音频/IO 等）【未实现】 +// 19301 - 机器人推送 API（主动状态推送）【未实现】 +// ============================================================================ + #include "../include/agv_src1100.h" #include #include @@ -10,1821 +22,2750 @@ // #include "json/json.h" #include "nlohmann/json.hpp" using json = nlohmann::json; -namespace cmvr::device { +namespace cmvr::device +{ + // ============================================================================ + // 构造函数 / 析构函数 + // ============================================================================ -// ====================================================================== -// 构造函数（多端口版本） -// ====================================================================== + /** + * @brief 从 Protobuf 配置对象构造设备实例 + * @param cfg AGVsrc1100Config 配置对象 + * @note 连接所有端口：19204/19205/19206/19207/19210/19301 + */ + 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(); // 设备 ID + ip_ = cfg.ip(); // 设备 IP 地址 + enable_ = cfg.enable(); // 是否启用 -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(); // 19204 – 状态查询 + port_control_ = cfg.port_control(); // 19205 – 控制指令 + port_nav_ = cfg.port_nav(); // 19206 – 导航任务 + port_config_ = cfg.port_config(); // 19207 – 配置操作 + port_other_ = cfg.port_other(); // 19210 – 其他外设 + port_push_ = cfg.port_push(); // 19301 – 数据推送 - 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_); + } - 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_); + LOG(INFO) << "port_status=" << port_status_ + << ", port_control=" << port_control_ + << ", port_nav=" << port_nav_ + << ", port_config=" << port_config_ + << ", port_other=" << port_other_ + << ", port_push=" << port_push_; } -} -AgvSrc1100::~AgvSrc1100() { - stop(); - for (auto& t : connect_threads_) { - if (t.joinable()) t.join(); + /** + * @brief 析构函数：停止设备并等待所有连接线程结束 + */ + AgvSrc1100::~AgvSrc1100() { + stop(); // 关闭所有 socket + // 等待所有异步连接线程结束 + 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_; -} + /** + * @brief 获取当前设备运行状态 + * @return Status 枚举值 + */ + AbstractAgv::Status AgvSrc1100::state() const { + return status_; + } -// ====================================================================== -// 通用异步连接 -// ====================================================================== -void AgvSrc1100::asyncConnect(int& sock, int port) { - int s = socket(AF_INET, SOCK_STREAM, 0); - if (s < 0) { + /** + * @brief 获取最后一次发生的错误信息 + * @return 错误描述字符串 + */ + std::string AgvSrc1100::lastError() const { std::lock_guard lock(mutex_); - last_error_ = "socket create failed"; - status_ = Status::FAULT; - return; + return last_error_; } - 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; + /** + * @brief 启动设备：标记为运行状态 + * @note socket 连接已在构造时完成，此处仅改变状态标志 + */ + void AgvSrc1100::start() { + status_ = Status::RUNNING; } - { - std::lock_guard lock(mutex_); - sock = s; - status_ = Status::INITIALIZED; + /** + * @brief 停止设备：关闭所有 TCP 连接并释放资源 + */ + void AgvSrc1100::stop() { + // 定义 lambda 关闭单个 socket 并置为 -1 + auto closeSocket = [](int& s) { + if (s >= 0) { close(s); s = -1; } + }; + // 关闭所有端口的 socket + closeSocket(sock_status_); + closeSocket(sock_control_); + closeSocket(sock_nav_); + closeSocket(sock_config_); + closeSocket(sock_other_); + closeSocket(sock_push_); + status_ = Status::STOPPED; } -} -void AgvSrc1100::start() { - status_ = Status::RUNNING; -} + /** + * @brief 周期性更新设备状态 + * @note 当前为空实现，预留扩展（如心跳检测、自动重连等） + */ + void AgvSrc1100::update() {} -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() {} + /** + * @brief 清空 socket 接收缓冲区残留数据，防止粘包 + * @param sock 目标 socket + * @note 非阻塞读取并丢弃所有可读数据 + */ + void AgvSrc1100::flushSocket(int sock) { + uint8_t tmp[128]; // 临时缓冲区 + ssize_t rlen; + // MSG_DONTWAIT: 非阻塞模式，读到没有数据时立即返回 + while ((rlen = recv(sock, tmp, sizeof(tmp), MSG_DONTWAIT)) > 0); + } -void AgvSrc1100::flushSocket(int sock) { - uint8_t tmp[128]; - ssize_t rlen; - while ((rlen = recv(sock, tmp, sizeof(tmp), MSG_DONTWAIT)) > 0); -} + /** + * @brief 设置 socket 接收超时时间 + * @param sock 目标 socket + * @param timeout_ms 超时时间（毫秒），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; // 微秒 + // SO_RCVTIMEO: 设置接收超时 + setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + } -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)); -} + /** + * @brief 异步连接指定端口 + * @param sock 输出参数，连接成功后的 socket 描述符 + * @param port 目标端口号 + * @note 在独立线程中执行，连接失败时设置错误状态 + */ + void AgvSrc1100::asyncConnect(int& sock, int port) { + // 创建 TCP socket + 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); // IP 地址转换 + + // 发起 TCP 连接 + if (connect(s, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + close(s); // 连接失败，关闭 socket + std::lock_guard lock(mutex_); + last_error_ = "connect failed port:" + std::to_string(port); + status_ = Status::FAULT; + return; + } + + // 连接成功，保存 socket 描述符 + { + std::lock_guard lock(mutex_); + sock = s; + status_ = Status::INITIALIZED; + } + } + + /** + * @brief 发送请求并接收完整响应 + * @param sock 已连接的 socket + * @param header 16 字节协议帧头（包含命令码等） + * @return true 表示成功，current_json_ 存储响应 JSON + * @note 适用于无负载或简单查询命令（如 1000/1004/1007） + */ bool AgvSrc1100::sendAndRecv(int sock, const uint8_t* header) { - std::lock_guard lock(mutex_); + // 加锁，防止多个线程同时操作 current_json_ + 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 (sock < 0) { + last_error_ = "socket not connected"; + return false; } - } - // 解析地图文件详情数组 - 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); + // 清空残留数据，防止读取到上一次的响应 + flushSocket(sock); + + // 发送 16 字节协议帧头 + if (send(sock, header, 16, MSG_NOSIGNAL) != 16) { + last_error_ = "send header failed"; + return false; } - } -} + // 设置接收超时 100ms，避免无限阻塞 + setSocketTimeout(sock, 100); -// =========================================================================== -// 接口：上传地图 → 命令码 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; - } + const int RECV_BUF_LEN = 16384; + uint8_t recvBuf[RECV_BUF_LEN] = {0}; + int totalRecv = 0; - // 清空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) + // 循环接收数据，直到读空或缓冲区满 + while (totalRecv < RECV_BUF_LEN) { - usleep(10000); - continue; + int ret = recv(sock, recvBuf + totalRecv, RECV_BUF_LEN - totalRecv, 0); + if (ret <= 0) break; // 读空或超时，退出循环 + totalRecv += ret; } - totalSent += ret; - } - if (totalSent != 16) - { + + // 取消超时设置 setSocketTimeout(sock, 0); - info.ret_code = -2; - info.err_msg = "send frame header failed"; - return; + + if (totalRecv <= 0) { + last_error_ = "no data received"; + return false; + } + + // 将接收到的数据转为字符串 + std::string rawData(reinterpret_cast(recvBuf), totalRecv); + // 查找 JSON 起始和结束位置 + size_t jsonStart = rawData.find('{'); + size_t jsonEnd = rawData.rfind('}'); + + // 提取 JSON 内容，兜底处理不完整数据 + 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数据，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) + /** + * @brief 解析 JSON 字符串并做异常捕获 + * @param json_str 输入的 JSON 字符串 + * @param root 输出参数，解析后的 json 对象 + * @param err_msg 输出参数，解析失败时的错误信息 + * @return true 表示解析成功，false 表示失败 + */ + bool AgvSrc1100::parseJson(const std::string& json_str, json& root, std::string& err_msg) { - int sendLen = std::min((size_t)SLICE_SIZE, totalSize - offset); - int ret = send(sock, data + offset, sendLen, 0); - if (ret <= 0) + root.clear(); + try + { + root = json::parse(json_str); // 使用 nlohmann/json 解析 + } + catch (json::parse_error& e) // 捕获解析异常 + { + err_msg = "JSON parse error: " + std::string(e.what()); + return false; + } + return true; + } + + // ============================================================================ + // 端口 19204 – 机器人状态 API（允许 10 个连接） + // 功能：查询机器人各种状态信息（只读操作） + // ============================================================================ + + // -------------------------------------------------------------------------- + // 1000 - robot_status_info_req 查询机器人基本信息 + // -------------------------------------------------------------------------- + void AgvSrc1100::getStatusInfo(AgvStatusInfo& info) { + int sock = sock_status_; + if (sock < 0) { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 构造 16 字节协议帧头 + // 格式: 0x5A 0x01 0x00 0x01 + 长度(4字节) + 命令码(2字节) + 保留(6字节) + // 命令码 0x03E8 = 1000 + 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; + } + + // 从成员变量中读取响应 JSON + 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 + 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 - robot_status_battery_req 查询电池状态 + // -------------------------------------------------------------------------- + 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; + } + + // 命令码 0x03EF = 1007 + 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; + } + + 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; + } + + // 提取电池相关字段 + 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 - robot_status_loc_req 查询机器人位置 + // -------------------------------------------------------------------------- + void AgvSrc1100::getRobotLocation(RobotLocation& info) + { + int sock = sock_status_; + if (sock < 0) + { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 命令码 0x03EC = 1004 + 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; + } + + 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; + } + + // 提取位置相关字段 + 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", ""); + } + + + // -------------------------------------------------------------------------- + // 1300 - robot_status_map_req 查询地图状态 + // -------------------------------------------------------------------------- + void AgvSrc1100::getMapStatus(MapStatus& info) + { + int sock = sock_status_; + if (sock < 0) + { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 命令码 0x0514 = 1300 + 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; + } + + 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; + } + + // 提取地图字段 + 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); + } + } + } + + + // -------------------------------------------------------------------------- + // 1022 - robot_status_loadmap_req 查询地图载入状态 + // -------------------------------------------------------------------------- + void AgvSrc1100::queryLoadMapStatus(QueryLoadMapStatusRes& res) + { + int sock = sock_status_; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "socket not connected"; + return; + } + + // 命令码 0x03FE = 1022，无负载，长度=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); // 1秒超时 + + // 发送帧头（有重试机制） + 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; + } + + // 提取 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; + } + + // loadmap_status: 0=失败, 1=成功, 2=载入中 + 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 - robot_status_station_req 查询站点列表 + // -------------------------------------------------------------------------- + void AgvSrc1100::queryStationList(QueryStationRes& res) + { + int sock = sock_status_; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "socket not connected"; + return; + } + + // 命令码 0x0515 = 1301，无负载，长度=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), + 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); + } + } + } + + + // -------------------------------------------------------------------------- + // 1060 - robot_status_current_lock_req 查询当前控制权所有者 + // -------------------------------------------------------------------------- + void AgvSrc1100::getCurrentLockStatus(CurrentLockStatus& info) + { + int sock = sock_status_; + if (sock < 0) + { + info.ret_code = -1; + info.err_msg = "socket not connected"; + return; + } + + // 命令码 0x0424 = 1060，无负载，长度=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, + 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 map payload failed"; + info.err_msg = "send frame header 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) + 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) { - retry--; - usleep(10000); - continue; + raw_data.append(reinterpret_cast(recv_buf.data()), recv_ret); } - total_sent += ret; - } - // 帧头发送失败直接返回错误 - if (total_sent != 16) - { setSocketTimeout(sock, 0); - info.ret_code = -2; - info.err_msg = "send frame header failed"; - return; + + if (raw_data.empty()) + { + info.ret_code = -2; + info.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) + { + 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 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", ""); } - // 分片发送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) + + // -------------------------------------------------------------------------- + // 1020 - robot_status_task_req 查询当前导航状态 + // -------------------------------------------------------------------------- + void AgvSrc1100::robotStatusTaskCurrent(RobotStatusTaskCurrentRes& res, const RobotStatusTaskCurrentReq& req) { - int send_len = std::min((size_t)slice_size, total_size - offset); - int ret = send(sock, data + offset, send_len, 0); - if (ret <= 0) + int sock = sock_status_; + LOG(INFO) << "robotStatusTaskCurrent sock fd=" << sock; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "status port socket invalid"; + return; + } + + // 组装请求 JSON: {"simple": true/false} + json send_json; + send_json["simple"] = req.simple; + std::string payload = send_json.dump(); + uint16_t payload_len = static_cast(payload.size()); + + // 命令码 0x03FC = 1020 + static uint8_t frame_header[16] = { + 0x5A,0x01,0x00,0x01, + 0x00,0x00, + 0,0, + 0x03,0xFC, + 0,0,0,0,0,0 + }; + frame_header[6] = static_cast((payload_len >> 8) & 0xFF); + frame_header[7] = static_cast(payload_len & 0xFF); + + flushSocket(sock); + setSocketTimeout(sock, 3000); + + // 发送帧头 + int sent = 0; + int retry = 5; + while (sent < 16 && retry > 0) + { + int w = send(sock, frame_header + sent, 16 - sent, MSG_NOSIGNAL); + if (w <= 0) + { + retry--; + usleep(20000); + continue; + } + sent += w; + } + if (sent != 16) + { + res.ret_code = -2; + res.err_msg = "send frame header failed"; + return; + } + + // 分片发送 payload + const int slice = 1024; + size_t off = 0; + size_t total = payload.size(); + const char* data = payload.data(); + while (off < total) + { + int wlen = static_cast(std::min((size_t)slice, total - off)); + int w = send(sock, data + off, wlen, MSG_NOSIGNAL); + if (w <= 0) + { + res.ret_code = -2; + res.err_msg = "send payload failed"; + return; + } + off += w; + } + + // 接收响应 + std::string raw; + uint8_t buf[65536]; + int rlen = 0; + while ((rlen = recv(sock, buf, sizeof(buf), MSG_NOSIGNAL)) > 0) + { + raw.append((char*)buf, rlen); + } + if (raw.empty()) + { + res.ret_code = -2; + res.err_msg = "no response data"; + return; + } + + // 提取 JSON + size_t json_begin = raw.find('{'); + if (json_begin == std::string::npos) + { + res.ret_code = -4; + res.err_msg = "response no json"; + return; + } + std::string json_str = raw.substr(json_begin); + if (json_str.back() != '}') + json_str += "}"; + + json root; + try + { + root = json::parse(json_str); + } + catch (json::parse_error& e) + { + res.ret_code = -3; + res.err_msg = std::string("json parse err:") + 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() : ""; + + // 提取导航状态字段 + // task_status: 0=NONE, 1=WAITING, 2=RUNNING, 3=SUSPENDED, 4=COMPLETED, 5=FAILED, 6=CANCELED + res.task_status = root.contains("task_status") ? root["task_status"].get() : 0; + // task_type: 0=无, 1=自由导航到坐标, 2=自由导航到站点, 3=路径导航到站点 + 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() : ""; + + // 提取目标坐标 [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() : ""; + c.has_goods = item.contains("has_goods") ? item["has_goods"].get() : false; + res.containers.push_back(c); + } + } + } + + + // -------------------------------------------------------------------------- + // 1110 - robot_status_task_status_package_req 批量查询任务状态 + // -------------------------------------------------------------------------- + void AgvSrc1100::robotStatusTaskPackage(QueryTaskStatusPackageRes& res, const QueryTaskStatusPackageReq& req) + { + int sock = sock_status_; + LOG(INFO) << "robotStatusTaskPackage sock fd=" << sock; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "status port socket invalid"; + return; + } + + // 强制携带 task_ids 字段，空数组也生成，避免设备报 60002 错误 + json send_json; + json arr = json::array(); + for (auto& tid : req.task_ids) + arr.push_back(tid); + send_json["task_ids"] = arr; + + std::string payload = send_json.dump(); + uint16_t payload_len = static_cast(payload.size()); + + // 命令码 0x0456 = 1110 + static uint8_t frame_header[16] = { + 0x5A,0x01,0x00,0x01, + 0x00,0x00, + 0,0, + 0x04,0x56, + 0,0,0,0,0,0 + }; + frame_header[6] = static_cast((payload_len >> 8) & 0xFF); + frame_header[7] = static_cast(payload_len & 0xFF); + + flushSocket(sock); + setSocketTimeout(sock, 3000); + + // 发送帧头 + int sent = 0; + int retry = 5; + while (sent < 16 && retry > 0) + { + int w = send(sock, frame_header + sent, 16 - sent, MSG_NOSIGNAL); + if (w <= 0) + { + retry--; + usleep(20000); + continue; + } + sent += w; + } + if (sent != 16) + { + res.ret_code = -2; + res.err_msg = "send frame header failed"; + return; + } + + // 分片发送 payload + const int slice = 1024; + size_t off = 0; + size_t total = payload.size(); + const char* data = payload.data(); + while (off < total) + { + int wlen = static_cast(std::min((size_t)slice, total - off)); + int w = send(sock, data + off, wlen, MSG_NOSIGNAL); + if (w <= 0) + { + res.ret_code = -2; + res.err_msg = "send payload failed"; + return; + } + off += w; + } + + // 接收响应 + std::string raw; + uint8_t buf[65536]; + int rlen = 0; + while ((rlen = recv(sock, buf, sizeof(buf), MSG_NOSIGNAL)) > 0) + { + raw.append((char*)buf, rlen); + } + if (raw.empty()) + { + res.ret_code = -2; + res.err_msg = "no response data"; + return; + } + + // 提取 JSON + size_t json_begin = raw.find('{'); + if (json_begin == std::string::npos) + { + res.ret_code = -4; + res.err_msg = "response no json"; + return; + } + std::string json_str = raw.substr(json_begin); + if (json_str.back() != '}') + json_str += "}"; + + json root; + try + { + root = json::parse(json_str); + } + catch (json::parse_error& e) + { + res.ret_code = -3; + res.err_msg = std::string("json parse err:") + 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() : ""; + + // 解析 task_status_package 对象 + if (root.contains("task_status_package")) + { + auto pkg_json = root["task_status_package"]; + auto& pkg_out = res.task_status_package; + + pkg_out.closest_target = pkg_json.contains("closest_target") ? pkg_json["closest_target"].get() : ""; + pkg_out.source_name = pkg_json.contains("source_name") ? pkg_json["source_name"].get() : ""; + pkg_out.target_name = pkg_json.contains("target_name") ? pkg_json["target_name"].get() : ""; + pkg_out.percentage = pkg_json.contains("percentage") ? pkg_json["percentage"].get() : 0.0; + pkg_out.distance = pkg_json.contains("distance") ? pkg_json["distance"].get() : 0.0; + pkg_out.info = pkg_json.contains("info") ? pkg_json["info"].get() : ""; + + // 解析任务状态列表 + if (pkg_json.contains("task_status_list") && pkg_json["task_status_list"].is_array()) + { + for (auto& item : pkg_json["task_status_list"]) + { + SingleTaskStatusItem st; + st.task_id = item.contains("task_id") ? item["task_id"].get() : ""; + st.status = item.contains("status") ? item["status"].get() : 0; + st.type = item.contains("type") ? item["type"].get() : 0; + pkg_out.task_status_list.push_back(st); + } + } + } + } + + + // ============================================================================ + // 端口 19205 – 机器人控制 API（允许 5 个连接） + // 功能：下发控制指令，改变机器人运动或状态 + // ============================================================================ + + // -------------------------------------------------------------------------- + // 2010 - robot_control_motion_req 开环速度运动控制 + // -------------------------------------------------------------------------- + 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，只包含非零有效字段 + json send_json; + if (req.vx != 0.0) send_json["vx"] = req.vx; // X 方向速度 + if (req.vy != 0.0) send_json["vy"] = req.vy; // Y 方向速度 + 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()); + + // 命令码 0x07DA = 2010 + 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; + } + + // 分片发送 payload + 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; + } + + // 提取并解析 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 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 - robot_control_loadmap_req 切换载入地图 + // -------------------------------------------------------------------------- + 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()); + + // 命令码 0x07E6 = 2022 + 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); + + // 发送帧头 + 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; + } + + // 分片发送 payload + 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 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; + } + + + // -------------------------------------------------------------------------- + // 2000 - robot_control_stop_req 停止开环运动 命令码0x07D0，应答码12000(0x2EE0) + // 下发零速度开环指令，终止当前持续运动 + // -------------------------------------------------------------------------- + // -------------------------------------------------------------------------- + // 2000 - robot_control_stop_req 停止开环运动 命令码0x07D0，应答码12000(0x2EE0) + // 下发零速度开环指令，终止当前持续运动 + // -------------------------------------------------------------------------- + + void AgvSrc1100::robotControlStop(RobotControlStopRes& res) + { + int sock = sock_control_; + if (sock < 0) { + res.ret_code = -1; + res.err_msg = "control socket disconnected"; + return; + } + + // 业务负载长度为 0 + uint32_t payload_len = 0; + + // 标准 16 字节帧头（长度 4 字节，命令码 0x07D0，保留 6 字节） + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + static_cast((payload_len >> 24) & 0xFF), + static_cast((payload_len >> 16) & 0xFF), + static_cast((payload_len >> 8) & 0xFF), + static_cast(payload_len & 0xFF), + 0x07, 0xD0, + 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, MSG_NOSIGNAL); + if (ret <= 0) { + usleep(10000); + continue; + } + total_sent += ret; + } + if (total_sent != 16) { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send header failed"; + return; + } + + // 无 payload，无需发送额外数据 + + // 接收响应（使用较大缓冲区） + const int RECV_BUF = 131072; + std::vector recv_buf(RECV_BUF); + std::string raw_data; + int rlen; + while ((rlen = recv(sock, recv_buf.data(), RECV_BUF, MSG_NOSIGNAL)) > 0) { + raw_data.append(reinterpret_cast(recv_buf.data()), rlen); + } + setSocketTimeout(sock, 0); + + if (raw_data.empty()) { + res.ret_code = -2; + res.err_msg = "no response from device"; + return; + } + + // 提取 JSON（容错补全） + size_t json_start = raw_data.find('{'); + if (json_start == std::string::npos) { + res.ret_code = -4; + res.err_msg = "no JSON in response"; + return; + } + std::string json_str = raw_data.substr(json_start); + // 补全缺失的 '}' + int left = std::count(json_str.begin(), json_str.end(), '{'); + int right = std::count(json_str.begin(), json_str.end(), '}'); + while (right < left) { json_str += "}"; right++; } + + 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", ""); + } + + + // ============================================================================ + // 端口 19206 – 机器人导航 API（允许 5 个连接） + // 功能：下发导航任务 + // ============================================================================ + + // -------------------------------------------------------------------------- + // 3066 - robot_task_gotargetlist_req 指定路径导航（多点） + // 功能：下发多段连续站点导航任务序列，任务串行排队执行 + // 必填字段：task_id / source_id / id + // -------------------------------------------------------------------------- + void AgvSrc1100::robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) + { + int sock = sock_nav_; + LOG(INFO) << "robotGoTargetList sock fd=" << sock; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "nav socket invalid"; + return; + } + + // 组装请求 JSON: {"move_task_list": [{"task_id":"xxx","source_id":"xxx","id":"xxx"}, ...]} + 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; + + // 命令码 0x0BFA = 3066 + 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); + + // 发送帧头，带重试 + 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; + } + total_sent += ret; + } + if (total_sent != 16) + { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send frame header failed"; + return; + } + + // 分片发送 payload（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; + } + + // 提取并解析 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 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() : ""; + } + + + // -------------------------------------------------------------------------- + // 3051 - robot_task_gotarget_req 单点路径导航（自动规划） + // 功能：给定目标站点，机器人自动规划路径前往 + // 注意：仅用于单车测试，严禁用于多车调度场景 + // -------------------------------------------------------------------------- + void AgvSrc1100::robotGoTarget(RobotGoTargetRes& res, const RobotGoTargetReq& req) + { + int sock = sock_nav_; + LOG(INFO) << "robotGoTarget control sock fd=" << sock; + if (sock < 0) + { + res.ret_code = -1; + res.err_msg = "control port socket invalid"; + return; + } + + // 组装请求 JSON（根据协议 3051 的字段结构） + json send_json; + // 必填字段 + send_json["source_id"] = req.source_id; + send_json["id"] = req.id; + if (!req.task_id.empty()) + send_json["task_id"] = req.task_id; + + // 基础运动参数（只添加非默认值） + if (req.angle != 0) send_json["angle"] = req.angle; + if (!req.method.empty()) send_json["method"] = req.method; + if (req.max_speed != 0) send_json["max_speed"] = req.max_speed; + if (req.max_wspeed != 0) send_json["max_wspeed"] = req.max_wspeed; + if (req.max_acc != 0) send_json["max_acc"] = req.max_acc; + if (req.max_wacc != 0) send_json["max_wacc"] = req.max_wacc; + if (req.duration > 0) send_json["duration"] = req.duration; + if (req.orientation != 0) send_json["orientation"] = req.orientation; + if (req.spin) send_json["spin"] = true; + if (req.delay > 0) send_json["delay"] = req.delay; + if (req.start_rot_dir != 0) send_json["start_rot_dir"] = req.start_rot_dir; + if (req.end_rot_dir != 0) send_json["end_rot_dir"] = req.end_rot_dir; + if (req.reach_dist != 0) send_json["reach_dist"] = req.reach_dist; + if (req.reach_angle != 0) send_json["reach_angle"] = req.reach_angle; + if (!req.skill_name.empty()) send_json["skill_name"] = req.skill_name; + + // PGV 二次定位参数 + auto& pgv = req.pgv; + if (pgv.use_pgv) send_json["use_pgv"] = true; + if (pgv.use_down_pgv) send_json["use_down_pgv"] = true; + if (pgv.pgv_adjust_dist != 0) send_json["pgv_adjust_dist"] = pgv.pgv_adjust_dist; + if (pgv.pgv_adjust_cx != 0) send_json["pgv_adjust_cx"] = pgv.pgv_adjust_cx; + if (pgv.pgv_adjust_cy != 0) send_json["pgv_adjust_cy"] = pgv.pgv_adjust_cy; + if (pgv.pgv_x_adjust != 0) send_json["pgv_x_adjust"] = pgv.pgv_x_adjust; + + // 设备操作（顶升/货叉/辊筒/牵引/音频/等待DI/设置DO/脚本） + if (!req.operation.empty()) + { + send_json["operation"] = req.operation; + + // 顶升设备 JackLoad/JackUnload/JackHeight + if (req.operation.substr(0,4) == "Jack") + { + if (req.jack_height != 0) send_json["jack_height"] = req.jack_height; + } + // 货叉设备 ForkLoad/ForkUnload/ForkHeight/ForkForward + else if (req.operation.substr(0,4) == "Fork") + { + if (req.start_height != 0) send_json["start_height"] = req.start_height; + if (req.end_height != 0) send_json["end_height"] = req.end_height; + if (req.fork_mid_height != 0) send_json["fork_mid_height"] = req.fork_mid_height; + if (req.fork_dist != 0) send_json["fork_dist"] = req.fork_dist; + } + // 辊筒设备 RollerLoad/RollerUnload/RollerRoll/... + else if (req.operation.substr(0,6) == "Roller") + { + if (!req.direction.empty()) send_json["direction"] = req.direction; + } + // 牵引设备 HookLoad/HookUnload + else if (req.operation.substr(0,4) == "Hook") + { + if (req.recognize) send_json["recognize"] = true; + } + // 音频设备 sound + else if (req.operation == "sound") + { + json sound; + auto& sa = req.sounds_args; + if (!sa.name.empty()) sound["name"] = sa.name; + if (sa.loop != 0) sound["loop"] = sa.loop; + if (sa.stop != 0) sound["stop"] = sa.stop; + send_json["sounds_args"] = sound; + } + // 等待 DI 触发 + else if (req.operation == "WaitDI") + { + json args_obj; + json di_arr = json::array(); + for (auto& di : req.wait_di_args.DI) + { + json di_item; + di_item["id"] = di.id; + di_item["status"] = di.status; + di_arr.push_back(di_item); + } + args_obj["DI"] = di_arr; + if (req.wait_di_args.timeout > 0) args_obj["timeout"] = req.wait_di_args.timeout; + send_json["args"] = args_obj; + } + // 控制 DO 开关 + else if (req.operation == "SetDO") + { + json args_obj; + json do_arr = json::array(); + for (auto& d : req.set_do_args.DO) + { + json do_item; + do_item["id"] = d.id; + do_item["status"] = d.status; + do_arr.push_back(do_item); + } + args_obj["DO"] = do_arr; + send_json["args"] = args_obj; + } + } + + // 视觉识别标志 + if (req.recognize) send_json["recognize"] = true; + if (!req.recfile.empty()) send_json["recfile"] = req.recfile; + + // 脚本执行 + if (!req.script_name.empty()) + { + send_json["script_name"] = req.script_name; + send_json["script_stage"] = req.script_stage; + json s_arg; + for (auto& kv : req.script_args.str_kv) s_arg[kv.first] = kv.second; + for (auto& kv : req.script_args.num_kv) s_arg[kv.first] = kv.second; + json do_arr = json::array(); + for (auto& d : req.script_args.do_list) + { + json o; o["id"] = d.id; o["status"] = d.status; do_arr.push_back(o); + } + if (!do_arr.empty()) s_arg["DO"] = do_arr; + json di_arr = json::array(); + for (auto& di : req.script_args.di_list) + { + json o; o["id"] = di.id; o["status"] = di.status; di_arr.push_back(o); + } + if (!di_arr.empty()) s_arg["DI"] = di_arr; + send_json["script_args"] = s_arg; + } + + // 原地旋转（GoByOdometer） + if (req.move_angle != 0) + { + send_json["move_angle"] = req.move_angle; + send_json["speed_w"] = req.speed_w; + send_json["loc_mode"] = req.loc_mode; + } + + // 自由导航（仅双轮差速底盘） + if (req.freeGo.x != 0 || req.freeGo.y != 0) + { + json fg; + fg["x"] = req.freeGo.x; + fg["y"] = req.freeGo.y; + fg["theta"] = req.freeGo.theta; + send_json["freeGo"] = fg; + } + + std::string payload = send_json.dump(); + uint16_t payload_len = static_cast(payload.size()); + + // 命令码 0x0BEB = 3051 + uint8_t frame_header[16] = { + 0x5A,0x01,0x00,0x01, + 0x00,0x00, + 0,0, + 0x0B,0xEB, + 0,0,0,0,0,0 + }; + frame_header[6] = static_cast((payload_len >> 8) & 0xFF); + frame_header[7] = static_cast(payload_len & 0xFF); + + flushSocket(sock); + setSocketTimeout(sock, 3000); + + // 发送帧头 + int sent = 0; + int retry = 5; + while (sent < 16 && retry > 0) + { + int w = send(sock, frame_header + sent, 16 - sent, MSG_NOSIGNAL); + if (w <= 0) + { + retry--; + usleep(20000); + continue; + } + sent += w; + } + if (sent != 16) + { + res.ret_code = -2; + res.err_msg = "send frame header failed"; + return; + } + + // 分片发送 payload + const int slice = 1024; + size_t off = 0; + size_t total = payload.size(); + const char* data = payload.data(); + while (off < total) + { + int wlen = static_cast(std::min((size_t)slice, total - off)); + int w = send(sock, data + off, wlen, MSG_NOSIGNAL); + if (w <= 0) + { + res.ret_code = -2; + res.err_msg = "send payload failed"; + return; + } + off += w; + } + + // 接收响应 + std::string raw; + uint8_t buf[65536]; + int rlen = 0; + while ((rlen = recv(sock, buf, sizeof(buf), MSG_NOSIGNAL)) > 0) + { + raw.append((char*)buf, rlen); + } + if (raw.empty()) + { + res.ret_code = -2; + res.err_msg = "no response data"; + return; + } + + // 提取并解析 JSON + size_t json_start = raw.find('{'); + if (json_start == std::string::npos) + { + res.ret_code = -4; + res.err_msg = "response no valid json"; + return; + } + std::string json_str = raw.substr(json_start); + if (json_str.back() != '}') json_str += "}"; + + json root; + try + { + root = json::parse(json_str); + } + catch (json::parse_error& e) + { + res.ret_code = -3; + res.err_msg = std::string("json parse err:") + 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() : ""; + } + + + // ============================================================================ + // 端口 19207 – 机器人配置 API（允许 5 个连接） + // 功能：配置类操作（地图上传下载、控制权管理等） + // ============================================================================ + + // -------------------------------------------------------------------------- + // 4005 - robot_config_lock_req 抢占控制权 + // 功能：向机器人申请独占控制权，成功后获得操作权限 + // -------------------------------------------------------------------------- + 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()); + + // 命令码 0x0FA5 = 4005 + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + 0x00, 0x00, + static_cast((payload_len >> 8) & 0xFF), + static_cast(payload_len & 0xFF), + 0x0F, 0xA5, + 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; + } + + // 分片发送 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 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", ""); + } + + + + // ============================================================================ + // 3003 - robot_task_cancel_req 取消当前导航任务 (导航端口 19206) + // ============================================================================ + void AgvSrc1100::robotTaskCancel(RobotTaskCancelRes& res) { + int sock = sock_nav_; + if (sock < 0) { + res.ret_code = -1; + res.err_msg = "nav socket disconnected"; + return; + } + + uint32_t payload_len = 0; + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + static_cast((payload_len >> 24) & 0xFF), + static_cast((payload_len >> 16) & 0xFF), + static_cast((payload_len >> 8) & 0xFF), + static_cast(payload_len & 0xFF), + 0x0B, 0xBB, // 命令码 3003 + 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, MSG_NOSIGNAL); + if (ret <= 0) { + usleep(10000); + continue; + } + total_sent += ret; + } + if (total_sent != 16) { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send header failed"; + return; + } + + // 接收响应（先读 16 字节头，再按长度读 JSON） + uint8_t recv_header[16]; + size_t header_read = 0; + while (header_read < 16) { + int ret = recv(sock, recv_header + header_read, 16 - header_read, MSG_NOSIGNAL); + if (ret <= 0) { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "recv header failed or timeout"; + return; + } + header_read += ret; + } + + uint32_t data_len = (recv_header[4] << 24) | + (recv_header[5] << 16) | + (recv_header[6] << 8) | + recv_header[7]; + + std::string json_str; + if (data_len > 0) { + json_str.resize(data_len); + size_t total_read = 0; + while (total_read < data_len) { + int ret = recv(sock, &json_str[total_read], data_len - total_read, MSG_NOSIGNAL); + if (ret <= 0) { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "recv payload failed or timeout"; + return; + } + total_read += ret; + } + } + setSocketTimeout(sock, 0); + + if (json_str.empty()) { + res.ret_code = -4; + res.err_msg = "empty json data"; + return; + } + + // 容错提取 JSON + size_t json_start = json_str.find('{'); + if (json_start == std::string::npos) { + res.ret_code = -4; + res.err_msg = "no JSON found in payload"; + return; + } + size_t json_end = json_str.rfind('}'); + if (json_end != std::string::npos) { + json_str = json_str.substr(json_start, json_end - json_start + 1); + } else { + json_str = json_str.substr(json_start) + "}"; + } + + 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) << "robotTaskCancel success, ret_code=" << res.ret_code; + } + + // ============================================================================ + // 3002 - robot_task_resume_req 继续当前导航任务 (导航端口 19206) + // ============================================================================ + void AgvSrc1100::robotTaskResume(RobotTaskResumeRes& res) { + int sock = sock_nav_; + if (sock < 0) { + res.ret_code = -1; + res.err_msg = "nav socket disconnected"; + return; + } + + uint32_t payload_len = 0; + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + static_cast((payload_len >> 24) & 0xFF), + static_cast((payload_len >> 16) & 0xFF), + static_cast((payload_len >> 8) & 0xFF), + static_cast(payload_len & 0xFF), + 0x0B, 0xBA, // 命令码 3002 + 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, MSG_NOSIGNAL); + if (ret <= 0) { + usleep(10000); + continue; + } + total_sent += ret; + } + if (total_sent != 16) { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send header failed"; + return; + } + + // 接收响应（先读 16 字节头，再按长度读 JSON） + uint8_t recv_header[16]; + size_t header_read = 0; + while (header_read < 16) { + int ret = recv(sock, recv_header + header_read, 16 - header_read, MSG_NOSIGNAL); + if (ret <= 0) { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "recv header failed or timeout"; + return; + } + header_read += ret; + } + + uint32_t data_len = (recv_header[4] << 24) | + (recv_header[5] << 16) | + (recv_header[6] << 8) | + recv_header[7]; + + std::string json_str; + if (data_len > 0) { + json_str.resize(data_len); + size_t total_read = 0; + while (total_read < data_len) { + int ret = recv(sock, &json_str[total_read], data_len - total_read, MSG_NOSIGNAL); + if (ret <= 0) { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "recv payload failed or timeout"; + return; + } + total_read += ret; + } + } + setSocketTimeout(sock, 0); + + if (json_str.empty()) { + res.ret_code = -4; + res.err_msg = "empty json data"; + return; + } + + // 容错提取 JSON + size_t json_start = json_str.find('{'); + if (json_start == std::string::npos) { + res.ret_code = -4; + res.err_msg = "no JSON found in payload"; + return; + } + size_t json_end = json_str.rfind('}'); + if (json_end != std::string::npos) { + json_str = json_str.substr(json_start, json_end - json_start + 1); + } else { + json_str = json_str.substr(json_start) + "}"; + } + + 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) << "robotTaskResume success, ret_code=" << res.ret_code; + } + + + + // ============================================================================ + // 3001 - robot_task_pause_req 暂停当前导航任务 (控制端口 19205) + // ============================================================================ + void AgvSrc1100::robotTaskPause(RobotTaskPauseRes& res) { + int sock = sock_nav_; // 使用控制端口 + if (sock < 0) { + res.ret_code = -1; + res.err_msg = "control socket disconnected"; + return; + } + + // 业务负载长度为 0（无 JSON 数据） + uint32_t payload_len = 0; + + // 标准 16 字节帧头（长度 4 字节，命令码 0x0BB9，保留 6 字节） + uint8_t send_buf[16] = { + 0x5A, 0x01, 0x00, 0x01, + static_cast((payload_len >> 24) & 0xFF), + static_cast((payload_len >> 16) & 0xFF), + static_cast((payload_len >> 8) & 0xFF), + static_cast(payload_len & 0xFF), + 0x0B, 0xB9, // 命令码 3001 + 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, MSG_NOSIGNAL); + if (ret <= 0) { + usleep(10000); + continue; + } + total_sent += ret; + } + if (total_sent != 16) { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "send header failed"; + return; + } + + // 接收响应（先读 16 字节帧头，再按长度读 JSON） + uint8_t recv_header[16]; + size_t header_read = 0; + while (header_read < 16) { + int ret = recv(sock, recv_header + header_read, 16 - header_read, MSG_NOSIGNAL); + if (ret <= 0) { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "recv header failed or timeout"; + return; + } + header_read += ret; + } + + // 解析数据区长度（大端 4 字节） + uint32_t data_len = (recv_header[4] << 24) | + (recv_header[5] << 16) | + (recv_header[6] << 8) | + recv_header[7]; + + // 读取数据区（JSON） + std::string json_str; + if (data_len > 0) { + json_str.resize(data_len); + size_t total_read = 0; + while (total_read < data_len) { + int ret = recv(sock, &json_str[total_read], data_len - total_read, MSG_NOSIGNAL); + if (ret <= 0) { + setSocketTimeout(sock, 0); + res.ret_code = -2; + res.err_msg = "recv payload failed or timeout"; + return; + } + total_read += ret; + } + } + setSocketTimeout(sock, 0); + + if (json_str.empty()) { + res.ret_code = -4; + res.err_msg = "empty json data"; + return; + } + + // 容错提取 JSON（可能包含前缀/后缀） + size_t json_start = json_str.find('{'); + if (json_start == std::string::npos) { + res.ret_code = -4; + res.err_msg = "no JSON found in payload"; + return; + } + size_t json_end = json_str.rfind('}'); + if (json_end != std::string::npos) { + json_str = json_str.substr(json_start, json_end - json_start + 1); + } else { + json_str = json_str.substr(json_start) + "}"; + } + + // 解析 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) << "robotTaskPause success, ret_code=" << res.ret_code; + } + + + + + + // -------------------------------------------------------------------------- + // 4010 - robot_config_uploadmap_req 上传地图 + // 功能：将地图文件上传到机器人 + // -------------------------------------------------------------------------- + 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; + } + + // 清空残留数据，设置超时 + flushSocket(sock); + setSocketTimeout(sock, 1000); + + // 计算负载长度（地图 JSON 数据） + uint16_t payload_len = static_cast(map_json.size()); + // 命令码 0x0FAA = 4010 + 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 + }; + + // 发送帧头（带重试） + 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; + } + + // 分片发送地图数据（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 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", ""); + } + + + // -------------------------------------------------------------------------- + // 4011 - robot_config_downloadmap_req 下载地图 + // 功能：从机器人下载指定地图文件 + // -------------------------------------------------------------------------- + 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: {"map_name":"xxx"} + std::string req_json = R"({"map_name":")" + map_name + R"("})"; + uint8_t payload_len = static_cast(req_json.size()); + + // 命令码 0x0FAB = 4011 + 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; } - 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); + // 接收地图数据（地图文件可能较大，使用 65536 字节缓冲区） + const int RECV_BUF_LEN = 65536; + std::vector recvBuf(RECV_BUF_LEN); + std::string rawData; + int ret = 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) + setSocketTimeout(sock, 500); // 延长超时适配大地图 + while ((ret = recv(sock, recvBuf.data(), RECV_BUF_LEN, 0)) > 0) { - retry--; - usleep(10000); - continue; + rawData.append(reinterpret_cast(recvBuf.data()), ret); } - 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) + if (rawData.empty()) { - 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"; + info.ret_code = -2; + info.err_msg = "no response data"; 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); + // 提取并解析 JSON + size_t jsonStart = rawData.find('{'); + size_t jsonEnd = rawData.rfind('}'); + std::string json_str; - 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) + if (jsonStart == std::string::npos) { - 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"; + info.ret_code = -4; + info.err_msg = "json not found"; 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) + if (jsonEnd == std::string::npos) { - retry_cnt--; - usleep(10000); - continue; + json_str = rawData.substr(jsonStart) + "}"; } - 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) + else { - retry_cnt--; - usleep(10000); - continue; + json_str = rawData.substr(jsonStart, jsonEnd - jsonStart + 1); } - 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"]) + json root; + std::string err; + if (!parseJson(json_str, root, err)) { - 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"; + info.ret_code = -3; + info.err_msg = err; return; } - offset += ret; + + 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 : ""; } - // 接收并解析响应 - 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() : ""; -} + // ============================================================================ + // 端口 19210 – 其他 API（允许 5 个连接） + // 功能：外设控制（顶升/货叉/辊筒/音频/IO 等） + // 当前未实现具体方法，预留扩展 + // ============================================================================ - //=========================================================================== -// 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); - } - } -} + // ============================================================================ + // 端口 19301 – 机器人推送 API（允许 10 个连接） + // 功能：接收机器人主动推送的实时状态数据 + // 当前未实现具体方法，预留扩展 + // ============================================================================ -} // namespace cmvr::device - - +} \ No newline at end of file diff --git a/cmvr-es/service/grpc/include/grpc_agv_service.h b/cmvr-es/service/grpc/include/grpc_agv_service.h index d620c474..02253dda 100644 --- a/cmvr-es/service/grpc/include/grpc_agv_service.h +++ b/cmvr-es/service/grpc/include/grpc_agv_service.h @@ -1,6 +1,14 @@ // // Created by xtkuang on 2025/6/1. // +/** + * @file grpc_agv_service.h + * @brief gRPC AGV 服务实现类的头文件。 + * 继承自自动生成的 api::AgvService::Service，提供所有 AGV 相关 RPC 接口的具体实现。 + * 通过 DeviceManager 获取对应的 AGV 设备实例（如 AgvSrc1100），将请求转发至设备层。 + * @author xtkuang + * @date 2025-06-01 + */ #ifndef GRPC_AGV_SERVICE_H #define GRPC_AGV_SERVICE_H @@ -11,121 +19,260 @@ namespace cmvr::service { - class gRPCAGVServiceImpl final : public api::AgvService::Service { - public: - gRPCAGVServiceImpl(); - ~gRPCAGVServiceImpl() override = default; +/** + * @brief gRPC AGV 服务实现类。 + * 负责将 gRPC 请求转换为对具体 AGV 设备的调用，并统一处理异常、日志和时间戳。 + */ +class gRPCAGVServiceImpl final : public api::AgvService::Service { +public: + /** + * @brief 构造函数，获取 DeviceManager 单例引用。 + */ + gRPCAGVServiceImpl(); - // AGV服务接口 - grpc::Status GetStatusInfo(grpc::ServerContext* context, - const api::GetAgvStatusInfoCommand_Request* request, - api::GetAgvStatusInfoCommand_Feedback* response) override; + /** + * @brief 默认析构函数。 + */ + ~gRPCAGVServiceImpl() override = default; - 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; + /** + * @brief 获取 AGV 基本信息（命令码 1000）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID（header.device_id）。 + * @param response 响应消息，填充 AGV 状态信息（版本、型号、IP、MAC 等）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status GetStatusInfo(grpc::ServerContext* context, + const api::GetAgvStatusInfoCommand_Request* request, + api::GetAgvStatusInfoCommand_Feedback* response) override; - grpc::Status RobotConfigDownloadMap(grpc::ServerContext* context, - const api::RobotConfigDownloadMapCommand_Request* request, - api::RobotConfigDownloadMapCommand_Feedback* response) override; + /** + * @brief 查询电池状态（命令码 1007）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID 和可选的 simple 标志。 + * @param response 响应消息，填充电池电量、温度、充放电状态等。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status GetBatteryStatus(grpc::ServerContext* context, + const api::RobotStatusBatteryCommand_Request* request, + api::RobotStatusBatteryCommand_Feedback* response) override; - grpc::Status GetMapStatus(grpc::ServerContext* context, - const api::RobotStatusMapCommand_Request* request, - api::RobotStatusMapCommand_Feedback* response) override; + /** + * @brief 查询机器人当前位置（命令码 1004）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID。 + * @param response 响应消息，填充坐标、朝向角、置信度、当前站点等。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status GetRobotLocation(grpc::ServerContext* context, + const api::RobotStatusLocCommand_Request* request, + api::RobotStatusLocCommand_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 下载指定地图（命令码 4011）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID 和地图名称。 + * @param response 响应消息，返回地图 JSON 内容或错误码。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotConfigDownloadMap(grpc::ServerContext* context, + const api::RobotConfigDownloadMapCommand_Request* request, + api::RobotConfigDownloadMapCommand_Feedback* response) override; + /** + * @brief 查询地图状态（命令码 1300）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID。 + * @param response 响应消息，返回当前地图名称、所有地图列表及文件详情。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status GetMapStatus(grpc::ServerContext* context, + const api::RobotStatusMapCommand_Request* request, + api::RobotStatusMapCommand_Feedback* response) override; - /** - * @brief gRPC接口 查询当前控制权持有者 - */ - grpc::Status GetCurrentLockStatus(grpc::ServerContext* context, - const api::RobotStatusCurrentLockCommand_Request* request, - api::RobotStatusCurrentLockCommand_Feedback* response) override; + /** + * @brief 上传地图（命令码 4010）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID 和完整的地图 JSON 字符串。 + * @param response 响应消息，返回上传结果（ret_code）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotConfigUploadMap(grpc::ServerContext* context, + const api::RobotConfigUploadMapCommand_Request* request, + api::RobotConfigUploadMapCommand_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 抢占机器人控制权（命令码 4005）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID 和抢占者昵称。 + * @param response 响应消息，返回抢占结果（ret_code）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotConfigLock(grpc::ServerContext* context, + const api::RobotConfigLockCommand_Request* request, + api::RobotConfigLockCommand_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 查询当前控制权持有者（命令码 1060）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID。 + * @param response 响应消息，返回是否被锁定、持有者 IP/端口/昵称等信息。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status GetCurrentLockStatus(grpc::ServerContext* context, + const api::RobotStatusCurrentLockCommand_Request* request, + api::RobotStatusCurrentLockCommand_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 下发开环速度运动指令（命令码 2010）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID 和速度参数（vx, vy, w, steer, duration 等）。 + * @param response 响应消息，返回指令下发结果（ret_code）。 + * @note 此指令会强制取消当前自动导航任务，多舵轮设备仅 vx/vy/w 生效。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotMotionControl(grpc::ServerContext* context, + const api::RobotMotionControlCommand_Request* request, + api::RobotMotionControlCommand_Feedback* response) override; + /** + * @brief 切换载入地图（命令码 2022）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID 和目标地图名称。 + * @param response 响应消息，返回切换结果（ret_code）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + 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 RobotGoTargetList( - grpc::ServerContext* context, - const api::RobotGoTargetListCommand_Request* request, - api::RobotGoTargetListCommand_Feedback* response) override; + /** + * @brief 停止开环运动（命令码 2000）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID（无业务数据）。 + * @param response 响应消息，返回停止结果（ret_code）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotControlStop(grpc::ServerContext* context, + const api::RobotControlStopCommand::Request* request, + api::RobotControlStopCommand::Feedback* response) override; + // ===================== 导航任务接口 ===================== - grpc::Status RobotStatusTask( - grpc::ServerContext* context, - const api::RobotStatusTaskCommand_Request* request, - api::RobotStatusTaskCommand_Feedback* response) override; + /** + * @brief 查询地图载入状态（命令码 1022）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID。 + * @param response 响应消息，返回 loadmap_status（0=失败, 1=成功, 2=载入中）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status QueryLoadMapStatus(grpc::ServerContext* context, + const api::RobotQueryLoadMapStatusCommand_Request* request, + api::RobotQueryLoadMapStatusCommand_Feedback* response) override; + /** + * @brief 查询当前地图所有站点信息（命令码 1301）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID。 + * @param response 响应消息，返回站点列表（ID、坐标、类型等）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status QueryStationList(grpc::ServerContext* context, + const api::QueryStationListCommand_Request* request, + api::QueryStationListCommand_Feedback* response) override; + /** + * @brief 指定路径导航（命令码 3066）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID 和多段导航任务列表（move_task_list）。 + * @param response 响应消息，返回下发结果（ret_code=0 仅表示接收成功，不表示执行完成）。 + * @attention 每个任务必须包含 task_id, source_id, id，且相邻站点间必须有直接路径。 + * 任务会排队执行，适合多车调度场景。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotGoTargetList(grpc::ServerContext* context, + const api::RobotGoTargetListCommand_Request* request, + api::RobotGoTargetListCommand_Feedback* response) override; + /** + * @brief 查询当前实时导航状态（命令码 1020）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID 和 simple 标志。 + * @param response 响应消息，返回任务状态、任务类型、目标、路径等信息。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotStatusTaskCurrent(grpc::ServerContext* context, + const api::RobotStatusTaskCurrentCommand_Request* request, + api::RobotStatusTaskCurrentCommand_Feedback* response) override; + /** + * @brief 批量查询任务状态（命令码 1110）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID 和 task_ids 列表（为空则查询所有未完成+最近完成）。 + * @param response 响应消息，返回任务状态包（进度、距离、各任务状态等）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotStatusTaskPackage(grpc::ServerContext* context, + const api::RobotStatusTaskPackageCommand_Request* request, + api::RobotStatusTaskPackageCommand_Feedback* response) override; + /** + * @brief 单点站点自动规划导航（命令码 3051）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID 和完整的导航参数（支持自由导航、动作、PGV 等）。 + * @param response 响应消息，返回下发结果（ret_code=0 仅表示接收成功）。 + * @warning 严禁用于多车调度场景，仅限单车测试；新任务会取消当前任务（不排队）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotGoTarget(grpc::ServerContext* context, + const api::RobotGoTargetCommand_Request* request, + api::RobotGoTargetCommand_Feedback* response) override; - private: - device::DeviceManager& dmgr_; - }; -} + /** + * @brief 暂停当前导航任务（命令码 3001）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID（无业务数据）。 + * @param response 响应消息，返回暂停结果（ret_code）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotTaskPause(grpc::ServerContext* context, + const api::RobotTaskPauseCommand::Request* request, + api::RobotTaskPauseCommand::Feedback* response) override; -#endif //GRPC_AGV_SERVICE_H \ No newline at end of file + /** + * @brief 继续当前导航任务（命令码 3002）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID（无业务数据）。 + * @param response 响应消息，返回继续结果（ret_code）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotTaskResume(grpc::ServerContext* context, + const api::RobotTaskResumeCommand::Request* request, + api::RobotTaskResumeCommand::Feedback* response) override; + + /** + * @brief 取消当前导航任务（命令码 3003）。 + * @param context gRPC 上下文（未使用）。 + * @param request 请求消息，包含设备 ID（无业务数据）。 + * @param response 响应消息，返回取消结果（ret_code）。 + * @return 始终返回 grpc::Status::OK，错误信息通过 response.header 返回。 + */ + grpc::Status RobotTaskCancel(grpc::ServerContext* context, + const api::RobotTaskCancelCommand::Request* request, + api::RobotTaskCancelCommand::Feedback* response) override; + +private: + device::DeviceManager& dmgr_; ///< 设备管理器引用，用于根据 device_id 获取 AGV 设备实例。 +}; + +} // namespace cmvr::service + +#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 index 8b7fd741..a48e03b2 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -1,6 +1,15 @@ // // Created by xtkuang on 2025/6/1. // +/** + * @file grpc_agv_service.cpp + * @brief gRPC AGV 服务实现。 + * 每个 RPC 方法从请求头中提取 device_id，通过 DeviceManager 获取对应的 AGV 设备， + * 调用设备层的具体方法，并将结果转换为 Protobuf 响应。 + * 统一处理异常，设置响应头中的 success/error_message 和时间戳。 + * @author xtkuang + * @date 2025-06-01 + */ #include "./service/grpc/include/grpc_agv_service.h" @@ -12,9 +21,21 @@ namespace cmvr::service { + /** + * @brief 构造函数，获取 DeviceManager 单例。 + */ gRPCAGVServiceImpl::gRPCAGVServiceImpl() : dmgr_(device::DeviceManager::getInstance()) { } + // ===================== GetStatusInfo ===================== + /** + * @brief 获取 AGV 基本信息。 + * @details 从请求中提取 device_id，调用设备层的 getStatusInfo，填充响应。 + * @param context 未使用。 + * @param request 包含设备 ID。 + * @param response 返回状态信息和响应头。 + * @return 始终返回 OK，错误通过 response.header 传递。 + */ grpc::Status gRPCAGVServiceImpl::GetStatusInfo( grpc::ServerContext* context, const api::GetAgvStatusInfoCommand_Request* request, @@ -28,6 +49,7 @@ namespace cmvr::service { device::AbstractAgv::AgvStatusInfo device_status; agv_device->getStatusInfo(device_status); + // 映射到 Protobuf 消息 auto* proto_status = response->mutable_status(); proto_status->set_id(device_status.id); proto_status->set_vehicle_id(device_status.vehicle_id); @@ -58,62 +80,75 @@ namespace cmvr::service { } } - -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) + // ===================== GetBatteryStatus ===================== + /** + * @brief 查询电池状态。 + * @details 支持 simple 参数，调用设备层 getBatteryStatus。 + * @param context 未使用。 + * @param request 包含设备 ID 和 simple 标志。 + * @param response 返回电池信息。 + */ + grpc::Status gRPCAGVServiceImpl::GetBatteryStatus( + grpc::ServerContext* context, + const api::RobotStatusBatteryCommand_Request* request, + api::RobotStatusBatteryCommand_Feedback* response) { - auto* header = response->mutable_header(); - header->set_success(false); - header->set_error_message(e.what()); - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; + 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; + } } -} - + // ===================== GetRobotLocation ===================== + /** + * @brief 查询机器人当前位置。 + * @param context 未使用。 + * @param request 包含设备 ID。 + * @param response 返回坐标、朝向、置信度等。 + */ grpc::Status gRPCAGVServiceImpl::GetRobotLocation( grpc::ServerContext* context, const api::RobotStatusLocCommand_Request* request, @@ -157,7 +192,13 @@ api::RobotStatusBatteryCommand_Feedback* response) } } - + // ===================== RobotConfigDownloadMap ===================== + /** + * @brief 下载指定地图。 + * @param context 未使用。 + * @param request 包含设备 ID 和地图名称。 + * @param response 返回地图 JSON 内容或错误码。 + */ grpc::Status gRPCAGVServiceImpl::RobotConfigDownloadMap( grpc::ServerContext* context, const api::RobotConfigDownloadMapCommand_Request* request, @@ -196,8 +237,13 @@ api::RobotStatusBatteryCommand_Feedback* response) } } - - + // ===================== GetMapStatus ===================== + /** + * @brief 查询地图状态。 + * @param context 未使用。 + * @param request 包含设备 ID。 + * @param response 返回当前地图、地图列表及文件详情。 + */ grpc::Status gRPCAGVServiceImpl::GetMapStatus( grpc::ServerContext* context, const api::RobotStatusMapCommand_Request* request, @@ -249,7 +295,13 @@ api::RobotStatusBatteryCommand_Feedback* response) } } - + // ===================== RobotConfigUploadMap ===================== + /** + * @brief 上传地图。 + * @param context 未使用。 + * @param request 包含设备 ID 和地图 JSON 字符串。 + * @param response 返回上传结果。 + */ grpc::Status gRPCAGVServiceImpl::RobotConfigUploadMap( grpc::ServerContext* context, const api::RobotConfigUploadMapCommand_Request* request, @@ -287,39 +339,33 @@ api::RobotStatusBatteryCommand_Feedback* response) } } - + // ===================== RobotConfigLock ===================== /** - * @brief gRPC 抢占机器人控制权接口 - * @param context grpc上下文 - * @param request 前端请求参数，携带nick_name抢占者名称 - * @param response 抢占结果返回体 - */ + * @brief 抢占机器人控制权。 + * @param context 未使用。 + * @param request 包含设备 ID 和抢占者昵称。 + * @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(""); @@ -330,7 +376,6 @@ api::RobotStatusBatteryCommand_Feedback* response) } catch (std::exception &e) { - // 统一异常捕获，和相机/查询接口风格完全一致 auto* header = response->mutable_header(); header->set_success(false); header->set_error_message(e.what()); @@ -339,71 +384,72 @@ api::RobotStatusBatteryCommand_Feedback* response) } } - -/** - * @brief 查询机器人当前控制权信息gRPC接口 - */ -grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( + // ===================== GetCurrentLockStatus ===================== + /** + * @brief 查询当前控制权持有者。 + * @param context 未使用。 + * @param request 包含设备 ID。 + * @param response 返回锁定状态、持有者信息等。 + */ + 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; + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (GetCurrentLockStatus): id=" << dev_id; + + const auto agv_dev = dmgr_.getDevice(dev_id); + + device::AbstractAgv::CurrentLockStatus lock_info; + agv_dev->getCurrentLockStatus(lock_info); + + 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); + + 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) + { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(e.what()); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } } -} - + // ===================== RobotMotionControl ===================== + /** + * @brief 开环速度运动控制。 + * @param context 未使用。 + * @param request 包含设备 ID 和速度参数。 + * @param response 返回指令下发结果。 + */ 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(); @@ -413,17 +459,14 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( 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(""); @@ -436,7 +479,6 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( } catch (std::exception& e) { - // 全局统一异常捕获模板 auto* header = response->mutable_header(); header->set_success(false); header->set_error_message(e.what()); @@ -445,6 +487,13 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( } } + // ===================== RobotLoadMap ===================== + /** + * @brief 切换载入地图。 + * @param context 未使用。 + * @param request 包含设备 ID 和目标地图名称。 + * @param response 返回切换结果。 + */ grpc::Status gRPCAGVServiceImpl::RobotLoadMap( grpc::ServerContext* context, const api::RobotLoadMapCommand_Request* request, @@ -470,7 +519,6 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( 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 @@ -483,13 +531,18 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( auto* header = response->mutable_header(); header->set_success(false); header->set_error_message(e.what()); - // 修复：取timestamp子字段 setCurrentTimestamp(header->mutable_timestamp()); return grpc::Status::OK; } } - + // ===================== QueryLoadMapStatus ===================== + /** + * @brief 查询地图载入状态。 + * @param context 未使用。 + * @param request 包含设备 ID。 + * @param response 返回 loadmap_status。 + */ grpc::Status gRPCAGVServiceImpl::QueryLoadMapStatus( grpc::ServerContext* context, const api::RobotQueryLoadMapStatusCommand_Request* request, @@ -529,6 +582,13 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( } } + // ===================== QueryStationList ===================== + /** + * @brief 查询当前地图所有站点信息。 + * @param context 未使用。 + * @param request 包含设备 ID。 + * @param response 返回站点列表。 + */ grpc::Status gRPCAGVServiceImpl::QueryStationList( grpc::ServerContext* context, const api::QueryStationListCommand_Request* request, @@ -547,7 +607,6 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( 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(); @@ -584,7 +643,13 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( } } - + // ===================== RobotGoTargetList ===================== + /** + * @brief 指定路径导航（多点）。 + * @param context 未使用。 + * @param request 包含设备 ID 和 move_task_list。 + * @param response 返回下发结果（仅表示指令被接收）。 + */ grpc::Status gRPCAGVServiceImpl::RobotGoTargetList( grpc::ServerContext* context, const api::RobotGoTargetListCommand_Request* request, @@ -636,28 +701,31 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( } } - - - - grpc::Status gRPCAGVServiceImpl::RobotStatusTask( + // ===================== RobotStatusTaskCurrent ===================== + /** + * @brief 查询当前实时导航状态。 + * @param context 未使用。 + * @param request 包含设备 ID 和 simple 标志。 + * @param response 返回任务状态、类型、目标、路径等。 + */ + grpc::Status gRPCAGVServiceImpl::RobotStatusTaskCurrent( grpc::ServerContext* context, - const api::RobotStatusTaskCommand_Request* request, - api::RobotStatusTaskCommand_Feedback* response) + const api::RobotStatusTaskCurrentCommand_Request* request, + api::RobotStatusTaskCurrentCommand_Feedback* response) { try { std::string dev_id = request->header().device_id(); - LOG(INFO) << "[gRPCAGVServiceImpl] (RobotStatusTask): id=" << dev_id; + LOG(INFO) << "[gRPCAGVServiceImpl] (RobotStatusTaskCurrent): id=" << dev_id; const auto agv_dev = dmgr_.getDevice(dev_id); - // 嵌套结构体 必须加 AbstractAgv:: - device::AbstractAgv::RobotStatusTaskReq req; + device::AbstractAgv::RobotStatusTaskCurrentReq req; if (request->has_data()) { req.simple = request->data().simple(); } - device::AbstractAgv::RobotStatusTaskRes res; - agv_dev->robotStatusTask(res, req); + device::AbstractAgv::RobotStatusTaskCurrentRes res; + agv_dev->robotStatusTaskCurrent(res, req); auto* out_data = response->mutable_data(); out_data->set_ret_code(res.ret_code); @@ -690,7 +758,7 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( header->set_error_message(""); setCurrentTimestamp(header->mutable_timestamp()); - LOG(INFO) << "RobotStatusTask device_id:" << dev_id << " task_status: " << res.task_status; + LOG(INFO) << "RobotStatusTaskCurrent dev_id:" << dev_id << " task_status:" << res.task_status; return grpc::Status::OK; } catch (std::exception &e) @@ -703,6 +771,438 @@ grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus( } } + // ===================== RobotStatusTaskPackage ===================== + /** + * @brief 批量查询任务状态。 + * @param context 未使用。 + * @param request 包含设备 ID 和 task_ids 列表。 + * @param response 返回任务状态包（进度、距离、每个任务状态等）。 + */ + grpc::Status gRPCAGVServiceImpl::RobotStatusTaskPackage( + grpc::ServerContext* context, + const api::RobotStatusTaskPackageCommand_Request* request, + api::RobotStatusTaskPackageCommand_Feedback* response) + { + try { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[gRPCAGVServiceImpl] (RobotStatusTaskPackage): id=" << dev_id; + const auto agv_dev = dmgr_.getDevice(dev_id); + device::AbstractAgv::QueryTaskStatusPackageReq req; + const auto& pb_ids = request->data().task_ids(); + for (auto& tid : pb_ids) + { + req.task_ids.push_back(tid); + } -} \ No newline at end of file + device::AbstractAgv::QueryTaskStatusPackageRes res; + agv_dev->robotStatusTaskPackage(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); + + auto* pkg_pb = out_data->mutable_task_status_package(); + auto& pkg_data = res.task_status_package; + pkg_pb->set_closest_target(pkg_data.closest_target); + pkg_pb->set_source_name(pkg_data.source_name); + pkg_pb->set_target_name(pkg_data.target_name); + pkg_pb->set_percentage(pkg_data.percentage); + pkg_pb->set_distance(pkg_data.distance); + pkg_pb->set_info(pkg_data.info); + + for (auto& st_item : pkg_data.task_status_list) + { + auto* st_pb = pkg_pb->add_task_status_list(); + st_pb->set_task_id(st_item.task_id); + st_pb->set_status(st_item.status); + st_pb->set_type(st_item.type); + } + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "RobotStatusTaskPackage dev_id:" << dev_id << " query task count:" << req.task_ids.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; + } + } + + // ===================== RobotGoTarget ===================== + /** + * @brief 单点站点自动规划导航。 + * @param context 未使用。 + * @param request 包含设备 ID 和完整的导航参数（支持自由导航、动作、PGV、脚本等）。 + * @param response 返回下发结果。 + * @warning 严禁用于多车调度场景，仅限单车测试。 + */ + grpc::Status gRPCAGVServiceImpl::RobotGoTarget( + grpc::ServerContext* context, + const api::RobotGoTargetCommand_Request* request, + api::RobotGoTargetCommand_Feedback* response) + { + try + { + std::string dev_id = request->header().device_id(); + LOG(INFO) << "[RobotGoTarget] device_id:" << dev_id; + auto agv_dev = dmgr_.getDevice(dev_id); + if (!agv_dev) + { + auto* header = response->mutable_header(); + header->set_success(false); + header->set_error_message("device not found"); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + + device::AbstractAgv::RobotGoTargetReq req; + auto& pb_data = request->data(); + + // -------- 必填字段 -------- + req.source_id = pb_data.source_id(); + req.id = pb_data.id(); + if (pb_data.has_task_id()) req.task_id = pb_data.task_id(); + + // -------- 运动参数 -------- + if (pb_data.has_angle()) req.angle = pb_data.angle(); + if (pb_data.has_method()) req.method = pb_data.method(); + if (pb_data.has_max_speed()) req.max_speed = pb_data.max_speed(); + if (pb_data.has_max_wspeed()) req.max_wspeed = pb_data.max_wspeed(); + if (pb_data.has_max_acc()) req.max_acc = pb_data.max_acc(); + if (pb_data.has_max_wacc()) req.max_wacc = pb_data.max_wacc(); + if (pb_data.has_duration()) req.duration = pb_data.duration(); + if (pb_data.has_orientation()) req.orientation = pb_data.orientation(); + if (pb_data.has_spin()) req.spin = pb_data.spin(); + if (pb_data.has_delay()) req.delay = pb_data.delay(); + if (pb_data.has_start_rot_dir()) req.start_rot_dir = pb_data.start_rot_dir(); + if (pb_data.has_end_rot_dir()) req.end_rot_dir = pb_data.end_rot_dir(); + if (pb_data.has_reach_dist()) req.reach_dist = pb_data.reach_dist(); + if (pb_data.has_reach_angle()) req.reach_angle = pb_data.reach_angle(); + if (pb_data.has_skill_name()) req.skill_name = pb_data.skill_name(); + + // -------- PGV 二次定位 -------- + if (pb_data.has_pgv()) + { + auto& pgv_pb = pb_data.pgv(); + req.pgv.use_pgv = pgv_pb.use_pgv(); + req.pgv.use_down_pgv = pgv_pb.use_down_pgv(); + if (pgv_pb.has_pgv_adjust_dist()) req.pgv.pgv_adjust_dist = pgv_pb.pgv_adjust_dist(); + if (pgv_pb.has_pgv_adjust_cx()) req.pgv.pgv_adjust_cx = pgv_pb.pgv_adjust_cx(); + if (pgv_pb.has_pgv_adjust_cy()) req.pgv.pgv_adjust_cy = pgv_pb.pgv_adjust_cy(); + if (pgv_pb.has_pgv_x_adjust()) req.pgv.pgv_x_adjust = pgv_pb.pgv_x_adjust(); + } + + // -------- 设备动作 -------- + if (pb_data.has_operation()) req.operation = pb_data.operation(); + if (pb_data.has_jack_height()) req.jack_height = pb_data.jack_height(); + if (pb_data.has_start_height()) req.start_height = pb_data.start_height(); + if (pb_data.has_end_height()) req.end_height = pb_data.end_height(); + if (pb_data.has_fork_mid_height()) req.fork_mid_height = pb_data.fork_mid_height(); + if (pb_data.has_fork_dist()) req.fork_dist = pb_data.fork_dist(); + if (pb_data.has_direction()) req.direction = pb_data.direction(); + if (pb_data.has_recognize()) req.recognize = pb_data.recognize(); + if (pb_data.has_recfile()) req.recfile = pb_data.recfile(); + + // -------- 音频 -------- + if (pb_data.has_sounds_args()) + { + auto& s_pb = pb_data.sounds_args(); + if (s_pb.has_name()) req.sounds_args.name = s_pb.name(); + if (s_pb.has_loop()) req.sounds_args.loop = s_pb.loop(); + if (s_pb.has_stop()) req.sounds_args.stop = s_pb.stop(); + } + + // -------- WaitDI -------- + if (pb_data.has_wait_di_args()) + { + auto& di_pb = pb_data.wait_di_args(); + req.wait_di_args.timeout = di_pb.timeout(); + for (auto& item : di_pb.di()) + { + device::AbstractAgv::DIItem di; + di.id = item.id(); + di.status = item.status(); + req.wait_di_args.DI.push_back(di); + } + } + + // -------- SetDO -------- + if (pb_data.has_set_do_args()) + { + auto& do_pb = pb_data.set_do_args(); + for (auto& item : do_pb.do_list()) + { + device::AbstractAgv::DOItem d; + d.id = item.id(); + d.status = item.status(); + req.set_do_args.DO.push_back(d); + } + } + + // -------- 脚本 -------- + if (pb_data.has_script_name()) req.script_name = pb_data.script_name(); + if (pb_data.has_script_stage()) req.script_stage = pb_data.script_stage(); + if (pb_data.has_script_args()) + { + auto& s_arg_pb = pb_data.script_args(); + for (auto& kv : s_arg_pb.str_kv()) req.script_args.str_kv[kv.first] = kv.second; + for (auto& kv : s_arg_pb.num_kv()) req.script_args.num_kv[kv.first] = kv.second; + for (auto& d : s_arg_pb.do_list()) + { + device::AbstractAgv::DOItem di; + di.id = d.id(); + di.status = d.status(); + req.script_args.do_list.push_back(di); + } + for (auto& di : s_arg_pb.di_list()) + { + device::AbstractAgv::DIItem d; + d.id = di.id(); + d.status = di.status(); + req.script_args.di_list.push_back(d); + } + } + + // -------- 原地旋转 -------- + if (pb_data.has_move_angle()) req.move_angle = pb_data.move_angle(); + if (pb_data.has_speed_w()) req.speed_w = pb_data.speed_w(); + if (pb_data.has_loc_mode()) req.loc_mode = pb_data.loc_mode(); + + // -------- 自由导航 -------- + if (pb_data.has_freego()) + { + auto& fg_pb = pb_data.freego(); + req.freeGo.x = fg_pb.x(); + req.freeGo.y = fg_pb.y(); + req.freeGo.theta = fg_pb.theta(); + } + + // 调用设备层 + device::AbstractAgv::RobotGoTargetRes res; + agv_dev->robotGoTarget(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); + + auto* header = response->mutable_header(); + header->set_success(true); + header->set_error_message(""); + setCurrentTimestamp(header->mutable_timestamp()); + + LOG(INFO) << "[RobotGoTarget] finish 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(std::string("exception:") + e.what()); + setCurrentTimestamp(header->mutable_timestamp()); + return grpc::Status::OK; + } + } + + // ===================== RobotControlStop ===================== + /** + * @brief 停止开环运动。 + * @param context 未使用。 + * @param request 包含设备 ID（无业务数据）。 + * @param response 返回停止结果。 + */ + grpc::Status gRPCAGVServiceImpl::RobotControlStop( + grpc::ServerContext* context, + const api::RobotControlStopCommand::Request* request, + api::RobotControlStopCommand::Feedback* response) { + + const auto& header = request->header(); + std::string device_id = header.device_id(); + auto* fb_header = response->mutable_header(); + + if (device_id.empty()) { + fb_header->set_error_message("Missing device_id"); + return grpc::Status::OK; + } + + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + fb_header->set_error_message("Device not found or not an AGV"); + return grpc::Status::OK; + } + + cmvr::device::AbstractAgv::RobotControlStopRes res; + agv->robotControlStop(res); + + auto* status = response->mutable_status(); + status->set_ret_code(res.ret_code); + status->set_create_on(res.create_on); + status->set_err_msg(res.err_msg); + + if (res.ret_code == 0) { + fb_header->set_error_message(""); + } else { + fb_header->set_error_message(res.err_msg.empty() ? "Stop motion failed" : res.err_msg); + } + + return grpc::Status::OK; + } + + // ===================== RobotTaskPause ===================== + /** + * @brief 暂停当前导航任务。 + * @param context 未使用。 + * @param request 包含设备 ID。 + * @param response 返回暂停结果。 + */ + grpc::Status gRPCAGVServiceImpl::RobotTaskPause( + grpc::ServerContext* context, + const api::RobotTaskPauseCommand::Request* request, + api::RobotTaskPauseCommand::Feedback* response) { + + const auto& header = request->header(); + std::string device_id = header.device_id(); + auto* fb_header = response->mutable_header(); + + if (device_id.empty()) { + fb_header->set_error_message("Missing device_id"); + return grpc::Status::OK; + } + + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + fb_header->set_error_message("Device not found or not an AGV"); + return grpc::Status::OK; + } + + cmvr::device::AbstractAgv::RobotTaskPauseRes res; + try { + agv->robotTaskPause(res); + } catch (const std::exception& e) { + fb_header->set_error_message(std::string("Exception: ") + e.what()); + return grpc::Status::OK; + } + + auto* status = response->mutable_status(); + status->set_ret_code(res.ret_code); + status->set_create_on(res.create_on); + status->set_err_msg(res.err_msg); + + if (res.ret_code != 0) { + fb_header->set_error_message(res.err_msg.empty() ? "Pause task failed" : res.err_msg); + } else { + fb_header->set_error_message(""); + } + + return grpc::Status::OK; + } + + // ===================== RobotTaskResume ===================== + /** + * @brief 继续当前导航任务。 + * @param context 未使用。 + * @param request 包含设备 ID。 + * @param response 返回继续结果。 + */ + grpc::Status gRPCAGVServiceImpl::RobotTaskResume( + grpc::ServerContext* context, + const api::RobotTaskResumeCommand::Request* request, + api::RobotTaskResumeCommand::Feedback* response) { + + const auto& header = request->header(); + std::string device_id = header.device_id(); + auto* fb_header = response->mutable_header(); + + if (device_id.empty()) { + fb_header->set_error_message("Missing device_id"); + return grpc::Status::OK; + } + + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + fb_header->set_error_message("Device not found or not an AGV"); + return grpc::Status::OK; + } + + cmvr::device::AbstractAgv::RobotTaskResumeRes res; + try { + agv->robotTaskResume(res); + } catch (const std::exception& e) { + fb_header->set_error_message(std::string("Exception: ") + e.what()); + return grpc::Status::OK; + } + + auto* status = response->mutable_status(); + status->set_ret_code(res.ret_code); + status->set_create_on(res.create_on); + status->set_err_msg(res.err_msg); + + if (res.ret_code != 0) { + fb_header->set_error_message(res.err_msg.empty() ? "Resume task failed" : res.err_msg); + } else { + fb_header->set_error_message(""); + } + + return grpc::Status::OK; + } + + // ===================== RobotTaskCancel ===================== + /** + * @brief 取消当前导航任务。 + * @param context 未使用。 + * @param request 包含设备 ID。 + * @param response 返回取消结果。 + */ + grpc::Status gRPCAGVServiceImpl::RobotTaskCancel( + grpc::ServerContext* context, + const api::RobotTaskCancelCommand::Request* request, + api::RobotTaskCancelCommand::Feedback* response) { + + const auto& header = request->header(); + std::string device_id = header.device_id(); + auto* fb_header = response->mutable_header(); + + if (device_id.empty()) { + fb_header->set_error_message("Missing device_id"); + return grpc::Status::OK; + } + + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + fb_header->set_error_message("Device not found or not an AGV"); + return grpc::Status::OK; + } + + cmvr::device::AbstractAgv::RobotTaskCancelRes res; + try { + agv->robotTaskCancel(res); + } catch (const std::exception& e) { + fb_header->set_error_message(std::string("Exception: ") + e.what()); + return grpc::Status::OK; + } + + auto* status = response->mutable_status(); + status->set_ret_code(res.ret_code); + status->set_create_on(res.create_on); + status->set_err_msg(res.err_msg); + + if (res.ret_code != 0) { + fb_header->set_error_message(res.err_msg.empty() ? "Cancel task failed" : res.err_msg); + } else { + fb_header->set_error_message(""); + } + + return grpc::Status::OK; + } + +} // namespace cmvr::service \ No newline at end of file diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto index d2a4fad9..8cfe89a1 100644 --- a/protos/cmvr/api/agv_command.proto +++ b/protos/cmvr/api/agv_command.proto @@ -1,109 +1,161 @@ +/** + * @file agv_command.proto + * @brief AGV 控制相关的 Protobuf 消息定义。 + * 涵盖仙工智能（Seer）SRC 系列控制器的主要 API 命令， + * 包括状态查询、电池、位置、地图管理、控制权、运动控制、导航任务等。 + * @note 所有命令均遵循仙工协议帧格式，通过 gRPC 服务（AgvService）对外暴露。 + */ + syntax = "proto3"; import "cmvr/api/common.proto"; package cmvr.api; -// AGV状态信息 +// ============================================================================ +// 1. 基本信息查询（命令码 1000, 0x03E8） +// ============================================================================ + +/** + * @brief AGV 基本信息响应结构。 + * @note 对应仙工 API 1000（0x03E8）的响应体，包含设备标识、软硬件版本、网络状态等。 + */ 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; // 错误信息 + optional string id = 1; ///< AGV 设备唯一 ID + optional string vehicle_id = 2; ///< 车辆编号（如 "agv_001"） + optional string version = 3; ///< 软件版本号 + optional string model = 4; ///< 车型型号（如 "SRC-1100"） + optional string dsp_version = 5; ///< DSP 固件版本 + optional string current_ip = 6; ///< 当前 IP 地址 + optional string mac = 7; ///< MAC 地址 + optional int32 rssi = 8; ///< Wi-Fi 信号强度（0~100） + optional int32 ret_code = 9; ///< 返回码，0 表示成功 + optional string err_msg = 10; ///< 错误信息（失败时填充） } -// 获取AGV状态命令 +/** + * @brief 获取 AGV 状态命令（请求/反馈）。 + * @note 请求携带设备 ID（header.device_id），反馈返回 AgvStatusInfo。 + */ message GetAgvStatusInfoCommand { message Request { - CommandHeader.Request header = 1; + CommandHeader.Request header = 1; ///< 公共请求头，包含 device_id } message Feedback { - CommandHeader.Feedback header = 1; - AgvStatusInfo status = 2; + CommandHeader.Feedback header = 1; ///< 公共响应头（success/error_message/timestamp） + AgvStatusInfo status = 2; ///< AGV 状态信息 } } -// ===================== 电池状态查询 ===================== +// ============================================================================ +// 2. 电池状态查询（命令码 1007, 0x03EF） +// ============================================================================ + +/** + * @brief 电池状态响应结构。 + * @note 对应仙工 API 1007（0x03EF），支持简单/全量两种模式。 + */ 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; // 错误信息 + optional double battery_level = 1; ///< 电池电量（0~1，对应 0%~100%） + 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; ///< 是否连接手动充电器（仅 SRC-2000） + optional bool auto_charge = 9; ///< 是否连接自动充电桩（仅 SRC-2000） + optional int32 battery_cycle = 10; ///< 电池循环次数（从 BMS 读取，可能不准） + optional string battery_user_data = 11; ///< 电池用户自定义数据 + optional string extra = 12; ///< 扩展字段（预留） + optional int32 ret_code = 13; ///< 返回码，0 表示成功 + optional string create_on = 14; ///< 数据创建时间戳（ISO 8601） + optional string err_msg = 15; ///< 错误信息 } +/** + * @brief 电池状态请求数据。 + */ message RobotStatusBatteryRequestData { - optional bool simple = 1; // true:仅简单数据 false:全量数据，默认false + optional bool simple = 1; ///< true=仅关键数据，false=全量数据（默认 false） } +/** + * @brief 电池状态查询命令。 + */ message RobotStatusBatteryCommand { message Request { - CommandHeader.Request header = 1; - RobotStatusBatteryRequestData data = 2; + CommandHeader.Request header = 1; ///< 公共请求头 + RobotStatusBatteryRequestData data = 2; ///< 请求参数 } message Feedback { - CommandHeader.Feedback header = 1; - AgvBatteryStatus status = 2; + CommandHeader.Feedback header = 1; ///< 公共响应头 + AgvBatteryStatus status = 2; ///< 电池状态信息 } } +// ============================================================================ +// 3. 机器人位置查询（命令码 1004, 0x03EC） +// ============================================================================ -// ===================== 机器人位置查询 ===================== +/** + * @brief 机器人位置响应结构。 + * @note 对应仙工 API 1004（0x03EC），返回世界坐标系下的位姿。 + */ 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; // 错误信息 + optional double x = 1; ///< X 坐标（米） + optional double y = 2; ///< Y 坐标（米） + optional double angle = 3; ///< 朝向角（弧度），逆时针为正 + optional double confidence = 4; ///< 定位置信度（0~1） + optional string current_station = 5; ///< 当前所在站点 ID（若在站点上） + optional string last_station = 6; ///< 上一个经过的站点 ID + optional int32 loc_method = 7; ///< 定位方式（0=自然轮廓, 1=反光柱, 2=二维码, 3=里程计...） + optional int32 ret_code = 8; ///< 返回码，0 表示成功 + optional string create_on = 9; ///< 时间戳 + optional string err_msg = 10; ///< 错误信息 } +/** + * @brief 位置查询命令（无请求数据）。 + */ message RobotStatusLocCommand { message Request { - CommandHeader.Request header = 1; + CommandHeader.Request header = 1; ///< 公共请求头 } message Feedback { - CommandHeader.Feedback header = 1; - AgvRobotLocation status = 2; + CommandHeader.Feedback header = 1; ///< 公共响应头 + AgvRobotLocation status = 2; ///< 位置信息 } } -// ===================== 地图下载 ===================== +// ============================================================================ +// 4. 地图下载（命令码 4011, 0x0FAB） +// ============================================================================ + +/** + * @brief 地图下载请求数据。 + */ message RobotConfigDownloadMapRequestData { - optional string map_name = 1; + optional string map_name = 1; ///< 要下载的地图名称（必填） } +/** + * @brief 地图下载响应结果。 + */ message AgvDownloadMapResult { - optional int32 ret_code = 1; - optional string create_on = 2; - optional string err_msg = 3; - optional string map_content = 4; + optional int32 ret_code = 1; ///< 返回码，0 表示成功 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 + optional string map_content = 4; ///< 地图 JSON 文件内容（成功时填充） } +/** + * @brief 地图下载命令。 + */ message RobotConfigDownloadMapCommand { message Request { CommandHeader.Request header = 1; @@ -116,25 +168,36 @@ message RobotConfigDownloadMapCommand { } -// 单条地图文件信息 +// ============================================================================ +// 5. 地图状态查询（命令码 1300, 0x0514） +// ============================================================================ + +/** + * @brief 单个地图文件信息。 + */ message MapFileInfo { - optional string name = 1; - optional string modified = 2; - optional int64 size = 3; + optional string name = 1; ///< 文件名 + optional string modified = 2; ///< 最后修改时间 + optional int64 size = 3; ///< 文件大小（字节） } -// 地图查询响应 +/** + * @brief 地图状态响应结构。 + * @note 对应仙工 API 1300（0x0514），返回当前地图及所有地图列表。 + */ 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; + optional string current_map = 1; ///< 当前加载的地图名称 + optional string current_map_md5 = 2; ///< 当前地图的 MD5 校验值 + repeated string maps = 3; ///< 所有地图名称列表 + repeated MapFileInfo map_files_info = 4; ///< 每个地图文件的详细信息 + optional int32 ret_code = 5; ///< 返回码，0 表示成功 + optional string create_on = 6; ///< 时间戳 + optional string err_msg = 7; ///< 错误信息 } -// 查询地图状态命令 +/** + * @brief 地图状态查询命令（无请求数据）。 + */ message RobotStatusMapCommand { message Request { CommandHeader.Request header = 1; @@ -146,17 +209,29 @@ message RobotStatusMapCommand { } -// ===================== 上传地图 ===================== +// ============================================================================ +// 6. 上传地图（命令码 4010, 0x0FAA） +// ============================================================================ + +/** + * @brief 上传地图请求数据。 + */ message RobotConfigUploadMapRequestData { - optional string map_content = 1; + optional string map_content = 1; ///< 完整的地图 JSON 字符串（必填） } +/** + * @brief 上传地图响应结果。 + */ message AgvUploadMapResult { - optional int32 ret_code = 1; - optional string create_on = 2; - optional string err_msg = 3; + optional int32 ret_code = 1; ///< 返回码，0 表示成功 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 } +/** + * @brief 上传地图命令。 + */ message RobotConfigUploadMapCommand { message Request { CommandHeader.Request header = 1; @@ -169,67 +244,65 @@ message RobotConfigUploadMapCommand { } -// ===================== 抢占控制权 ====================== -// 抢占控制权 请求参数 +// ============================================================================ +// 7. 抢占控制权（命令码 4005, 0x0FA5） +// ============================================================================ + +/** + * @brief 抢占控制权请求数据。 + */ message RobotConfigLockRequestData { - // 控制权抢占者名称，必填 - optional string nick_name = 1; + optional string nick_name = 1; ///< 抢占者昵称/标识（必填） } -// 抢占控制权 返回结果 +/** + * @brief 抢占控制权响应结果。 + */ message AgvLockResult { - // API错误码 - optional int32 ret_code = 1; - // API上传时间戳 - optional string create_on = 2; - // 错误描述信息 - optional string err_msg = 3; + optional int32 ret_code = 1; ///< 返回码，0 表示成功 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 } -// 抢占控制权 命令封装 +/** + * @brief 抢占控制权命令。 + */ message RobotConfigLockCommand { message Request { CommandHeader.Request header = 1; - // 请求业务参数 RobotConfigLockRequestData data = 2; } message Feedback { CommandHeader.Feedback header = 1; - // 抢占控制权响应数据 AgvLockResult status = 2; } } +// ============================================================================ +// 8. 查询当前控制权所有者（命令码 1060, 0x0424） +// ============================================================================ -// ===================== 查询当前控制权所有者 ====================== -// 查询当前控制权所有者响应结构体 +/** + * @brief 当前控制权所有者响应结构。 + * @note 对应仙工 API 1060（0x0424）。 + */ 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; + optional bool locked = 1; ///< 是否被锁定（有持有者） + optional string ip = 2; ///< 持有者 IP 地址 + optional int32 port = 3; ///< 持有者端口 + optional uint32 type = 4; ///< 持有者类型（0=default, 2=roboshop, 0xDD=srd） + optional string nick_name = 5; ///< 持有者昵称 + optional int64 time_t = 6; ///< 锁定时间戳（Unix 秒） + optional string desc = 7; ///< 描述信息 + optional int32 ret_code = 8; ///< 返回码，0 表示成功 + optional string create_on = 9; ///< 时间戳 + optional string err_msg = 10; ///< 错误信息 } - - -// 查询当前控制权指令 +/** + * @brief 查询控制权命令（无请求数据）。 + */ message RobotStatusCurrentLockCommand { message Request { CommandHeader.Request header = 1; @@ -241,27 +314,37 @@ message RobotStatusCurrentLockCommand { } -//==================== 开环速度运动控制 命令码：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) +// ============================================================================ +// 9. 开环速度运动控制（命令码 2010, 0x07DA） +// ============================================================================ + +/** + * @brief 开环速度运动控制请求数据。 + * @note 命令码 2010（0x07DA）。下发后会强制取消当前自动导航任务。 + * 多舵轮设备仅 vx/vy/w 生效；steer/real_steer 仅单舵轮设备有效。 + * duration = -1 表示持续运行直到下一次指令。 + */ 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; + optional double vx = 1; ///< X 方向线速度（m/s） + optional double vy = 2; ///< Y 方向线速度（m/s） + optional double w = 3; ///< 角速度（rad/s），逆时针为正 + optional double steer = 4; ///< 舵轮转向角（单舵轮，单位 rad） + optional double real_steer = 5; ///< 实际舵轮角度（单舵轮，优先级高于 steer） + optional int64 duration = 6; ///< 持续时间（ms），-1 表示一直运行 } +/** + * @brief 开环运动控制响应结果。 + */ message RobotMotionControlResult { - optional int32 ret_code = 1; - optional string create_on = 2; - optional string err_msg = 3; + optional int32 ret_code = 1; ///< 返回码，0 表示成功 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 } +/** + * @brief 开环运动控制命令。 + */ message RobotMotionControlCommand { message Request { CommandHeader.Request header = 1; @@ -274,18 +357,30 @@ message RobotMotionControlCommand { } -//==================== 切换载入地图 命令码：2022(0x07E6) ==================== -// 切换指定地图，合法地图会更新设备current_map字段 +// ============================================================================ +// 10. 切换载入地图（命令码 2022, 0x07E6） +// ============================================================================ + +/** + * @brief 切换地图请求数据。 + * @note 命令码 2022（0x07E6）。目标地图必须已存在于机器人中。 + */ message RobotLoadMapRequestData { - optional string map_name = 1; // 地图名，仅字母数字-_，不可缺省 + optional string map_name = 1; ///< 目标地图名称（必填，仅字母数字-_） } +/** + * @brief 切换地图响应结果。 + */ message RobotLoadMapResult { - optional int32 ret_code = 1; - optional string create_on = 2; - optional string err_msg = 3; + optional int32 ret_code = 1; ///< 返回码，0 表示成功 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 } +/** + * @brief 切换地图命令。 + */ message RobotLoadMapCommand { message Request { CommandHeader.Request header = 1; @@ -298,15 +393,25 @@ message RobotLoadMapCommand { } -//==================== 查询地图载入状态 命令码：1022(0x03FE) ==================== -// loadmap_status:0失败 1成功 2载入中；LOADING状态不可重定位 +// ============================================================================ +// 11. 查询地图载入状态（命令码 1022, 0x03FE） +// ============================================================================ + +/** + * @brief 地图载入状态响应。 + * @note 命令码 1022（0x03FE）。loadmap_status: 0=失败, 1=成功, 2=载入中。 + * 载入中（2）时禁止执行重定位操作。 + */ message RobotQueryLoadMapStatusResult { - optional int32 loadmap_status = 1; - optional int32 ret_code = 2; - optional string create_on = 3; - optional string err_msg = 4; + optional int32 loadmap_status = 1; ///< 0=失败, 1=成功, 2=载入中 + optional int32 ret_code = 2; ///< 返回码，0 表示成功 + optional string create_on = 3; ///< 时间戳 + optional string err_msg = 4; ///< 错误信息 } +/** + * @brief 查询地图载入状态命令（无请求数据）。 + */ message RobotQueryLoadMapStatusCommand { message Request { CommandHeader.Request header = 1; @@ -318,29 +423,40 @@ message RobotQueryLoadMapStatusCommand { } +// ============================================================================ +// 12. 查询站点列表（命令码 1301, 0x0515） +// ============================================================================ -//==================== 查询当前地图所有站点信息 命令码：1301(0x0515) ==================== +/** + * @brief 单个站点信息。 + */ 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; + optional string id = 1; ///< 站点唯一 ID + optional string type = 2; ///< 站点类型（如 "LocationMark", "ChargePoint"） + optional double x = 3; ///< X 坐标（米） + optional double y = 4; ///< Y 坐标（米） + optional double r = 5; ///< 朝向角（弧度） + optional string desc = 6; ///< 描述信息 + optional string executor = 7; ///< 执行器信息 + optional string prepoint = 8; ///< 前置点 ID + optional string recfile = 9; ///< 识别文件路径 + optional bool spin = 10; ///< 是否允许自旋 + optional bool use_down_pgv = 11; ///< 是否使用下视 PGV } +/** + * @brief 站点列表响应结果。 + */ message QueryStationListResult { - repeated StationItem stations = 1; - optional int32 ret_code = 2; - optional string create_on = 3; - optional string err_msg = 4; + repeated StationItem stations = 1; ///< 站点列表 + optional int32 ret_code = 2; ///< 返回码，0 表示成功 + optional string create_on = 3; ///< 时间戳 + optional string err_msg = 4; ///< 错误信息 } +/** + * @brief 查询站点列表命令（无请求数据）。 + */ message QueryStationListCommand { message Request { CommandHeader.Request header = 1; @@ -351,28 +467,44 @@ message QueryStationListCommand { } } -//==================== 指定路径导航 命令码：3066(0x0BFA) ==================== -// 单段导航任务单元 + +// ============================================================================ +// 13. 指定路径导航（命令码 3066, 0x0BFA） +// ============================================================================ + +/** + * @brief 单段导航任务单元。 + * @note 用于 3066 命令的 move_task_list 数组。 + * source_id 和 id 之间必须有直接路径，不可跳点。 + */ 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; + optional string task_id = 1; ///< 任务唯一 ID（必填） + optional string source_id = 2; ///< 起始站点 ID（必填） + optional string id = 3; ///< 目标站点 ID（必填） + optional string operation = 4; ///< 到达后执行的动作（如 "JackLoad"） + optional double jack_height = 5; ///< 顶升高度（米） } -// 指定路径导航请求体 +/** + * @brief 指定路径导航请求数据。 + */ message RobotGoTargetListRequestData { - repeated MoveTaskItem move_task_list = 1; + repeated MoveTaskItem move_task_list = 1; ///< 导航任务序列（顺序执行） } -// 指定路径导航返回结果 +/** + * @brief 指定路径导航响应结果。 + * @note ret_code=0 仅表示指令被接收，不表示执行完成。 + */ message RobotGoTargetListResult { - optional int32 ret_code = 1; - optional string create_on = 2; - optional string err_msg = 3; + optional int32 ret_code = 1; ///< 返回码，0 表示成功接收 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 } +/** + * @brief 指定路径导航命令。 + */ message RobotGoTargetListCommand { message Request { CommandHeader.Request header = 1; @@ -385,40 +517,390 @@ message RobotGoTargetListCommand { } -//==================== 查询当前导航状态 robot_status_task_req 1020(0x03FC) 应答0x2B0C ==================== -message QueryCurrentNavReqData { - optional bool simple = 1; +// ============================================================================ +// 14. 查询当前实时导航状态（命令码 1020, 0x03FC） +// ============================================================================ + +/** + * @brief 1020 命令请求数据。 + */ +message RobotStatusTaskReqData { + optional bool simple = 1; ///< true=仅返回 task_status，false=全量 } +/** + * @brief 导航任务中的料箱信息。 + */ message NavContainerItem { - optional string container_name = 1; - optional string desc = 2; - optional string goods_id = 3; - optional bool has_goods = 4; + optional string container_name = 1; ///< 料箱名称 + optional string desc = 2; ///< 描述 + optional string goods_id = 3; ///< 绑定的货物 ID + 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; +/** + * @brief 1020 命令响应数据。 + */ +message RobotStatusTaskResData { + optional int32 task_status = 1; ///< 0=NONE, 1=WAITING, 2=RUNNING, 3=SUSPENDED, 4=COMPLETED, 5=FAILED, 6=CANCELED + optional int32 task_type = 2; ///< 0=无, 1=自由导航到坐标, 2=自由导航到站点, 3=路径导航到站点, 7=平动转动 + optional string target_id = 3; ///< 目标站点 ID（task_type 为 2/3 时） + repeated double target_point = 4; ///< 目标坐标 [x, y, r]（task_type 为 1 时） + 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; ///< 返回码，0 表示成功 + optional string create_on = 10; ///< 时间戳 + optional string err_msg = 11; ///< 错误信息 } -message RobotStatusTaskCommand { +/** + * @brief 查询当前导航状态命令。 + */ +message RobotStatusTaskCurrentCommand { message Request { CommandHeader.Request header = 1; - QueryCurrentNavReqData data = 2; + RobotStatusTaskReqData data = 2; } message Feedback { CommandHeader.Feedback header = 1; - QueryCurrentNavResData data = 2; + RobotStatusTaskResData data = 2; } } + + +// ============================================================================ +// 15. 批量查询任务状态（命令码 1110, 0x0456） +// ============================================================================ + +/** + * @brief 1110 命令请求数据。 + */ +message QueryTaskStatusPackageReqData { + repeated string task_ids = 1; ///< 要查询的任务 ID 列表；为空则查所有未完成 + 最近一条完成 +} + +/** + * @brief 单条任务状态项。 + */ +message SingleTaskStatusItem { + optional string task_id = 1; ///< 任务 ID + optional int32 status = 2; ///< 状态码（同 task_status） + optional int32 type = 3; ///< 任务类型（同 task_type） +} + +/** + * @brief 任务状态包（包含最近完成和所有未完成的任务）。 + */ +message TaskStatusPackage { + optional string closest_target = 1; ///< 最近站点 ID + optional string source_name = 2; ///< 起点站点名称 + optional string target_name = 3; ///< 终点站点名称 + optional double percentage = 4; ///< 任务进度百分比（0~100） + optional double distance = 5; ///< 剩余距离（米） + optional string info = 6; ///< 附加信息 + repeated SingleTaskStatusItem task_status_list = 7; ///< 所有任务状态列表 +} + +/** + * @brief 1110 命令响应数据。 + */ +message QueryTaskStatusPackageResData { + optional TaskStatusPackage task_status_package = 1; ///< 任务状态包 + optional int32 ret_code = 2; ///< 返回码，0 表示成功 + optional string create_on = 3; ///< 时间戳 + optional string err_msg = 4; ///< 错误信息 +} + +/** + * @brief 批量查询任务状态命令。 + */ +message RobotStatusTaskPackageCommand { + message Request { + CommandHeader.Request header = 1; + QueryTaskStatusPackageReqData data = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + QueryTaskStatusPackageResData data = 2; + } +} + + +// ============================================================================ +// 16. 单点站点自动规划导航（命令码 3051, 0x0BEB） +// ============================================================================ + +/** + * @brief DI（数字输入）点位。 + */ +message DIItem { + int32 id = 1; ///< DI 编号 + bool status = 2; ///< 状态（true=高电平, false=低电平） +} + +/** + * @brief DO（数字输出）点位。 + */ +message DOItem { + int32 id = 1; ///< DO 编号 + bool status = 2; ///< 状态（true=高电平, false=低电平） +} + +/** + * @brief 音频播放参数。 + */ +message SoundArgs { + optional string name = 1; ///< 音频文件名 + optional int32 loop = 2; ///< 循环次数（0=播放一次, 1=循环） + optional int32 stop = 3; ///< 1=停止播放 +} + +/** + * @brief WaitDI 动作参数。 + */ +message WaitDIArgs { + repeated DIItem DI = 1; ///< 需要等待的 DI 条件列表 + optional double timeout = 2; ///< 超时时间（秒），0 表示无限等待 +} + +/** + * @brief SetDO 动作参数。 + */ +message SetDOArgs { + repeated DOItem do_list = 1; ///< 要设置的 DO 列表 +} + +/** + * @brief PGV（二维码）二次定位参数。 + */ +message PgvParam { + optional bool use_pgv = 1; ///< 是否使用上视 PGV + optional bool use_down_pgv = 2; ///< 是否使用下视 PGV + optional double pgv_adjust_dist = 3; ///< 最大调整半径（米） + optional double pgv_adjust_cx = 4; ///< 二维码坐标系 X 偏移（米） + optional double pgv_adjust_cy = 5; ///< 二维码坐标系 Y 偏移（米） + optional double pgv_x_adjust = 6; ///< 仅调整 X 方向误差（米） +} + +/** + * @brief 自由导航目标点（坐标 + 朝向）。 + */ +message FreeGoPoint { + double x = 1; ///< X 坐标（米） + double y = 2; ///< Y 坐标（米） + double theta = 3; ///< 朝向角（弧度） +} + +/** + * @brief 脚本自定义参数。 + */ +message ScriptArgs { + map str_kv = 1; ///< 字符串键值对 + map num_kv = 2; ///< 数字键值对 + repeated DOItem do_list = 3; ///< DO 列表 + repeated DIItem di_list = 4; ///< DI 列表 +} + +/** + * @brief 3051 命令请求数据（单点站点自动规划导航）。 + * @warning 严禁用于多车调度场景，仅限单车测试/验证。 + * 下发此任务会取消当前正在执行的任务（不排队）。 + * 成功下发后会自动清除指定的 warning/error 报错码。 + * 自由导航（freego）仅支持双轮差速底盘。 + */ +message RobotGoTargetReqData { + // -------- 必填字段 -------- + string source_id = 1; ///< 起始站点 ID（"SELF_POSITION" 表示当前位置） + string id = 2; ///< 目标站点 ID（"SELF_POSITION" 表示原地执行 operation） + optional string task_id = 3; ///< 任务 ID（建议提供） + + // -------- 运动通用参数 -------- + optional double angle = 4; ///< 目标点朝向角（弧度） + optional string method = 5; ///< "forward"（正走）或 "backward"（倒走） + optional double max_speed = 6; ///< 最大线速度（m/s），0 使用默认 + optional double max_wspeed = 7; ///< 最大角速度（rad/s） + optional double max_acc = 8; ///< 最大加速度（m/s²） + optional double max_wacc = 9; ///< 最大角加速度（rad/s²） + optional int64 duration = 10; ///< 到达后等待时间（毫秒） + optional int32 orientation = 11; ///< 全向车保持的角度（暂不使用） + optional bool spin = 12; ///< 是否随动（托盘旋转） + optional int64 delay = 13; ///< 延迟结束导航状态（毫秒），缺省 0 + optional int32 start_rot_dir = 14; ///< 起步旋转方向：-1=顺时针, 0=就近, 1=逆时针 + optional int32 end_rot_dir = 15; ///< 到点旋转方向：-1=顺时针, 0=就近, 1=逆时针 + optional double reach_dist = 16; ///< 到点位置精度（米） + optional double reach_angle = 17; ///< 到点角度精度（弧度） + optional string skill_name = 18; ///< 技能名称："Action" 或 "GotoSpecifiedPose" + + // -------- PGV -------- + optional PgvParam pgv = 19; ///< 二次定位参数 + + // -------- 设备动作 -------- + optional string operation = 20; ///< 操作类型（JackLoad/ForkUnload/RollerLoad/HookLoad/WaitDI/SetDO/sound/Script 等） + optional double jack_height = 21; ///< 顶升高度（米） + optional double start_height = 22; ///< 货叉起步前举升高度（米） + optional double end_height = 23; ///< 货叉到点后举升高度（米） + optional double fork_mid_height = 24; ///< 货叉行走过程中举升高度（米） + optional double fork_dist = 25; ///< 货叉前移距离（米） + optional string direction = 26; ///< 方向："left"/"right"/"front"/"back" + optional bool recognize = 27; ///< 是否启用视觉识别 + optional string recfile = 28; ///< 识别文件路径（如 "shelf/s0002.shelf"） + + // -------- 音频 -------- + optional SoundArgs sounds_args = 29; ///< 音频参数 + + // -------- WaitDI / SetDO -------- + optional WaitDIArgs wait_di_args = 30; ///< WaitDI 参数 + optional SetDOArgs set_do_args = 31; ///< SetDO 参数 + + // -------- 脚本 -------- + optional string script_name = 32; ///< 脚本文件名 + optional ScriptArgs script_args = 33; ///< 脚本参数 + optional int32 script_stage = 34; ///< 执行阶段：0=导航前, 1=导航中, 2=导航后, 3=脚本控制导航 + + // -------- 原地旋转（GoByOdometer） -------- + optional double move_angle = 35; ///< 旋转角度（弧度） + optional double speed_w = 36; ///< 旋转角速度（rad/s） + optional int32 loc_mode = 37; ///< 定位模式：1=激光定位, 0=里程定位 + + // -------- 自由导航（仅双轮差速） -------- + optional FreeGoPoint freego = 38; ///< 自由导航目标（若提供则忽略 id 字段） +} + +/** + * @brief 3051 命令响应数据。 + */ +message RobotGoTargetResData { + optional int32 ret_code = 1; ///< 返回码，0 表示成功接收 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 +} + +/** + * @brief 单点导航命令。 + */ +message RobotGoTargetCommand { + message Request { + CommandHeader.Request header = 1; + RobotGoTargetReqData data = 2; + } + message Feedback { + CommandHeader.Feedback header = 1; + RobotGoTargetResData data = 2; + } +} + + +// ============================================================================ +// 17. 停止开环运动（命令码 2000, 0x07D0） +// ============================================================================ + +/** + * @brief 停止开环运动请求数据（无业务字段）。 + * @note 命令码 2000（0x07D0）。下发零速度指令，终止当前持续运动。 + */ +message RobotControlStopRequestData +{ +} + +/** + * @brief 停止开环运动响应结果。 + */ +message RobotControlStopResult +{ + optional int32 ret_code = 1; ///< 返回码，0 表示成功 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 +} + +/** + * @brief 停止开环运动命令。 + */ +message RobotControlStopCommand +{ + message Request + { + CommandHeader.Request header = 1; + RobotControlStopRequestData data = 2; + } + message Feedback + { + CommandHeader.Feedback header = 1; + RobotControlStopResult status = 2; + } +} + + +// ============================================================================ +// 18. 暂停/继续/取消导航任务（命令码 3001/3002/3003） +// ============================================================================ + +/** + * @brief 暂停当前导航任务（命令码 3001, 0x0BB9）。 + * @note 请求无业务数据。 + */ +message RobotTaskPauseCommand { + message Request { + CommandHeader.Request header = 1; + // 无 data 字段 + } + message Feedback { + CommandHeader.Feedback header = 1; + /** + * @brief 暂停结果。 + */ + message Result { + optional int32 ret_code = 1; ///< 返回码，0 表示成功 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 + } + Result status = 2; + } +} + +/** + * @brief 继续当前导航任务（命令码 3002, 0x0BBA）。 + * @note 请求无业务数据。 + */ +message RobotTaskResumeCommand { + message Request { + CommandHeader.Request header = 1; + // 无 data 字段 + } + message Feedback { + CommandHeader.Feedback header = 1; + /** + * @brief 继续结果。 + */ + message Result { + optional int32 ret_code = 1; ///< 返回码，0 表示成功 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 + } + Result status = 2; + } +} + +/** + * @brief 取消当前导航任务（命令码 3003, 0x0BBB）。 + * @note 请求无业务数据。 + */ +message RobotTaskCancelCommand { + message Request { + CommandHeader.Request header = 1; + // 无 data 字段 + } + message Feedback { + CommandHeader.Feedback header = 1; + /** + * @brief 取消结果。 + */ + message Result { + optional int32 ret_code = 1; ///< 返回码，0 表示成功 + optional string create_on = 2; ///< 时间戳 + optional string err_msg = 3; ///< 错误信息 + } + Result status = 2; + } +} \ No newline at end of file diff --git a/protos/cmvr/api/agv_service.proto b/protos/cmvr/api/agv_service.proto index 6434904d..521ac025 100644 --- a/protos/cmvr/api/agv_service.proto +++ b/protos/cmvr/api/agv_service.proto @@ -1,3 +1,8 @@ +/** + * @file agv_service.proto + * @brief 定义AGV服务的gRPC接口，聚合所有AGV相关命令。 + * 服务端实现类为 gRPCAGVServiceImpl。 + */ syntax = "proto3"; import "cmvr/api/agv_command.proto"; @@ -45,16 +50,25 @@ service AgvService { rpc RobotGoTargetList(RobotGoTargetListCommand.Request) returns (RobotGoTargetListCommand.Feedback); // 查询当前导航状态 1020 robot_status_task_req - rpc RobotStatusTask(RobotStatusTaskCommand.Request) returns (RobotStatusTaskCommand.Feedback); + // 1020 查询当前实时导航 robot_status_task_req + rpc RobotStatusTaskCurrent(RobotStatusTaskCurrentCommand.Request) returns (RobotStatusTaskCurrentCommand.Feedback); + + // 批量查询任务状态 1110 robot_status_task_status_package_req + rpc RobotStatusTaskPackage(RobotStatusTaskPackageCommand.Request) returns (RobotStatusTaskPackageCommand.Feedback); + + + // 3051 单点站点自动规划导航 robot_task_gotarget_req 0x0BEB + rpc RobotGoTarget(RobotGoTargetCommand.Request) returns (RobotGoTargetCommand.Feedback); + // 0x07D0 停止开环运动 + rpc RobotControlStop(RobotControlStopCommand.Request) returns (RobotControlStopCommand.Feedback); - - - - - - -} - + // 暂停当前导航任务 命令码 3001 (0x0BB9) + rpc RobotTaskPause(RobotTaskPauseCommand.Request) returns (RobotTaskPauseCommand.Feedback); + // 继续当前导航任务 命令码 3002 (0x0BBA) + rpc RobotTaskResume(RobotTaskResumeCommand.Request) returns (RobotTaskResumeCommand.Feedback); + // 取消当前导航任务 命令码 3003 (0x0BBB) + rpc RobotTaskCancel(RobotTaskCancelCommand.Request) returns (RobotTaskCancelCommand.Feedback); +} \ No newline at end of file