SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

agv

Browse Source
This commit is contained in:
tankaitao 2026-06-17 14:50:37 +08:00
parent 59944d6c0d
commit f6817c7b1c
23 changed files with 3800 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -122,6 +122,8 @@ target_link_libraries(cmvr_es PRIVATE
cmvr_es::device::humanoid_robot
cmvr_es::common
cmvr_es::applications
cmvr_es::device::agv_src1100
)
install(TARGETS cmvr_es RUNTIME DESTINATION bin)

30
cmvr-es/common/config/agv_config/agv_config.pb.txt Normal file
View File

@ -0,0 +1,30 @@
src1100_agvs {
id: "agv_src1100"
ip: "192.168.192.5"
port_status: 19301
port_control: 19301
port_nav: 19301
port_config: 19301
port_other: 19301
port_push: 19301
enable: true
}
src1100_agvs {
id: "agv_src2200"
ip: "192.168.192.5"
port_status: 19204
port_control: 19205
port_nav: 19206
port_config: 19207
port_other: 19210
port_push: 19301
enable: true
}

7
cmvr-es/common/utils/config_helper/include/config_helper.h
View File

@ -10,6 +10,7 @@
#include "cmvr/config/microphone_config/microphone_config.pb.h"
#include "cmvr/config/speaker_config/speaker_conifg.pb.h"
#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h"
#include "cmvr/config/agv_config/agv_config.grpc.pb.h"
#include "cmvr/config/biohead_config/biohead_config.pb.h"
#include "cmvr/config/robot_config/robot_config.pb.h"
#include "cmvr/config/server_config/server_config.pb.h"
@ -85,6 +86,12 @@ namespace cmvr
return SET_CONFIG(config, touch_screen_app_config_file);
}
static bool getAgvConfig(config::AGVConfig& config)
{
return GET_CONFIG(agv_config_file, config);
}
static bool getHeadConfig(config::HeadConfig& config)
{
return GET_CONFIG(biohead_config_file, config);

2
cmvr-es/common/utils/config_helper/include/config_setting.h
View File

@ -6,6 +6,8 @@ DECLARE_string(dexhand_config_file);
DECLARE_string(microphone_config_file);
DECLARE_string(speaker_config_file);
DECLARE_string(touch_screen_app_config_file);
DECLARE_string(agv_config_file);
DECLARE_string(biohead_config_file);
DECLARE_string(robot_config_file);
DECLARE_string(server_config_file);

4
cmvr-es/common/utils/config_helper/src/config_setting.cpp
View File

@ -89,6 +89,10 @@ DEFINE_string(touch_screen_app_config_file,
basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt",
"The configuration file for TouchScreenApp");
DEFINE_string(agv_config_file,
basePath() + "agv_config/agv_config.pb.txt",
"AGV config file");
DEFINE_string(biohead_config_file,
basePath() + "biohead_config/biohead_config.pb.txt",
"The configuration file for BioHead");

1
cmvr-es/device_manager/CMakeLists.txt
View File

@ -15,6 +15,7 @@ target_link_libraries(device_manager PRIVATE
cmvr_es::device::rh56dftp_dexhand
cmvr_es::device::px_6ax_gen3
cmvr::device::head_esp32
cmvr_es::device::agv_src1100
osqp
cmvr_es::device::humanoid_robot
# cmvr_es::device::aubo_robot

2
cmvr-es/device_manager/include/device_factory.h
View File

@ -28,7 +28,7 @@ namespace cmvr::device {
template <typename DeviceType, typename ConfigType>
std::shared_ptr<DeviceType> createFromConfig(const ConfigType& cfg);
private:
std::shared_ptr<AbstractAGV> create_agv_(const XmlNode& cfg);
std::shared_ptr<AbstractAgv> create_agv_(const XmlNode& cfg);
std::shared_ptr<AbstractBattery> create_battery_(const XmlNode& cfg);
std::shared_ptr<AbstractCamera> create_camera_(const XmlNode& cfg);
std::shared_ptr<AbstractDexHand> create_dexhand_(const XmlNode& cfg);

2
cmvr-es/device_manager/include/device_manager.h
View File

@ -16,7 +16,7 @@
namespace cmvr::device {
using DeviceVariant = std::variant<
std::shared_ptr<AbstractAGV>,
std::shared_ptr<AbstractAgv>,
std::shared_ptr<AbstractBattery>,
std::shared_ptr<AbstractCamera>,
std::shared_ptr<AbstractDexHand>,

21
cmvr-es/device_manager/src/device_factory.cpp
View File

@ -15,12 +15,13 @@
//#include "robot/ti5_robot/ti5_robot.h"
#include "cmvr/api/system_command.pb.h"
#include "data_center/include/motors_info.h"
#include "devices/agv/agv_src1100/include/agv_src1100.h"
//#include "devices/robot/aubo_robot/include/aubo_robot.h"
using namespace std;
using namespace cmvr::device;
template std::shared_ptr<AbstractAGV> DeviceFactory::create<AbstractAGV>(const XmlNode&);
template std::shared_ptr<AbstractAgv> DeviceFactory::create<AbstractAgv>(const XmlNode&);
template std::shared_ptr<AbstractBattery> DeviceFactory::create<AbstractBattery>(const XmlNode&);
template std::shared_ptr<AbstractCamera> DeviceFactory::create<AbstractCamera>(const XmlNode&);
template std::shared_ptr<AbstractDexHand> DeviceFactory::create<AbstractDexHand>(const XmlNode&);
@ -32,7 +33,7 @@ template std::shared_ptr<AbstractBiohead> DeviceFactory::create<AbstractBiohead>
template <typename DeviceType>
std::shared_ptr<DeviceType> DeviceFactory::create(const XmlNode& cfg) {
if constexpr (std::is_same_v<DeviceType, AbstractAGV>) {
if constexpr (std::is_same_v<DeviceType, AbstractAgv>) {
return create_agv_(cfg);
} else if constexpr (std::is_same_v<DeviceType, AbstractBattery>) {
return create_battery_(cfg);
@ -57,7 +58,7 @@ std::shared_ptr<DeviceType> DeviceFactory::create(const XmlNode& cfg) {
}
}
std::shared_ptr<AbstractAGV> DeviceFactory::create_agv_(const XmlNode& cfg) {
std::shared_ptr<AbstractAgv> DeviceFactory::create_agv_(const XmlNode& cfg) {
return nullptr;
}
@ -184,6 +185,7 @@ template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<Abstra
template std::shared_ptr<AbstractDexHand> DeviceFactory::createFromConfig<AbstractDexHand>(const config::PX6AXGen3& cfg);
template std::shared_ptr<AbstractMicrophone> DeviceFactory::createFromConfig<AbstractMicrophone>(const config::FFMpegMicroPhoneConfig& cfg);
template std::shared_ptr<AbstractSpeaker> DeviceFactory::createFromConfig<AbstractSpeaker>(const config::FFMpegSpeakerConfig& cfg);
template std::shared_ptr<AbstractAgv> DeviceFactory::createFromConfig<AbstractAgv>(const config::AGVsrc1100Config& cfg);
template std::shared_ptr<AbstractBiohead> DeviceFactory::createFromConfig<AbstractBiohead>(const config::BioHeadConfig& cfg);
template std::shared_ptr<AbstractRobot> DeviceFactory::createFromConfig<AbstractRobot>(const config::HumanRobotConfig& cfg);
template std::shared_ptr<AbstractRobot> DeviceFactory::createFromConfig<AbstractRobot>(const config::EthercatRobotConfig& cfg);
@ -280,6 +282,18 @@ std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cf
throw runtime_error("[DeviceFactory]: Unsupported microphone device type");
}
}
else if constexpr (std::is_same_v<DeviceType, AbstractAgv>)
{
if constexpr (std::is_same_v<ConfigType, config::AGVsrc1100Config>)
{
return std::make_shared<AgvSrc1100>(cfg);
}
else
{
LOG(ERROR) << "[DeviceFactory]: Unsupported agv device type ";
throw runtime_error("[DeviceFactory]: Unsupported agv device type");
}
}
else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type";
throw runtime_error("[DeviceFactory]: Unsupported device type");
@ -289,4 +303,3 @@ std::shared_ptr<DeviceType> DeviceFactory::createFromConfig(const ConfigType& cf
return nullptr;
}
}

43
cmvr-es/device_manager/src/device_manager.cpp
View File

@ -9,7 +9,7 @@ using namespace std;
using namespace cmvr::device;
using namespace cmvr::device;
template std::shared_ptr<AbstractAGV> DeviceManager::getDevice(const std::string& device_id);
template std::shared_ptr<AbstractAgv> DeviceManager::getDevice(const std::string& device_id);
template std::shared_ptr<AbstractBattery> DeviceManager::getDevice(const std::string& device_id);
template std::shared_ptr<AbstractCamera> DeviceManager::getDevice(const std::string& device_id);
template std::shared_ptr<AbstractDexHand> DeviceManager::getDevice(const std::string& device_id);
@ -152,7 +152,7 @@ void DeviceManager::getDeviceList(std::list<std::pair<std::string, std::string>>
string dev_type;
std::visit([&](const auto& ptr) {
using T = std::decay_t<decltype(ptr)>;
if constexpr (std::is_same_v<T, std::shared_ptr<AbstractAGV>>) {
if constexpr (std::is_same_v<T, std::shared_ptr<AbstractAgv>>) {
dev_type = "AGV";
} else if constexpr (std::is_same_v<T, std::shared_ptr<AbstractBattery>>) {
dev_type = "Battery";
@ -223,12 +223,12 @@ void DeviceManager::init_devices_() {
LOG(ERROR) << "[DeviceManager]: Duplicate AGV Device ID" << id;
throw runtime_error("[DeviceManager]: Duplicate AGV Device ID" + id);
}
auto device = dev_factory_->create<AbstractAGV>(node);
auto device = dev_factory_->create<AbstractAgv>(node);
if (device == nullptr) {
throw std::runtime_error("[DeviceManager]: Factory returned nullptr for device ID: " + id);
}
devices_[id] = device;
std::get<std::shared_ptr<AbstractAGV>>(devices_[id])->init();
std::get<std::shared_ptr<AbstractAgv>>(devices_[id])->init();
LOG(INFO) << "[DeviceManager]: Init AGV " << id << " Success";
}
@ -312,24 +312,6 @@ void DeviceManager::init_devices_() {
LOG(INFO) << "[DeviceManager]: Init Robot " << id << " Success";
}
// EYOU的先用新版的
config::RobotConfig robot_config;
ConfigHelper::getRobotConfig(robot_config);
for (int i = 0; i < robot_config.ethercat_robots().size(); i++) {
auto robot = robot_config.ethercat_robots(i);
if (!robot.enable())
continue;
auto id = robot.id();
if (devices_.count(id)) {
LOG(ERROR) << "[DeviceManager]: Duplicate Robot Device ID" << id;
throw runtime_error("[DeviceManager]: Duplicate Robot Device ID" + id);
}
std::cout << "DexHand[" << i << "]: " << id << std::endl;
auto device = dev_factory_->createFromConfig<AbstractRobot,config::EthercatRobotConfig>(robot);
devices_[id] = device;
std::get<std::shared_ptr<AbstractRobot>>(devices_[id])->init();
}
config::MicroPhoneConfig micro_phone_config;
ConfigHelper::getMicroPhonesConfig(micro_phone_config);
for (int i = 0; i < micro_phone_config.ffmpeg_microphones().size(); i++) {
@ -380,6 +362,23 @@ void DeviceManager::init_devices_() {
devices_[id] = device;
std::get<std::shared_ptr<AbstractBiohead>>(devices_[id])->init();
}
config::AGVConfig agv_config;
ConfigHelper::getAgvConfig(agv_config);
for (int i = 0; i < agv_config.src1100_agvs().size(); i++) {
auto agv = agv_config.src1100_agvs(i);
if (!agv.enable())
continue;
auto id = agv.id();
if (devices_.count(id)) {
LOG(ERROR) << "[DeviceManager]: Duplicate agv Device ID" << id;
throw runtime_error("[DeviceManager]: Duplicate agv Device ID" + id);
}
std::cout << "AgvSrc1100[" << i << "]: " << id << std::endl;
auto device = dev_factory_->createFromConfig<AbstractAgv,config::AGVsrc1100Config>(agv);
devices_[id] = device;
std::get<std::shared_ptr<AbstractAgv>>(devices_[id])->init();
}
}
catch (const exception& e) {
LOG(ERROR) << e.what();

1
cmvr-es/devices/CMakeLists.txt
View File

@ -6,3 +6,4 @@ add_subdirectory(biohead)
add_subdirectory(robot)
add_subdirectory(canbus)
add_subdirectory(motor)
add_subdirectory(agv)

2
cmvr-es/devices/agv/CMakeLists.txt Normal file
View File

@ -0,0 +1,2 @@
add_subdirectory(agv_src1100)

448
cmvr-es/devices/agv/abstract_agv.h
View File

@ -1,36 +1,438 @@
//
// Created by xtkuang on 2025/5/8.
// Created by xtkuang on 2025/5/6.
//
#ifndef CMVR_ES_ABSTRACT_AGV_H
#define CMVR_ES_ABSTRACT_AGV_H
#pragma once
#include "../abstract_device.h"
#include <cmath>
#include <cstdint>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
#include <json/json.h>
#include "devices/abstract_device.h"
// 注意：这里只include，不使用
#include "cmvr/config/agv_config/agv_config.pb.h"
namespace cmvr::device {
class AbstractAGV: public AbstractDevice {
class AbstractAgv : public AbstractDevice {
public:
explicit AbstractAGV(const XmlNode &config): AbstractDevice(config) {};
~AbstractAGV() override=default;
virtual void getState(AGVState &state) {}
// navigation
virtual void eStop() {}
virtual void goHome() {}
virtual void moveto(math::Pose2d &location, double speed_ratio) {}
virtual void setVelocity(math::Vec3 linear, math::Vec3 angular) {}
// map
virtual void initMap(float resolution, int width, int height) {}
virtual void updateMap() {}
virtual void saveMap(const std::string& file_path) {}
virtual void loadMap(const std::string& file_path) {}
protected:
AGVState state_;
enum class Status {
CREATED,
INITIALIZED,
RUNNING,
PAUSED,
STOPPED,
FAULT
};
//查询机器人信息
struct AgvStatusInfo {
std::string id;
std::string vehicle_id;
std::string robot_note;
std::string version;
std::string model;
std::string dsp_version;
std::string gyro_version;
std::string map_version;
std::string model_version;
std::string netprotocol_version;
std::string modbus_version;
std::string current_map;
std::string current_map_md5;
std::string model_md5;
std::string ssid;
int rssi = 0;
std::string ap_addr;
std::string current_ip;
std::string mac;
std::string echoid_type;
std::string echoid;
int ret_code = 0;
std::string err_msg;
bool isValid() const {
return !id.empty() && ret_code == 0;
}
};
// 电池状态信息
struct BatteryStatus
{
double battery_level = 0.0;
double battery_temp = 0.0;
bool charging = false;
double voltage = 0.0;
double current = 0.0;
double max_charge_voltage = -1.0;
double max_charge_current = -1.0;
bool manual_charge = false;
bool auto_charge = false;
int32_t battery_cycle = 0;
std::string battery_user_data;
std::string extra;
int32_t ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 机器人位置信息
struct RobotLocation
{
double x = 0.0;
double y = 0.0;
double angle = 0.0;
double confidence = 0.0;
std::string current_station;
std::string last_station;
int32_t loc_method = 0;
int32_t ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 地图下载结果
struct DownloadMapResult
{
std::string map_name;
std::string map_content;
int32_t ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 单地图文件信息
struct MapFileInfo
{
std::string name;
std::string modified;
int64_t size = 0;
};
// 地图整体状态
struct MapStatus
{
std::string current_map;
std::string current_map_md5;
std::vector<std::string> maps;
std::vector<MapFileInfo> map_files_info;
int32_t ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 上传地图结果
struct UploadMapResult
{
int32_t ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 抢占控制权返回结果
struct LockResult
{
int32_t ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 当前控制权信息
struct CurrentLockStatus
{
bool locked = false;
std::string ip;
int32_t port = 0;
uint8_t type = 0;
std::string nick_name;
int64_t time_t = 0;
std::string desc;
int32_t ret_code = 0;
std::string create_on;
std::string err_msg;
};
//==================== 新增：运动控制结构体 ====================
// 开环运动下发参数结构体
struct MotionCtrlReq
{
double vx = 0.0;
double vy = 0.0;
double w = 0.0;
double steer = 0.0;
double real_steer = 0.0;
int64_t duration = -1; // -1 代表缺省，使用设备默认时长
};
// 运动控制返回结果结构体
struct MotionCtrlRes
{
int ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 切换地图入参
struct LoadMapReq
{
std::string map_name;
};
// 切换地图返回结果
struct LoadMapRes
{
int ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 查询地图载入状态返回
struct QueryLoadMapStatusRes
{
int32_t loadmap_status = 0; //0失败 1成功 2载入中
int ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 单个站点信息
struct StationItem
{
std::string id;
std::string type;
double x = 0.0;
double y = 0.0;
double r = 0.0;
std::string desc;
std::string executor;
std::string prepoint;
std::string recfile;
bool spin = false;
bool use_down_pgv = false;
};
// 查询站点列表返回结果
struct QueryStationRes
{
std::vector<StationItem> stations;
int ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 单段导航任务
struct MoveTaskItem
{
std::string task_id;
std::string source_id;
std::string id;
std::string operation;
double jack_height = 0.0;
};
// 指定路径导航入参
struct GoTargetListReq
{
std::vector<MoveTaskItem> move_task_list;
};
// 指定路径导航返回
struct GoTargetListRes
{
int ret_code = 0;
std::string create_on;
std::string err_msg;
};
// 导航料箱信息
struct NavContainerItem
{
std::string container_name;
std::string desc;
std::string goods_id;
bool has_goods = false;
};
// 1020 查询当前导航入参
struct RobotStatusTaskReq
{
bool simple = false;
};
// 1020 查询当前导航出参
struct RobotStatusTaskRes
{
int task_status = 0;
int task_type = 0;
std::string target_id;
std::vector<double> target_point;
std::vector<std::string> finished_path;
std::vector<std::string> unfinished_path;
std::string move_status_info;
std::vector<NavContainerItem> containers;
int ret_code = 0;
std::string create_on;
std::string err_msg;
};
using AgvState = AgvStatusInfo;
public:
AbstractAgv() = default;
~AbstractAgv() override = default;
AbstractAgv(const AbstractAgv&) = delete;
AbstractAgv& operator=(const AbstractAgv&) = delete;
AbstractAgv(AbstractAgv&&) = default;
AbstractAgv& operator=(AbstractAgv&&) = default;
// 纯虚接口
virtual Status state() const = 0;
virtual std::string lastError() const = 0;
void start() override = 0;
void stop() override = 0;
void update() override = 0;
virtual void getState(AgvState& state) {
state = AgvState{};
getStatusInfo(state);
}
virtual void getStatusInfo(AgvStatusInfo& info) = 0;
// 查询电池状态
virtual void getBatteryStatus(BatteryStatus& info, bool simple = false) = 0;
// 查询机器人位置
virtual void getRobotLocation(RobotLocation& info) = 0;
// 下载地图
virtual void downloadMap(DownloadMapResult& info, const std::string& map_name) = 0;
// 查询地图状态
virtual void getMapStatus(MapStatus& info) = 0;
// 上传地图
virtual void uploadMap(UploadMapResult& info, const std::string& map_json) = 0;
/**
* @brief 抢占机器人控制权
* @param info 出参，抢占结果
* @param nick_name 入参，抢占者名称
*/
virtual void lockRobotControl(LockResult& info, const std::string& nick_name) = 0;
/**
* @brief 下发开环速度运动指令 命令码2010(0x07DA)，使用控制端口sock_control_
* @param res 出参 运动指令返回结果
* @param req 入参 vx/vy/w/steer/real_steer/duration运动参数
* @note 下发后会强制中断当前自动导航任务；多舵轮设备仅vx/vy/w生效
*/
virtual void robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) = 0;
/**
* @brief 切换载入地图 命令码2022(0x07E6)，控制端口sock_control_
* @param res 出参 执行结果
* @param req 入参 目标地图名称
*/
virtual void robotLoadMap(LoadMapRes& res, const LoadMapReq& req) = 0;
/**
* @brief 1020(0x03FC) robot_status_task_req 查询当前导航状态 状态端口 sock_status_
* @param res 输出导航全量状态
* @param req simple=true 仅返回task_status
*/
virtual void robotStatusTask(RobotStatusTaskRes& res, const RobotStatusTaskReq& req) = 0;
virtual void move(double vx, double vy, double vw) {
throw std::logic_error("move not implemented");
}
virtual void stopMove() {
throw std::logic_error("stopMove not implemented");
}
/**
* @brief 查询当前机器人控制权持有者
* @param info 出参 控制权信息
*/
virtual void getCurrentLockStatus(CurrentLockStatus& info) = 0;
/**
* @brief 查询地图载入状态 命令码1022(0x03FE)，状态端口sock_status_
* @param res 出参 地图加载状态结果
* @note loadmap_status=2载入中禁止重定位
*/
virtual void queryLoadMapStatus(QueryLoadMapStatusRes& res) = 0;
/**
* @brief 查询当前地图全部站点信息 命令码1301(0x0515)，状态端口sock_status_
* @param res 出参 站点数组与执行结果
*/
virtual void queryStationList(QueryStationRes& res) = 0;
/**
* @brief 指定路径导航 命令码3066(0x0BFA)，控制端口sock_control_
* @param res 出参 下发任务结果
* @param req 入参 多段站点导航序列
* @note task_id、source_id、id 必填；任务串行追加执行
*/
virtual void robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) = 0;
protected:
std::string id_;
std::string ip_;
// ========================
// 多端口配置（对应 proto）
// ========================
int port_status_{0}; // 19204 状态查询
int port_control_{0}; // 19205 控制API
int port_nav_{0}; // 19206 导航API
int port_config_{0}; // 19207 配置API
int port_other_{0}; // 19210 其他API
int port_push_{0}; // 19301 推送
bool enable_{false};
};
} // namespace cmvr::device
#endif // CMVR_ES_ABSTRACT_AGV_H

20
cmvr-es/devices/agv/agv_src1100/CMakeLists.txt Normal file
View File

@ -0,0 +1,20 @@
add_library(agv_src1100 SHARED src/agv_src1100.cpp)
target_include_directories(agv_src1100 PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
${CMAKE_CURRENT_SOURCE_DIR}/include
${JSONCPP_INCLUDE_DIRS}
)
add_library(cmvr_es::device::agv_src1100 ALIAS agv_src1100)
# 关键修改：PRIVATE -> PUBLIC
target_link_libraries(agv_src1100 PUBLIC
jsoncpp
cmvr_es::common
cmvr_es::proto
glog
pthread
)
install(TARGETS agv_src1100 LIBRARY DESTINATION lib)

88
cmvr-es/devices/agv/agv_src1100/include/agv_src1100.h Normal file
View File

@ -0,0 +1,88 @@
//
// Created by linbo on 2025/6/20.
//
#ifndef AGV_SRC1100_H
#define AGV_SRC1100_H
#pragma once
#include "./agv/abstract_agv.h"
#include <mutex>
#include <atomic>
#include <thread>
#include "nlohmann/json.hpp"
using json = nlohmann::json;
namespace cmvr::device {
class AgvSrc1100 final : public AbstractAgv {
public:
explicit AgvSrc1100(const XmlNode& cfg);
explicit AgvSrc1100(const config::AGVsrc1100Config& cfg);
~AgvSrc1100() override;
Status state() const override;
std::string lastError() const override;
void start() override;
void stop() override;
void update() override;
void getStatusInfo(AgvStatusInfo& info) override;
void getBatteryStatus(BatteryStatus& info, bool simple = false) override;
void getRobotLocation(RobotLocation& info) override;
void downloadMap(DownloadMapResult& info, const std::string& map_name) override;
void getMapStatus(MapStatus& info) override;
void uploadMap(UploadMapResult& info, const std::string& map_json) override;
void lockRobotControl(LockResult& info, const std::string& nick_name) override;
void getCurrentLockStatus(CurrentLockStatus& info) override;
void robotMotionControl(MotionCtrlRes& res, const MotionCtrlReq& req) override;
void robotLoadMap(LoadMapRes& res, const LoadMapReq& req) override;
void queryLoadMapStatus(QueryLoadMapStatusRes& res) override;
void queryStationList(QueryStationRes& res) override;
void robotGoTargetList(GoTargetListRes& res, const GoTargetListReq& req) override;
void robotStatusTask(RobotStatusTaskRes& res, const RobotStatusTaskReq& req) override;
private:
void asyncConnect(int& sock, int port);
bool sendAndRecv(int sock, const uint8_t* header);
void flushSocket(int sock);
void setSocketTimeout(int sock, int timeout_ms);
// 辅助函数：将JSON字符串解析为Json::Value，同时做错误处理
bool parseJson(const std::string& json_str, json& root, std::string& err_msg);
mutable std::mutex mutex_;
std::atomic<Status> status_{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<std::thread> connect_threads_;
mutable std::mutex connect_mutex_;
};
} // namespace cmvr::device
#endif // AGV_SRC1100_H

1830
cmvr-es/devices/agv/agv_src1100/src/agv_src1100.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1
cmvr-es/service/CMakeLists.txt
View File

@ -9,6 +9,7 @@ add_library(service
grpc/src/grpc_humanoid_robot_service.cpp
grpc/src/grpc_hlc_service.cpp
grpc/src/server_runner.cpp
grpc/src/grpc_agv_service.cpp
)
target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

131
cmvr-es/service/grpc/include/grpc_agv_service.h Normal file
View File

@ -0,0 +1,131 @@
//
// Created by xtkuang on 2025/6/1.
//
#ifndef GRPC_AGV_SERVICE_H
#define GRPC_AGV_SERVICE_H
#include "cmvr/api/agv_service.grpc.pb.h"
#include "utils/base/include/grpc_utils.h"
#include "device_manager/include/device_manager.h"
namespace cmvr::service {
class gRPCAGVServiceImpl final : public api::AgvService::Service {
public:
gRPCAGVServiceImpl();
~gRPCAGVServiceImpl() override = default;
// AGV服务接口
grpc::Status GetStatusInfo(grpc::ServerContext* context,
const api::GetAgvStatusInfoCommand_Request* request,
api::GetAgvStatusInfoCommand_Feedback* response) override;
grpc::Status GetBatteryStatus(grpc::ServerContext* context,
const api::RobotStatusBatteryCommand_Request* request,
api::RobotStatusBatteryCommand_Feedback* response) override;
grpc::Status GetRobotLocation(grpc::ServerContext* context,
const api::RobotStatusLocCommand_Request* request,
api::RobotStatusLocCommand_Feedback* response) override;
grpc::Status RobotConfigDownloadMap(grpc::ServerContext* context,
const api::RobotConfigDownloadMapCommand_Request* request,
api::RobotConfigDownloadMapCommand_Feedback* response) override;
grpc::Status GetMapStatus(grpc::ServerContext* context,
const api::RobotStatusMapCommand_Request* request,
api::RobotStatusMapCommand_Feedback* response) override;
grpc::Status RobotConfigUploadMap(grpc::ServerContext* context,
const api::RobotConfigUploadMapCommand_Request* request,
api::RobotConfigUploadMapCommand_Feedback* response) override;
/**
* @brief gRPC接口：抢占机器人控制权
*/
grpc::Status RobotConfigLock(grpc::ServerContext* context,
const api::RobotConfigLockCommand_Request* request,
api::RobotConfigLockCommand_Feedback* response) override;
/**
* @brief gRPC接口 查询当前控制权持有者
*/
grpc::Status GetCurrentLockStatus(grpc::ServerContext* context,
const api::RobotStatusCurrentLockCommand_Request* request,
api::RobotStatusCurrentLockCommand_Feedback* response) override;
/**
* @brief AGV开环速度运动控制
* @param context grpc上下文
* @param request 请求体，header携带device_id区分多AGV
* @param response 运动指令执行结果
*/
grpc::Status RobotMotionControl(grpc::ServerContext* context,
const api::RobotMotionControlCommand_Request* request,
api::RobotMotionControlCommand_Feedback* response) override;
/**
* @brief 切换载入地图
* @param context grpc上下文
* @param request 请求体，携带目标地图名称
* @param response 切换地图执行结果
*/
grpc::Status RobotLoadMap(
grpc::ServerContext* context,
const api::RobotLoadMapCommand_Request* request,
api::RobotLoadMapCommand_Feedback* response) override;
/**
* @brief 查询地图载入状态
* @param context grpc上下文
* @param request 请求头
* @param response 地图加载状态结果
*/
grpc::Status QueryLoadMapStatus(
grpc::ServerContext* context,
const api::RobotQueryLoadMapStatusCommand_Request* request,
api::RobotQueryLoadMapStatusCommand_Feedback* response) override;
/**
* @brief 查询当前地图所有站点信息
* @param context grpc上下文
* @param request 请求头
* @param response 站点列表结果
*/
grpc::Status QueryStationList(
grpc::ServerContext* context,
const api::QueryStationListCommand_Request* request,
api::QueryStationListCommand_Feedback* response) override;
/**
* @brief 指定路径连续站点导航
* @param context grpc上下文
* @param request 导航任务列表请求
* @param response 下发任务返回结果
*/
grpc::Status RobotGoTargetList(
grpc::ServerContext* context,
const api::RobotGoTargetListCommand_Request* request,
api::RobotGoTargetListCommand_Feedback* response) override;
grpc::Status RobotStatusTask(
grpc::ServerContext* context,
const api::RobotStatusTaskCommand_Request* request,
api::RobotStatusTaskCommand_Feedback* response) override;
private:
device::DeviceManager& dmgr_;
};
}
#endif //GRPC_AGV_SERVICE_H

708
cmvr-es/service/grpc/src/grpc_agv_service.cpp Normal file
View File

@ -0,0 +1,708 @@
//
// Created by xtkuang on 2025/6/1.
//
#include "./service/grpc/include/grpc_agv_service.h"
#include <boost/container/container_fwd.hpp>
#include "devices/agv/agv_src1100/include/agv_src1100.h"
#include "devices/agv/abstract_agv.h"
namespace cmvr::service {
gRPCAGVServiceImpl::gRPCAGVServiceImpl() : dmgr_(device::DeviceManager::getInstance()) {
}
grpc::Status gRPCAGVServiceImpl::GetStatusInfo(
grpc::ServerContext* context,
const api::GetAgvStatusInfoCommand_Request* request,
api::GetAgvStatusInfoCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (GetStatusInfo): id=" << dev_id;
const auto agv_device = dmgr_.getDevice<device::AbstractAgv>(dev_id);
device::AbstractAgv::AgvStatusInfo device_status;
agv_device->getStatusInfo(device_status);
auto* proto_status = response->mutable_status();
proto_status->set_id(device_status.id);
proto_status->set_vehicle_id(device_status.vehicle_id);
proto_status->set_version(device_status.version);
proto_status->set_model(device_status.model);
proto_status->set_dsp_version(device_status.dsp_version);
proto_status->set_current_ip(device_status.current_ip);
proto_status->set_mac(device_status.mac);
proto_status->set_rssi(device_status.rssi);
proto_status->set_ret_code(device_status.ret_code);
proto_status->set_err_msg(device_status.err_msg);
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "GetStatusInfo device_id:" << dev_id << " vehicle_id: " << device_status.vehicle_id;
return grpc::Status::OK;
}
catch (std::exception &e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::GetBatteryStatus(
grpc::ServerContext* context,
const api::RobotStatusBatteryCommand_Request* request,
api::RobotStatusBatteryCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (GetBatteryStatus): id=" << dev_id;
const auto agv_device = dmgr_.getDevice<device::AbstractAgv>(dev_id);
bool simple = false;
if (request->has_data())
{
simple = request->data().simple();
}
device::AbstractAgv::BatteryStatus bat_info;
agv_device->getBatteryStatus(bat_info, simple);
auto* proto_bat = response->mutable_status();
proto_bat->set_battery_level(bat_info.battery_level);
proto_bat->set_battery_temp(bat_info.battery_temp);
proto_bat->set_charging(bat_info.charging);
proto_bat->set_voltage(bat_info.voltage);
proto_bat->set_current(bat_info.current);
proto_bat->set_max_charge_voltage(bat_info.max_charge_voltage);
proto_bat->set_max_charge_current(bat_info.max_charge_current);
proto_bat->set_manual_charge(bat_info.manual_charge);
proto_bat->set_auto_charge(bat_info.auto_charge);
proto_bat->set_battery_cycle(bat_info.battery_cycle);
proto_bat->set_battery_user_data(bat_info.battery_user_data);
proto_bat->set_extra(bat_info.extra);
proto_bat->set_ret_code(bat_info.ret_code);
proto_bat->set_create_on(bat_info.create_on);
proto_bat->set_err_msg(bat_info.err_msg);
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "GetBatteryStatus device_id:" << dev_id << " level: " << bat_info.battery_level;
return grpc::Status::OK;
}
catch (std::exception &e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::GetRobotLocation(
grpc::ServerContext* context,
const api::RobotStatusLocCommand_Request* request,
api::RobotStatusLocCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (GetRobotLocation): id=" << dev_id;
const auto agv_device = dmgr_.getDevice<device::AbstractAgv>(dev_id);
device::AbstractAgv::RobotLocation loc_info;
agv_device->getRobotLocation(loc_info);
auto* proto_loc = response->mutable_status();
proto_loc->set_x(loc_info.x);
proto_loc->set_y(loc_info.y);
proto_loc->set_angle(loc_info.angle);
proto_loc->set_confidence(loc_info.confidence);
proto_loc->set_current_station(loc_info.current_station);
proto_loc->set_last_station(loc_info.last_station);
proto_loc->set_loc_method(loc_info.loc_method);
proto_loc->set_ret_code(loc_info.ret_code);
proto_loc->set_create_on(loc_info.create_on);
proto_loc->set_err_msg(loc_info.err_msg);
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "GetRobotLocation device_id:" << dev_id << " x: " << loc_info.x << ", y: " << loc_info.y;
return grpc::Status::OK;
}
catch (std::exception &e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::RobotConfigDownloadMap(
grpc::ServerContext* context,
const api::RobotConfigDownloadMapCommand_Request* request,
api::RobotConfigDownloadMapCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (RobotConfigDownloadMap): id=" << dev_id;
const auto agv_device = dmgr_.getDevice<device::AbstractAgv>(dev_id);
std::string map_name = request->data().map_name();
device::AbstractAgv::DownloadMapResult down_info;
agv_device->downloadMap(down_info, map_name);
auto* proto_res = response->mutable_status();
proto_res->set_ret_code(down_info.ret_code);
proto_res->set_create_on(down_info.create_on);
proto_res->set_err_msg(down_info.err_msg);
proto_res->set_map_content(down_info.map_content);
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "DownloadMap device_id:" << dev_id << " map: " << map_name << ", ret_code: " << down_info.ret_code;
return grpc::Status::OK;
}
catch (std::exception &e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::GetMapStatus(
grpc::ServerContext* context,
const api::RobotStatusMapCommand_Request* request,
api::RobotStatusMapCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (GetMapStatus): id=" << dev_id;
const auto agv_device = dmgr_.getDevice<device::AbstractAgv>(dev_id);
device::AbstractAgv::MapStatus map_info;
agv_device->getMapStatus(map_info);
auto* proto_map = response->mutable_status();
proto_map->set_current_map(map_info.current_map);
proto_map->set_current_map_md5(map_info.current_map_md5);
proto_map->set_ret_code(map_info.ret_code);
proto_map->set_create_on(map_info.create_on);
proto_map->set_err_msg(map_info.err_msg);
for (auto& name : map_info.maps)
{
proto_map->add_maps(name);
}
for (auto& file : map_info.map_files_info)
{
auto* proto_file = proto_map->add_map_files_info();
proto_file->set_name(file.name);
proto_file->set_modified(file.modified);
proto_file->set_size(file.size);
}
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "GetMapStatus device_id:" << dev_id << " current map: " << map_info.current_map;
return grpc::Status::OK;
}
catch (std::exception &e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::RobotConfigUploadMap(
grpc::ServerContext* context,
const api::RobotConfigUploadMapCommand_Request* request,
api::RobotConfigUploadMapCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (RobotConfigUploadMap): id=" << dev_id;
const auto agv_device = dmgr_.getDevice<device::AbstractAgv>(dev_id);
std::string map_json = request->data().map_content();
device::AbstractAgv::UploadMapResult upload_info;
agv_device->uploadMap(upload_info, map_json);
auto* proto_res = response->mutable_status();
proto_res->set_ret_code(upload_info.ret_code);
proto_res->set_create_on(upload_info.create_on);
proto_res->set_err_msg(upload_info.err_msg);
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "UploadMap device_id:" << dev_id << " ret_code: " << upload_info.ret_code;
return grpc::Status::OK;
}
catch (std::exception &e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
/**
* @brief gRPC 抢占机器人控制权接口
* @param context grpc上下文
* @param request 前端请求参数，携带nick_name抢占者名称
* @param response 抢占结果返回体
*/
grpc::Status gRPCAGVServiceImpl::RobotConfigLock(
grpc::ServerContext* context,
const api::RobotConfigLockCommand_Request* request,
api::RobotConfigLockCommand_Feedback* response)
{
try {
// 动态从header拿设备ID，和查询接口统一
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (RobotConfigLock): id=" << dev_id;
// 根据device_id获取对应AGV实例
const auto agv_device = dmgr_.getDevice<device::AbstractAgv>(dev_id);
// 读取抢占者名称
std::string nick_name = request->data().nick_name();
device::AbstractAgv::LockResult lock_res;
// 调用底层抢占接口
agv_device->lockRobotControl(lock_res, nick_name);
// 填充protobuf返回数据
auto* proto_status = response->mutable_status();
proto_status->set_ret_code(lock_res.ret_code);
proto_status->set_create_on(lock_res.create_on);
proto_status->set_err_msg(lock_res.err_msg);
// 响应头标记成功 + 时间戳对齐全局规范
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "Lock robot control, device_id:" << dev_id << ", nick:" << nick_name << ", ret_code:" << lock_res.ret_code;
return grpc::Status::OK;
}
catch (std::exception &e)
{
// 统一异常捕获，和相机/查询接口风格完全一致
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
/**
* @brief 查询机器人当前控制权信息gRPC接口
*/
grpc::Status gRPCAGVServiceImpl::GetCurrentLockStatus(
grpc::ServerContext* context,
const api::RobotStatusCurrentLockCommand_Request* request,
api::RobotStatusCurrentLockCommand_Feedback* response)
{
try {
// 1. 读取请求device_id，打印日志（和相机代码保持一致）
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (GetCurrentLockStatus): id=" << dev_id;
// 2. 按device_id获取AGV设备实例，和相机 dmgr_.getDevice<AbstractCamera>(dev_id) 统一写法
const auto agv_dev = dmgr_.getDevice<device::AbstractAgv>(dev_id);
// 3. 调用底层接口获取控制权信息
device::AbstractAgv::CurrentLockStatus lock_info;
agv_dev->getCurrentLockStatus(lock_info);
// 4. 填充protobuf返回结构体
auto* proto_status = response->mutable_status();
proto_status->set_locked(lock_info.locked);
proto_status->set_ip(lock_info.ip);
proto_status->set_port(lock_info.port);
proto_status->set_type(lock_info.type);
proto_status->set_nick_name(lock_info.nick_name);
proto_status->set_time_t(lock_info.time_t);
proto_status->set_desc(lock_info.desc);
proto_status->set_ret_code(lock_info.ret_code);
proto_status->set_create_on(lock_info.create_on);
proto_status->set_err_msg(lock_info.err_msg);
// 5. 响应header标记成功 + 填充时间戳（对齐相机接口）
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
LOG(INFO) << "Query current lock status, device_id:" << dev_id
<< ", locked:" << lock_info.locked << ", nick:" << lock_info.nick_name;
return grpc::Status::OK;
}
catch (std::exception &e)
{
// 异常统一捕获，失败header，填错误信息+时间戳，和相机完全对齐
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::RobotMotionControl(
grpc::ServerContext* context,
const api::RobotMotionControlCommand_Request* request,
api::RobotMotionControlCommand_Feedback* response)
{
try {
// 从header读取device_id，区分多台AGV设备
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (RobotMotionControl): target device_id=" << dev_id;
const auto agv_dev = dmgr_.getDevice<device::AbstractAgv>(dev_id);
// grpc入参转换到底层结构体
device::AbstractAgv::MotionCtrlReq req;
auto& input_data = request->data();
req.vx = input_data.vx();
req.vy = input_data.vy();
req.w = input_data.w();
req.steer = input_data.steer();
req.real_steer = input_data.real_steer();
req.duration = input_data.duration();
// 调用底层运动控制接口
device::AbstractAgv::MotionCtrlRes res;
agv_dev->robotMotionControl(res, req);
// 填充grpc返回状态
auto* resp_status = response->mutable_status();
resp_status->set_ret_code(res.ret_code);
resp_status->set_create_on(res.create_on);
resp_status->set_err_msg(res.err_msg);
// 统一规范header：成功标记+时间戳
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "RobotMotionControl finish, dev_id:" << dev_id
<< " vx=" << req.vx << " vy=" << req.vy << " w=" << req.w
<< " ret_code=" << res.ret_code;
return grpc::Status::OK;
}
catch (std::exception& e)
{
// 全局统一异常捕获模板
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::RobotLoadMap(
grpc::ServerContext* context,
const api::RobotLoadMapCommand_Request* request,
api::RobotLoadMapCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (RobotLoadMap): target device_id=" << dev_id;
const auto agv_dev = dmgr_.getDevice<device::AbstractAgv>(dev_id);
device::AbstractAgv::LoadMapReq req;
auto& input_data = request->data();
req.map_name = input_data.map_name();
device::AbstractAgv::LoadMapRes res;
agv_dev->robotLoadMap(res, req);
auto* resp_status = response->mutable_status();
resp_status->set_ret_code(res.ret_code);
resp_status->set_create_on(res.create_on);
resp_status->set_err_msg(res.err_msg);
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
// 修复：取timestamp子字段
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "RobotLoadMap finish, dev_id:" << dev_id
<< " map_name=" << req.map_name
<< " ret_code=" << res.ret_code;
return grpc::Status::OK;
}
catch (std::exception& e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
// 修复：取timestamp子字段
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::QueryLoadMapStatus(
grpc::ServerContext* context,
const api::RobotQueryLoadMapStatusCommand_Request* request,
api::RobotQueryLoadMapStatusCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (QueryLoadMapStatus): device_id=" << dev_id;
const auto agv_dev = dmgr_.getDevice<device::AbstractAgv>(dev_id);
device::AbstractAgv::QueryLoadMapStatusRes res;
agv_dev->queryLoadMapStatus(res);
auto* resp_status = response->mutable_status();
resp_status->set_loadmap_status(res.loadmap_status);
resp_status->set_ret_code(res.ret_code);
resp_status->set_create_on(res.create_on);
resp_status->set_err_msg(res.err_msg);
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "QueryLoadMapStatus finish dev_id:" << dev_id
<< " loadmap_status=" << res.loadmap_status
<< " ret_code=" << res.ret_code;
return grpc::Status::OK;
}
catch (std::exception& e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::QueryStationList(
grpc::ServerContext* context,
const api::QueryStationListCommand_Request* request,
api::QueryStationListCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (QueryStationList): device_id=" << dev_id;
const auto agv_dev = dmgr_.getDevice<device::AbstractAgv>(dev_id);
device::AbstractAgv::QueryStationRes res;
agv_dev->queryStationList(res);
auto* resp_status = response->mutable_status();
resp_status->set_ret_code(res.ret_code);
resp_status->set_create_on(res.create_on);
resp_status->set_err_msg(res.err_msg);
// 填充站点数组
for (auto& st : res.stations)
{
auto* pb_st = resp_status->add_stations();
pb_st->set_id(st.id);
pb_st->set_type(st.type);
pb_st->set_x(st.x);
pb_st->set_y(st.y);
pb_st->set_r(st.r);
pb_st->set_desc(st.desc);
pb_st->set_executor(st.executor);
pb_st->set_prepoint(st.prepoint);
pb_st->set_recfile(st.recfile);
pb_st->set_spin(st.spin);
pb_st->set_use_down_pgv(st.use_down_pgv);
}
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "QueryStationList finish dev_id:" << dev_id
<< " station_count=" << res.stations.size()
<< " ret_code=" << res.ret_code;
return grpc::Status::OK;
}
catch (std::exception& e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::RobotGoTargetList(
grpc::ServerContext* context,
const api::RobotGoTargetListCommand_Request* request,
api::RobotGoTargetListCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (RobotGoTargetList): target device_id=" << dev_id;
const auto agv_dev = dmgr_.getDevice<device::AbstractAgv>(dev_id);
device::AbstractAgv::GoTargetListReq req;
const auto& pb_list = request->data().move_task_list();
for (const auto& pb_item : pb_list)
{
device::AbstractAgv::MoveTaskItem task;
task.task_id = pb_item.task_id();
task.source_id = pb_item.source_id();
task.id = pb_item.id();
task.operation = pb_item.operation();
task.jack_height = pb_item.jack_height();
req.move_task_list.push_back(task);
}
device::AbstractAgv::GoTargetListRes res;
agv_dev->robotGoTargetList(res, req);
auto* resp_status = response->mutable_status();
resp_status->set_ret_code(res.ret_code);
resp_status->set_create_on(res.create_on);
resp_status->set_err_msg(res.err_msg);
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "RobotGoTargetList finish dev_id:" << dev_id
<< " task_count=" << req.move_task_list.size()
<< " ret_code=" << res.ret_code;
return grpc::Status::OK;
}
catch (std::exception& e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCAGVServiceImpl::RobotStatusTask(
grpc::ServerContext* context,
const api::RobotStatusTaskCommand_Request* request,
api::RobotStatusTaskCommand_Feedback* response)
{
try {
std::string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCAGVServiceImpl] (RobotStatusTask): id=" << dev_id;
const auto agv_dev = dmgr_.getDevice<device::AbstractAgv>(dev_id);
// 嵌套结构体 必须加 AbstractAgv::
device::AbstractAgv::RobotStatusTaskReq req;
if (request->has_data())
{
req.simple = request->data().simple();
}
device::AbstractAgv::RobotStatusTaskRes res;
agv_dev->robotStatusTask(res, req);
auto* out_data = response->mutable_data();
out_data->set_ret_code(res.ret_code);
out_data->set_create_on(res.create_on);
out_data->set_err_msg(res.err_msg);
out_data->set_task_status(res.task_status);
out_data->set_task_type(res.task_type);
out_data->set_target_id(res.target_id);
out_data->set_move_status_info(res.move_status_info);
for (double p : res.target_point)
out_data->add_target_point(p);
for (auto& s : res.finished_path)
out_data->add_finished_path(s);
for (auto& s : res.unfinished_path)
out_data->add_unfinished_path(s);
for (auto& c : res.containers)
{
auto* pc = out_data->add_containers();
pc->set_container_name(c.container_name);
pc->set_desc(c.desc);
pc->set_goods_id(c.goods_id);
pc->set_has_goods(c.has_goods);
}
auto* header = response->mutable_header();
header->set_success(true);
header->set_error_message("");
setCurrentTimestamp(header->mutable_timestamp());
LOG(INFO) << "RobotStatusTask device_id:" << dev_id << " task_status: " << res.task_status;
return grpc::Status::OK;
}
catch (std::exception &e)
{
auto* header = response->mutable_header();
header->set_success(false);
header->set_error_message(e.what());
setCurrentTimestamp(header->mutable_timestamp());
return grpc::Status::OK;
}
}
}

3
cmvr-es/service/grpc/src/server_runner.cpp
View File

@ -22,6 +22,7 @@
#include "service/grpc/include/grpc_hlc_service.h"
#include "json/json.h"
#include <grpcpp/ext/proto_server_reflection_plugin.h>
#include "service/grpc/include/grpc_agv_service.h"
using namespace cmvr::service;
ServerRunner::ServerRunner() = default;
@ -118,6 +119,7 @@ void ServerRunner::threadMain() {
auto biohand_service = std::make_unique<gRPCMBioHeadServiceImpl>();
auto humanoid_robot_service = std::make_unique<gRPCHumanoidRobotServiceImpl>();
auto hlc_service = std::make_unique<gRPCHlcServiceImpl>();
auto agv_service = std::make_unique<gRPCAGVServiceImpl>();
grpc::ServerBuilder builder;
builder.AddListeningPort(local_address, grpc::InsecureServerCredentials());
@ -129,6 +131,7 @@ void ServerRunner::threadMain() {
builder.RegisterService(biohand_service.get());
builder.RegisterService(humanoid_robot_service.get());
builder.RegisterService(hlc_service.get());
builder.RegisterService(agv_service.get());
auto local_server = builder.BuildAndStart();
if (!local_server) {

424
protos/cmvr/api/agv_command.proto Normal file
View File

@ -0,0 +1,424 @@
syntax = "proto3";
import "cmvr/api/common.proto";
package cmvr.api;
// AGV状态信息
message AgvStatusInfo {
optional string id = 1; // AGV ID
optional string vehicle_id = 2; // 车辆ID
optional string version = 3; // 软件版本
optional string model = 4; // 型号
optional string dsp_version = 5; // DSP版本
optional string current_ip = 6; // 当前IP地址
optional string mac = 7; // MAC地址
optional int32 rssi = 8; // 信号强度
optional int32 ret_code = 9; // 返回码
optional string err_msg = 10; // 错误信息
}
// 获取AGV状态命令
message GetAgvStatusInfoCommand {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvStatusInfo status = 2;
}
}
// ===================== 电池状态查询 =====================
message AgvBatteryStatus {
optional double battery_level = 1; // 电池电量 [0,1]
optional double battery_temp = 2; // 电池温度 ℃
optional bool charging = 3; // 是否正在充电
optional double voltage = 4; // 电压 V
optional double current = 5; // 电流 A
optional double max_charge_voltage = 6; // 最大充电电压，-1=不支持
optional double max_charge_current = 7; // 最大充电电流，-1=不支持
optional bool manual_charge = 8; // 手动充电状态
optional bool auto_charge = 9; // 自动充电状态
optional int32 battery_cycle = 10; // 电池循环次数
optional string battery_user_data = 11; // 电池自定义数据
optional string extra = 12; // 客户自定义数据
optional int32 ret_code = 13; // 错误码
optional string create_on = 14; // 时间戳
optional string err_msg = 15; // 错误信息
}
message RobotStatusBatteryRequestData {
optional bool simple = 1; // true:仅简单数据 false:全量数据，默认false
}
message RobotStatusBatteryCommand {
message Request {
CommandHeader.Request header = 1;
RobotStatusBatteryRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvBatteryStatus status = 2;
}
}
// ===================== 机器人位置查询 =====================
message AgvRobotLocation {
optional double x = 1; // X坐标(m)
optional double y = 2; // Y坐标(m)
optional double angle = 3; // 姿态角(rad)
optional double confidence = 4; // 定位置信度[0,1]
optional string current_station = 5; // 当前最近站点ID
optional string last_station = 6; // 上一站点ID
optional int32 loc_method = 7; // 定位方式
optional int32 ret_code = 8; // 错误码
optional string create_on = 9; // 时间戳
optional string err_msg = 10; // 错误信息
}
message RobotStatusLocCommand {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvRobotLocation status = 2;
}
}
// ===================== 地图下载 =====================
message RobotConfigDownloadMapRequestData {
optional string map_name = 1;
}
message AgvDownloadMapResult {
optional int32 ret_code = 1;
optional string create_on = 2;
optional string err_msg = 3;
optional string map_content = 4;
}
message RobotConfigDownloadMapCommand {
message Request {
CommandHeader.Request header = 1;
RobotConfigDownloadMapRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvDownloadMapResult status = 2;
}
}
// 单条地图文件信息
message MapFileInfo {
optional string name = 1;
optional string modified = 2;
optional int64 size = 3;
}
// 地图查询响应
message AgvMapStatus {
optional string current_map = 1;
optional string current_map_md5 = 2;
repeated string maps = 3;
repeated MapFileInfo map_files_info = 4;
optional int32 ret_code = 5;
optional string create_on = 6;
optional string err_msg = 7;
}
// 查询地图状态命令
message RobotStatusMapCommand {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvMapStatus status = 2;
}
}
// ===================== 上传地图 =====================
message RobotConfigUploadMapRequestData {
optional string map_content = 1;
}
message AgvUploadMapResult {
optional int32 ret_code = 1;
optional string create_on = 2;
optional string err_msg = 3;
}
message RobotConfigUploadMapCommand {
message Request {
CommandHeader.Request header = 1;
RobotConfigUploadMapRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvUploadMapResult status = 2;
}
}
// ===================== 抢占控制权 ======================
// 抢占控制权 请求参数
message RobotConfigLockRequestData {
// 控制权抢占者名称，必填
optional string nick_name = 1;
}
// 抢占控制权 返回结果
message AgvLockResult {
// API错误码
optional int32 ret_code = 1;
// API上传时间戳
optional string create_on = 2;
// 错误描述信息
optional string err_msg = 3;
}
// 抢占控制权 命令封装
message RobotConfigLockCommand {
message Request {
CommandHeader.Request header = 1;
// 请求业务参数
RobotConfigLockRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
// 抢占控制权响应数据
AgvLockResult status = 2;
}
}
// ===================== 查询当前控制权所有者 ======================
// 查询当前控制权所有者响应结构体
message AgvCurrentLockStatus {
// 当前控制权是否被抢占
optional bool locked = 1;
// 抢占方IP
optional string ip = 2;
// 抢占方端口
optional int32 port = 3;
// 抢占者类型 0=default 2=roboshop 0xDD=srd
optional uint32 type = 4;
// 抢占者昵称
optional string nick_name = 5;
// 抢占时间戳 单位秒
optional int64 time_t = 6;
// 抢占方描述信息
optional string desc = 7;
// 接口错误码
optional int32 ret_code = 8;
// 接口时间戳
optional string create_on = 9;
// 错误信息
optional string err_msg = 10;
}
// 查询当前控制权指令
message RobotStatusCurrentLockCommand {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
AgvCurrentLockStatus status = 2;
}
}
//==================== 开环速度运动控制 命令码：2010(0x07DA) ====================
// 下发指令会强制取消当前正在执行的自动导航任务
// 多舵轮设备仅解析vx/vy/w；steer、real_steer仅单舵轮设备生效
// vx：X轴速度 m/s；vy：Y轴速度 m/s；w：旋转角速度 rad/s，逆时针为正、顺时针为负
// steer：单舵轮档位控制 -2回零 / ±15°增减；real_steer：目标舵角rad，优先级高于steer
// duration：持续时间ms，0=永久运行；缺省读取全局参数ControlMotionDuration(默认500ms)
message RobotMotionControlRequestData {
optional double vx = 1;
optional double vy = 2;
optional double w = 3;
optional double steer = 4;
optional double real_steer = 5;
optional int64 duration = 6;
}
message RobotMotionControlResult {
optional int32 ret_code = 1;
optional string create_on = 2;
optional string err_msg = 3;
}
message RobotMotionControlCommand {
message Request {
CommandHeader.Request header = 1;
RobotMotionControlRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
RobotMotionControlResult status = 2;
}
}
//==================== 切换载入地图 命令码：2022(0x07E6) ====================
// 切换指定地图，合法地图会更新设备current_map字段
message RobotLoadMapRequestData {
optional string map_name = 1; // 地图名，仅字母数字-_，不可缺省
}
message RobotLoadMapResult {
optional int32 ret_code = 1;
optional string create_on = 2;
optional string err_msg = 3;
}
message RobotLoadMapCommand {
message Request {
CommandHeader.Request header = 1;
RobotLoadMapRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
RobotLoadMapResult status = 2;
}
}
//==================== 查询地图载入状态 命令码：1022(0x03FE) ====================
// loadmap_status:0失败 1成功 2载入中；LOADING状态不可重定位
message RobotQueryLoadMapStatusResult {
optional int32 loadmap_status = 1;
optional int32 ret_code = 2;
optional string create_on = 3;
optional string err_msg = 4;
}
message RobotQueryLoadMapStatusCommand {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
RobotQueryLoadMapStatusResult status = 2;
}
}
//==================== 查询当前地图所有站点信息 命令码：1301(0x0515) ====================
message StationItem {
optional string id = 1;
optional string type = 2;
optional double x = 3;
optional double y = 4;
optional double r = 5;
optional string desc = 6;
optional string executor = 7;
optional string prepoint = 8;
optional string recfile = 9;
optional bool spin = 10;
optional bool use_down_pgv = 11;
}
message QueryStationListResult {
repeated StationItem stations = 1;
optional int32 ret_code = 2;
optional string create_on = 3;
optional string err_msg = 4;
}
message QueryStationListCommand {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
QueryStationListResult status = 2;
}
}
//==================== 指定路径导航 命令码：3066(0x0BFA) ====================
// 单段导航任务单元
message MoveTaskItem {
optional string task_id = 1;
optional string source_id = 2;
optional string id = 3;
optional string operation = 4;
optional double jack_height = 5;
}
// 指定路径导航请求体
message RobotGoTargetListRequestData {
repeated MoveTaskItem move_task_list = 1;
}
// 指定路径导航返回结果
message RobotGoTargetListResult {
optional int32 ret_code = 1;
optional string create_on = 2;
optional string err_msg = 3;
}
message RobotGoTargetListCommand {
message Request {
CommandHeader.Request header = 1;
RobotGoTargetListRequestData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
RobotGoTargetListResult status = 2;
}
}
//==================== 查询当前导航状态 robot_status_task_req 1020(0x03FC) 应答0x2B0C ====================
message QueryCurrentNavReqData {
optional bool simple = 1;
}
message NavContainerItem {
optional string container_name = 1;
optional string desc = 2;
optional string goods_id = 3;
optional bool has_goods = 4;
}
message QueryCurrentNavResData {
optional int32 task_status = 1;
optional int32 task_type = 2;
optional string target_id = 3;
repeated double target_point = 4;
repeated string finished_path = 5;
repeated string unfinished_path = 6;
optional string move_status_info = 7;
repeated NavContainerItem containers = 8;
optional int32 ret_code = 9;
optional string create_on = 10;
optional string err_msg = 11;
}
message RobotStatusTaskCommand {
message Request {
CommandHeader.Request header = 1;
QueryCurrentNavReqData data = 2;
}
message Feedback {
CommandHeader.Feedback header = 1;
QueryCurrentNavResData data = 2;
}
}

60
protos/cmvr/api/agv_service.proto Normal file
View File

@ -0,0 +1,60 @@
syntax = "proto3";
import "cmvr/api/agv_command.proto";
package cmvr.api;
service AgvService {
// 基本控制
rpc GetStatusInfo(GetAgvStatusInfoCommand.Request) returns (GetAgvStatusInfoCommand.Feedback);
rpc GetBatteryStatus(RobotStatusBatteryCommand.Request) returns (RobotStatusBatteryCommand.Feedback);
rpc GetRobotLocation(RobotStatusLocCommand.Request) returns (RobotStatusLocCommand.Feedback);
rpc RobotConfigDownloadMap(RobotConfigDownloadMapCommand.Request) returns (RobotConfigDownloadMapCommand.Feedback);
rpc GetMapStatus(RobotStatusMapCommand.Request) returns (RobotStatusMapCommand.Feedback);
rpc RobotConfigUploadMap(RobotConfigUploadMapCommand.Request) returns (RobotConfigUploadMapCommand.Feedback);
// 抢占机器人控制权
rpc RobotConfigLock(RobotConfigLockCommand.Request) returns (RobotConfigLockCommand.Feedback);
// 查询当前控制权所有者
rpc GetCurrentLockStatus(RobotStatusCurrentLockCommand.Request) returns (RobotStatusCurrentLockCommand.Feedback);
// 开环速度运动控制
rpc RobotMotionControl(RobotMotionControlCommand.Request) returns (RobotMotionControlCommand.Feedback);
// 切换载入地图 命令码2022
rpc RobotLoadMap(RobotLoadMapCommand.Request) returns (RobotLoadMapCommand.Feedback);
// 查询地图载入状态 命令码1022
rpc QueryLoadMapStatus(RobotQueryLoadMapStatusCommand.Request) returns (RobotQueryLoadMapStatusCommand.Feedback);
// 查询当前地图站点列表 命令码1301
rpc QueryStationList(QueryStationListCommand.Request) returns (QueryStationListCommand.Feedback);
// 指定路径导航 命令码3066
rpc RobotGoTargetList(RobotGoTargetListCommand.Request) returns (RobotGoTargetListCommand.Feedback);
// 查询当前导航状态 1020 robot_status_task_req
rpc RobotStatusTask(RobotStatusTaskCommand.Request) returns (RobotStatusTaskCommand.Feedback);
}

20
protos/cmvr/config/agv_config/agv_config.proto Normal file
View File

@ -0,0 +1,20 @@
// camera_config.proto - 所有配置都在这里
syntax = "proto3";
package cmvr.config;
message AGVsrc1100Config{
string id = 1;
string ip = 2;
bool enable = 3;
int32 port_status = 4; // 19204 状态查询
int32 port_control = 5; // 19205 控制API
int32 port_nav = 6; // 19206 导航API
int32 port_config = 7; // 19207 配置API
int32 port_other = 8; // 19210 其他API
int32 port_push = 9; // 19301 推送API
}
message AGVConfig{
repeated AGVsrc1100Config src1100_agvs = 1;
}