update src1100 module
This commit is contained in:
parent
6664d181cb
commit
2fa161f1cc
@ -1,6 +1,7 @@
|
||||
#ifndef CMVR_ES_AGV_TYPES_H
|
||||
#define CMVR_ES_AGV_TYPES_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
@ -216,6 +217,186 @@ struct AgvPathSegment {
|
||||
std::string target_station;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief AGV 扫图过程中产生的数据文件。
|
||||
*
|
||||
* content 可保存控制器返回的二进制内容，例如 SRC1100 的 rawmap zip 包。
|
||||
*/
|
||||
struct AgvMappingDataFile {
|
||||
std::string name;
|
||||
std::string content;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 从控制器增量获取的扫图数据批次。
|
||||
*/
|
||||
struct AgvMappingData {
|
||||
int start_index{0};
|
||||
int next_index{0};
|
||||
std::vector<AgvMappingDataFile> files;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 上位机请求的统一地图维度。
|
||||
*
|
||||
* 该枚举只表示上位机希望得到 2D、3D 或两者都要；不表示厂商文件格式。
|
||||
* 厂商原始地图必须由具体 AGV 驱动转换为下面的统一地图结构。
|
||||
*/
|
||||
enum class AgvMapDimension {
|
||||
Unspecified = 0,
|
||||
Map2D,
|
||||
Map3D,
|
||||
Map2DAnd3D
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 地图流中的更新类型。
|
||||
*/
|
||||
enum class AgvMapUpdateType {
|
||||
Unspecified = 0,
|
||||
Snapshot,
|
||||
Incremental,
|
||||
Reset
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 统一语义地图对象类型。
|
||||
*/
|
||||
enum class AgvMapObjectType {
|
||||
Unspecified = 0,
|
||||
Station,
|
||||
Line,
|
||||
Area,
|
||||
QrTag,
|
||||
Reflector,
|
||||
BinLocation,
|
||||
ExternalDevice
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 地图坐标系下的三维点，单位：米。
|
||||
*/
|
||||
struct AgvMapPoint3D {
|
||||
double x{0.0};
|
||||
double y{0.0};
|
||||
double z{0.0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 统一语义对象。几何点均使用地图坐标系，单位：米。
|
||||
*/
|
||||
struct AgvMapObject {
|
||||
std::string id;
|
||||
AgvMapObjectType type{AgvMapObjectType::Unspecified};
|
||||
std::vector<AgvMapPoint3D> points;
|
||||
double heading{0.0};
|
||||
std::unordered_map<std::string, std::string> properties;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 统一 2D 地图。
|
||||
*
|
||||
* data 采用行优先顺序，取值约定为 -1 未知、0 空闲、100 占据。
|
||||
* 当厂商地图只提供矢量/语义元素时，data 可以为空，objects 仍然有效。
|
||||
*/
|
||||
struct AgvUnifiedMap2D {
|
||||
std::string frame_id{"map"};
|
||||
double timestamp{0.0};
|
||||
double resolution{0.0};
|
||||
std::uint32_t width{0};
|
||||
std::uint32_t height{0};
|
||||
math::Pose2d origin{};
|
||||
std::vector<std::int32_t> data;
|
||||
std::vector<AgvMapObject> objects;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 统一 3D 点样本，坐标单位：米。
|
||||
*/
|
||||
struct AgvMapPointSample3D {
|
||||
double x{0.0};
|
||||
double y{0.0};
|
||||
double z{0.0};
|
||||
float intensity{0.0F};
|
||||
std::uint32_t ring{0};
|
||||
double time_offset{0.0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 统一 3D 占据体素。
|
||||
*/
|
||||
struct AgvMapVoxel3D {
|
||||
std::int32_t x{0};
|
||||
std::int32_t y{0};
|
||||
std::int32_t z{0};
|
||||
float probability{-1.0F};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 统一 3D 平面特征。
|
||||
*/
|
||||
struct AgvMapPlane3D {
|
||||
AgvMapPoint3D center;
|
||||
AgvMapPoint3D normal;
|
||||
double d{0.0};
|
||||
double radius{0.0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 统一 3D 地图。
|
||||
*/
|
||||
struct AgvUnifiedMap3D {
|
||||
std::string frame_id{"map"};
|
||||
double timestamp{0.0};
|
||||
double voxel_resolution{0.0};
|
||||
std::vector<AgvMapPointSample3D> points;
|
||||
std::vector<AgvMapVoxel3D> voxels;
|
||||
std::vector<AgvMapPlane3D> planes;
|
||||
std::vector<AgvMapObject> objects;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 地图流读取参数。
|
||||
*/
|
||||
struct AgvMapStreamOptions {
|
||||
AgvMapDimension dimension{AgvMapDimension::Unspecified};
|
||||
std::string map_name;
|
||||
std::string resume_token;
|
||||
bool snapshot{true};
|
||||
bool incremental{false};
|
||||
int max_chunk_bytes{0};
|
||||
int wait_timeout_ms{1000};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 建图/扫图启动参数。
|
||||
*/
|
||||
struct AgvMappingOptions {
|
||||
AgvMapDimension dimension{AgvMapDimension::Unspecified};
|
||||
std::string map_name;
|
||||
bool real_time{false};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 统一地图流中的单条更新。
|
||||
*/
|
||||
struct AgvUnifiedMapUpdate {
|
||||
std::string map_id;
|
||||
std::string session_id;
|
||||
std::uint64_t sequence{0};
|
||||
std::string resume_token;
|
||||
AgvMapDimension dimension{AgvMapDimension::Unspecified};
|
||||
AgvMapUpdateType update_type{AgvMapUpdateType::Unspecified};
|
||||
std::string frame_id{"map"};
|
||||
double timestamp{0.0};
|
||||
bool snapshot_begin{false};
|
||||
bool snapshot_end{false};
|
||||
std::uint32_t chunk_index{0};
|
||||
std::uint32_t chunk_count{0};
|
||||
std::optional<AgvUnifiedMap2D> map_2d;
|
||||
std::optional<AgvUnifiedMap3D> map_3d;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 当前导航任务状态。
|
||||
*
|
||||
|
||||
@ -11,7 +11,6 @@ agv {
|
||||
id: "src1100"
|
||||
src1100_agv {
|
||||
ip: "192.168.192.5"
|
||||
enable: false
|
||||
port_status: 19204
|
||||
port_control: 19205
|
||||
port_nav: 19206
|
||||
@ -19,6 +18,28 @@ agv {
|
||||
port_other: 19210
|
||||
port_push: 19301
|
||||
recv_timeout_ms: 1000
|
||||
enable_state_push: true
|
||||
state_push_interval_ms: 200
|
||||
state_push_included_fields: "x"
|
||||
state_push_included_fields: "y"
|
||||
state_push_included_fields: "angle"
|
||||
state_push_included_fields: "vx"
|
||||
state_push_included_fields: "vy"
|
||||
state_push_included_fields: "w"
|
||||
state_push_included_fields: "battery_level"
|
||||
state_push_included_fields: "battery_temp"
|
||||
state_push_included_fields: "charging"
|
||||
state_push_included_fields: "voltage"
|
||||
state_push_included_fields: "current"
|
||||
state_push_included_fields: "current_map"
|
||||
state_push_included_fields: "current_station"
|
||||
state_push_included_fields: "confidence"
|
||||
state_push_included_fields: "emergency"
|
||||
state_push_included_fields: "fatals"
|
||||
state_push_included_fields: "errors"
|
||||
enable_map_update: true
|
||||
map_update_interval_ms: 1000
|
||||
map_update_history_size: 8
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -58,14 +58,7 @@ device_manager {
|
||||
config_file: "devices/biohead/bio_head.pb.txt"
|
||||
enable: false
|
||||
}
|
||||
|
||||
devices {
|
||||
id: "agv_1"
|
||||
type: DEVICE_TYPE_AGV
|
||||
config_file: "devices/agv/agv.pb.txt"
|
||||
enable: false
|
||||
}
|
||||
|
||||
|
||||
devices {
|
||||
id: "src1100"
|
||||
type: DEVICE_TYPE_AGV
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
#define CMVR_ES_ABSTRACT_AGV_H
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
@ -184,6 +185,54 @@ public:
|
||||
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "downloadMap not implemented");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 开始扫图/建图。
|
||||
*/
|
||||
virtual AgvResult startMapping(const AgvMappingOptions& options = {})
|
||||
{
|
||||
(void)options;
|
||||
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "startMapping not implemented");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从指定下标开始获取厂商原始扫图数据。
|
||||
*
|
||||
* 该接口主要保留给具体驱动内部使用。对外 gRPC 地图流应优先使用
|
||||
* getUnifiedMapUpdate()，避免把厂商文件格式暴露给上位机。
|
||||
*/
|
||||
virtual AgvResult getMappingData(int start_index, AgvMappingData& data) const
|
||||
{
|
||||
(void)start_index;
|
||||
(void)data;
|
||||
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "getMappingData not implemented");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 获取统一地图更新。
|
||||
*
|
||||
* after_sequence 为 0 时通常返回最近可用的全量快照；大于 0 时返回
|
||||
* 指定序号之后的下一条更新。如果当前没有新地图，具体实现可在
|
||||
* options.wait_timeout_ms 内等待后台更新线程写入缓存。
|
||||
*/
|
||||
virtual AgvResult getUnifiedMapUpdate(
|
||||
std::uint64_t after_sequence,
|
||||
const AgvMapStreamOptions& options,
|
||||
AgvUnifiedMapUpdate& update) const
|
||||
{
|
||||
(void)after_sequence;
|
||||
(void)options;
|
||||
(void)update;
|
||||
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "getUnifiedMapUpdate not implemented");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 停止扫图/建图。
|
||||
*/
|
||||
virtual AgvResult stopMapping()
|
||||
{
|
||||
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "stopMapping not implemented");
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
@ -1,8 +1,13 @@
|
||||
#ifndef CMVR_ES_SRC1100_AGV_H
|
||||
#define CMVR_ES_SRC1100_AGV_H
|
||||
|
||||
#include <atomic>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <deque>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <json/json.h>
|
||||
@ -50,6 +55,13 @@ public:
|
||||
AgvResult switchMap(const std::string& map_name) override;
|
||||
AgvResult uploadMap(const std::string& map_name, const std::string& content) override;
|
||||
AgvResult downloadMap(const std::string& map_name, std::string& content) const override;
|
||||
AgvResult startMapping(const AgvMappingOptions& options = {}) override;
|
||||
AgvResult getMappingData(int start_index, AgvMappingData& data) const override;
|
||||
AgvResult getUnifiedMapUpdate(
|
||||
std::uint64_t after_sequence,
|
||||
const AgvMapStreamOptions& options,
|
||||
AgvUnifiedMapUpdate& update) const override;
|
||||
AgvResult stopMapping() override;
|
||||
|
||||
private:
|
||||
struct Ports {
|
||||
@ -64,6 +76,7 @@ private:
|
||||
AgvResult connect_();
|
||||
AgvResult disconnect_();
|
||||
AgvResult connectSocket_(int& sock, int port);
|
||||
AgvResult ensureOtherSocket_();
|
||||
void closeSocket_(int& sock) const;
|
||||
bool connected_() const;
|
||||
|
||||
@ -71,12 +84,55 @@ private:
|
||||
std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
Json::Value* response) const;
|
||||
AgvResult sendCommandRaw_(int sock,
|
||||
std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
std::string* response_payload) const;
|
||||
AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const;
|
||||
AgvResult configurePush_();
|
||||
void startPushThread_();
|
||||
void stopPushThread_();
|
||||
void pushLoop_();
|
||||
AgvRuntimeState queryRuntimeState_() const;
|
||||
void updateCachedRuntimeState_(const Json::Value& payload);
|
||||
void startMapUpdateThread_();
|
||||
void stopMapUpdateThread_();
|
||||
void mapUpdateLoop_();
|
||||
AgvResult refreshMapCacheOnce_(const AgvMapStreamOptions& options) const;
|
||||
AgvResult parseMapFileToUpdates_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
std::vector<AgvUnifiedMapUpdate>& updates) const;
|
||||
AgvResult parseSrc1100MapArchive_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
std::vector<AgvUnifiedMapUpdate>& updates) const;
|
||||
AgvResult parseSrc1100Map2D_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
AgvUnifiedMapUpdate& update) const;
|
||||
AgvResult parseSrc1100Map3D_(
|
||||
const std::string& file_name,
|
||||
const std::string& content,
|
||||
const AgvMapStreamOptions& options,
|
||||
AgvUnifiedMapUpdate& update) const;
|
||||
void cacheMapUpdates_(std::vector<AgvUnifiedMapUpdate> updates) const;
|
||||
bool findCachedMapUpdate_(
|
||||
std::uint64_t after_sequence,
|
||||
const AgvMapStreamOptions& options,
|
||||
AgvUnifiedMapUpdate& update) const;
|
||||
bool mapUpdateMatches_(
|
||||
const AgvUnifiedMapUpdate& update,
|
||||
const AgvMapStreamOptions& options) const;
|
||||
|
||||
static std::vector<std::uint8_t> buildFrame_(std::uint16_t command, const std::string& payload);
|
||||
static std::string toJsonString_(const Json::Value& value);
|
||||
static bool parseJson_(const std::string& input, Json::Value& output, std::string& error);
|
||||
static std::string extractJson_(const std::string& raw);
|
||||
static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload);
|
||||
static int optionalInt_(const AgvAdapterParams& params, const std::string& key, int fallback);
|
||||
static double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback);
|
||||
static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options);
|
||||
@ -87,6 +143,10 @@ private:
|
||||
std::string ip_;
|
||||
int recv_timeout_ms_{1000};
|
||||
Ports ports_;
|
||||
bool state_push_enabled_{false};
|
||||
bool map_update_enabled_{false};
|
||||
int map_update_interval_ms_{1000};
|
||||
std::size_t map_update_history_size_{8};
|
||||
|
||||
mutable std::mutex mutex_;
|
||||
int sock_status_{-1};
|
||||
@ -96,6 +156,22 @@ private:
|
||||
int sock_other_{-1};
|
||||
int sock_push_{-1};
|
||||
std::string last_error_;
|
||||
|
||||
std::atomic<bool> push_running_{false};
|
||||
std::thread push_thread_;
|
||||
mutable std::mutex runtime_state_mutex_;
|
||||
AgvRuntimeState cached_runtime_state_;
|
||||
bool cached_runtime_state_valid_{false};
|
||||
|
||||
mutable std::atomic<bool> map_update_running_{false};
|
||||
mutable std::thread map_update_thread_;
|
||||
mutable std::mutex map_update_mutex_;
|
||||
mutable std::condition_variable map_update_cv_;
|
||||
mutable std::deque<AgvUnifiedMapUpdate> cached_map_updates_;
|
||||
mutable std::uint64_t map_sequence_{0};
|
||||
mutable int next_mapping_index_{0};
|
||||
mutable std::size_t last_map_content_hash_{0};
|
||||
mutable std::string map_session_id_;
|
||||
};
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -63,6 +63,15 @@ public:
|
||||
grpc::Status downloadMap(grpc::ServerContext* context,
|
||||
const api::AgvMapCommand_Request* request,
|
||||
api::AgvMapCommand_Feedback* response) override;
|
||||
grpc::Status startMapping(grpc::ServerContext* context,
|
||||
const api::AgvStartMappingCommand_Request* request,
|
||||
api::AgvStartMappingCommand_Feedback* response) override;
|
||||
grpc::Status streamMap(grpc::ServerContext* context,
|
||||
const api::AgvMapStreamCommand_Request* request,
|
||||
grpc::ServerWriter<api::AgvMapStreamCommand_Feedback>* writer) override;
|
||||
grpc::Status stopMapping(grpc::ServerContext* context,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response) override;
|
||||
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#include "service/grpc/include/grpc_agv_service.h"
|
||||
|
||||
#include <cstdint>
|
||||
#include <exception>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
@ -98,6 +99,74 @@ math::Pose2d toPose2d(const msgs::AgvPose2d& src)
|
||||
return {src.x(), src.y(), src.theta()};
|
||||
}
|
||||
|
||||
device::AgvMapDimension toMapDimension(const msgs::AgvMapDimension src)
|
||||
{
|
||||
switch (src) {
|
||||
case msgs::AGV_MAP_2D:
|
||||
return device::AgvMapDimension::Map2D;
|
||||
case msgs::AGV_MAP_3D:
|
||||
return device::AgvMapDimension::Map3D;
|
||||
case msgs::AGV_MAP_2D_AND_3D:
|
||||
return device::AgvMapDimension::Map2DAnd3D;
|
||||
case msgs::AGV_MAP_DIMENSION_UNSPECIFIED:
|
||||
default:
|
||||
return device::AgvMapDimension::Unspecified;
|
||||
}
|
||||
}
|
||||
|
||||
msgs::AgvMapDimension toProtoMapDimension(const device::AgvMapDimension src)
|
||||
{
|
||||
switch (src) {
|
||||
case device::AgvMapDimension::Map2D:
|
||||
return msgs::AGV_MAP_2D;
|
||||
case device::AgvMapDimension::Map3D:
|
||||
return msgs::AGV_MAP_3D;
|
||||
case device::AgvMapDimension::Map2DAnd3D:
|
||||
return msgs::AGV_MAP_2D_AND_3D;
|
||||
case device::AgvMapDimension::Unspecified:
|
||||
default:
|
||||
return msgs::AGV_MAP_DIMENSION_UNSPECIFIED;
|
||||
}
|
||||
}
|
||||
|
||||
msgs::AgvMapUpdateType toProtoMapUpdateType(const device::AgvMapUpdateType src)
|
||||
{
|
||||
switch (src) {
|
||||
case device::AgvMapUpdateType::Snapshot:
|
||||
return msgs::AGV_MAP_UPDATE_SNAPSHOT;
|
||||
case device::AgvMapUpdateType::Incremental:
|
||||
return msgs::AGV_MAP_UPDATE_INCREMENTAL;
|
||||
case device::AgvMapUpdateType::Reset:
|
||||
return msgs::AGV_MAP_UPDATE_RESET;
|
||||
case device::AgvMapUpdateType::Unspecified:
|
||||
default:
|
||||
return msgs::AGV_MAP_UPDATE_UNSPECIFIED;
|
||||
}
|
||||
}
|
||||
|
||||
msgs::AgvMapObjectType toProtoMapObjectType(const device::AgvMapObjectType src)
|
||||
{
|
||||
switch (src) {
|
||||
case device::AgvMapObjectType::Station:
|
||||
return msgs::AGV_MAP_OBJECT_STATION;
|
||||
case device::AgvMapObjectType::Line:
|
||||
return msgs::AGV_MAP_OBJECT_LINE;
|
||||
case device::AgvMapObjectType::Area:
|
||||
return msgs::AGV_MAP_OBJECT_AREA;
|
||||
case device::AgvMapObjectType::QrTag:
|
||||
return msgs::AGV_MAP_OBJECT_QR_TAG;
|
||||
case device::AgvMapObjectType::Reflector:
|
||||
return msgs::AGV_MAP_OBJECT_REFLECTOR;
|
||||
case device::AgvMapObjectType::BinLocation:
|
||||
return msgs::AGV_MAP_OBJECT_BIN_LOCATION;
|
||||
case device::AgvMapObjectType::ExternalDevice:
|
||||
return msgs::AGV_MAP_OBJECT_EXTERNAL_DEVICE;
|
||||
case device::AgvMapObjectType::Unspecified:
|
||||
default:
|
||||
return msgs::AGV_MAP_OBJECT_UNSPECIFIED;
|
||||
}
|
||||
}
|
||||
|
||||
void fillPose2d(msgs::AgvPose2d* dst, const math::Pose2d& src)
|
||||
{
|
||||
dst->set_x(src.x);
|
||||
@ -154,6 +223,97 @@ void fillStation(msgs::AgvStation* dst, const device::AgvStation& src)
|
||||
dst->set_description(src.description);
|
||||
}
|
||||
|
||||
void fillMapPoint3D(msgs::AgvMapPoint3D* dst, const device::AgvMapPoint3D& src)
|
||||
{
|
||||
dst->set_x(src.x);
|
||||
dst->set_y(src.y);
|
||||
dst->set_z(src.z);
|
||||
}
|
||||
|
||||
void fillMapObject(msgs::AgvMapObject* dst, const device::AgvMapObject& src)
|
||||
{
|
||||
dst->set_id(src.id);
|
||||
dst->set_type(toProtoMapObjectType(src.type));
|
||||
for (const auto& point : src.points) {
|
||||
fillMapPoint3D(dst->add_points(), point);
|
||||
}
|
||||
dst->set_heading(src.heading);
|
||||
auto* properties = dst->mutable_properties();
|
||||
for (const auto& [key, value] : src.properties) {
|
||||
(*properties)[key] = value;
|
||||
}
|
||||
}
|
||||
|
||||
void fillUnifiedMap2D(msgs::AgvUnifiedMap2D* dst, const device::AgvUnifiedMap2D& src)
|
||||
{
|
||||
dst->set_frame_id(src.frame_id);
|
||||
dst->set_timestamp(src.timestamp);
|
||||
dst->set_resolution(src.resolution);
|
||||
dst->set_width(src.width);
|
||||
dst->set_height(src.height);
|
||||
fillPose2d(dst->mutable_origin(), src.origin);
|
||||
for (const auto value : src.data) {
|
||||
dst->add_data(value);
|
||||
}
|
||||
for (const auto& object : src.objects) {
|
||||
fillMapObject(dst->add_objects(), object);
|
||||
}
|
||||
}
|
||||
|
||||
void fillUnifiedMap3D(msgs::AgvUnifiedMap3D* dst, const device::AgvUnifiedMap3D& src)
|
||||
{
|
||||
dst->set_frame_id(src.frame_id);
|
||||
dst->set_timestamp(src.timestamp);
|
||||
dst->set_voxel_resolution(src.voxel_resolution);
|
||||
for (const auto& point : src.points) {
|
||||
auto* dst_point = dst->add_points();
|
||||
dst_point->set_x(point.x);
|
||||
dst_point->set_y(point.y);
|
||||
dst_point->set_z(point.z);
|
||||
dst_point->set_intensity(point.intensity);
|
||||
dst_point->set_ring(point.ring);
|
||||
dst_point->set_time_offset(point.time_offset);
|
||||
}
|
||||
for (const auto& voxel : src.voxels) {
|
||||
auto* dst_voxel = dst->add_voxels();
|
||||
dst_voxel->set_x(voxel.x);
|
||||
dst_voxel->set_y(voxel.y);
|
||||
dst_voxel->set_z(voxel.z);
|
||||
dst_voxel->set_probability(voxel.probability);
|
||||
}
|
||||
for (const auto& plane : src.planes) {
|
||||
auto* dst_plane = dst->add_planes();
|
||||
fillMapPoint3D(dst_plane->mutable_center(), plane.center);
|
||||
fillMapPoint3D(dst_plane->mutable_normal(), plane.normal);
|
||||
dst_plane->set_d(plane.d);
|
||||
dst_plane->set_radius(plane.radius);
|
||||
}
|
||||
for (const auto& object : src.objects) {
|
||||
fillMapObject(dst->add_objects(), object);
|
||||
}
|
||||
}
|
||||
|
||||
void fillUnifiedMapUpdate(msgs::AgvUnifiedMapUpdate* dst, const device::AgvUnifiedMapUpdate& src)
|
||||
{
|
||||
dst->set_map_id(src.map_id);
|
||||
dst->set_session_id(src.session_id);
|
||||
dst->set_sequence(src.sequence);
|
||||
dst->set_resume_token(src.resume_token);
|
||||
dst->set_dimension(toProtoMapDimension(src.dimension));
|
||||
dst->set_update_type(toProtoMapUpdateType(src.update_type));
|
||||
dst->set_frame_id(src.frame_id);
|
||||
dst->set_timestamp(src.timestamp);
|
||||
dst->set_snapshot_begin(src.snapshot_begin);
|
||||
dst->set_snapshot_end(src.snapshot_end);
|
||||
dst->set_chunk_index(src.chunk_index);
|
||||
dst->set_chunk_count(src.chunk_count);
|
||||
if (src.map_2d) {
|
||||
fillUnifiedMap2D(dst->mutable_map_2d(), *src.map_2d);
|
||||
} else if (src.map_3d) {
|
||||
fillUnifiedMap3D(dst->mutable_map_3d(), *src.map_3d);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
gRPCAgvServiceImpl::gRPCAgvServiceImpl()
|
||||
@ -478,4 +638,115 @@ grpc::Status gRPCAgvServiceImpl::downloadMap(grpc::ServerContext*,
|
||||
}
|
||||
}
|
||||
|
||||
grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*,
|
||||
const api::AgvStartMappingCommand_Request* request,
|
||||
api::AgvStartMappingCommand_Feedback* response)
|
||||
{
|
||||
try {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||
if (!agv) {
|
||||
return setDeviceNotFound(response, device_id);
|
||||
}
|
||||
device::AgvMappingOptions options;
|
||||
options.dimension = toMapDimension(request->dimension());
|
||||
options.map_name = request->map_name();
|
||||
options.real_time = request->real_time();
|
||||
const auto result = agv->startMapping(options);
|
||||
if (result.ok()) {
|
||||
response->set_session_id(device_id + "_mapping");
|
||||
}
|
||||
return setResponseResult(response, result);
|
||||
} catch (const std::exception& e) {
|
||||
fillFeedback(response->mutable_header(), false, e.what());
|
||||
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||
}
|
||||
}
|
||||
|
||||
grpc::Status gRPCAgvServiceImpl::streamMap(grpc::ServerContext* context,
|
||||
const api::AgvMapStreamCommand_Request* request,
|
||||
grpc::ServerWriter<api::AgvMapStreamCommand_Feedback>* writer)
|
||||
{
|
||||
try {
|
||||
const std::string device_id = request->header().device_id();
|
||||
auto agv = dmgr_.getDevice<device::AbstractAGV>(device_id);
|
||||
|
||||
api::AgvMapStreamCommand_Feedback feedback;
|
||||
if (!agv) {
|
||||
const std::string message = "AGV device not found: " + device_id;
|
||||
fillFeedback(feedback.mutable_header(), false, message);
|
||||
writer->Write(feedback);
|
||||
return grpc::Status(grpc::StatusCode::NOT_FOUND, message);
|
||||
}
|
||||
|
||||
device::AgvMapStreamOptions options;
|
||||
options.dimension = toMapDimension(request->dimension());
|
||||
options.map_name = request->map_name();
|
||||
options.resume_token = request->resume_token();
|
||||
options.snapshot = request->snapshot();
|
||||
options.incremental = request->incremental();
|
||||
options.max_chunk_bytes = request->max_chunk_bytes();
|
||||
|
||||
std::uint64_t after_sequence = 0;
|
||||
if (!request->resume_token().empty()) {
|
||||
try {
|
||||
after_sequence = static_cast<std::uint64_t>(std::stoull(request->resume_token()));
|
||||
} catch (...) {
|
||||
after_sequence = 0;
|
||||
}
|
||||
}
|
||||
|
||||
bool wrote_any = false;
|
||||
while (!context->IsCancelled()) {
|
||||
options.wait_timeout_ms = (!options.incremental && wrote_any) ? 20 : 1000;
|
||||
device::AgvUnifiedMapUpdate update;
|
||||
const auto result = agv->getUnifiedMapUpdate(after_sequence, options, update);
|
||||
if (!result.ok()) {
|
||||
if (result.code == device::AgvErrorCode::Timeout && wrote_any && !options.incremental) {
|
||||
return grpc::Status::OK;
|
||||
}
|
||||
if (result.code == device::AgvErrorCode::Timeout && wrote_any && options.incremental) {
|
||||
continue;
|
||||
}
|
||||
fillFeedback(feedback.mutable_header(), false, result.message);
|
||||
writer->Write(feedback);
|
||||
return resultToStatus(result);
|
||||
}
|
||||
|
||||
api::AgvMapStreamCommand_Feedback update_feedback;
|
||||
fillFeedback(update_feedback.mutable_header(), true);
|
||||
fillUnifiedMapUpdate(update_feedback.mutable_update(), update);
|
||||
if (!writer->Write(update_feedback)) {
|
||||
return grpc::Status(grpc::StatusCode::CANCELLED, "AGV map stream writer closed");
|
||||
}
|
||||
wrote_any = true;
|
||||
after_sequence = update.sequence;
|
||||
options.resume_token.clear();
|
||||
}
|
||||
|
||||
return grpc::Status(grpc::StatusCode::CANCELLED, "AGV map stream cancelled");
|
||||
} catch (const std::exception& e) {
|
||||
api::AgvMapStreamCommand_Feedback feedback;
|
||||
fillFeedback(feedback.mutable_header(), false, e.what());
|
||||
writer->Write(feedback);
|
||||
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||
}
|
||||
}
|
||||
|
||||
grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext*,
|
||||
const api::CommandHeader_Request* request,
|
||||
api::CommandHeader_Feedback* response)
|
||||
{
|
||||
try {
|
||||
auto agv = dmgr_.getDevice<device::AbstractAGV>(request->device_id());
|
||||
if (!agv) {
|
||||
return setDeviceNotFound(response, request->device_id());
|
||||
}
|
||||
return setResponseResult(response, agv->stopMapping());
|
||||
} catch (const std::exception& e) {
|
||||
fillFeedback(response, false, e.what());
|
||||
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace cmvr::service
|
||||
|
||||
@ -5,98 +5,208 @@ package cmvr.api;
|
||||
import "cmvr/api/common.proto";
|
||||
import "cmvr/msgs/agv.proto";
|
||||
|
||||
// 查询 AGV 运行状态命令。
|
||||
message AgvRuntimeStateCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
// AGV 当前运行状态快照。
|
||||
cmvr.msgs.AgvRuntimeState state = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// 查询 AGV 当前导航任务状态命令。
|
||||
message AgvNavigationStatusCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
// 当前导航任务状态。
|
||||
cmvr.msgs.AgvNavigationStatus status = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// 导航到指定地图位姿命令。
|
||||
message AgvNavigateToPoseCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
// 目标位姿。x/y 单位：米，theta 单位：弧度。
|
||||
cmvr.msgs.AgvPose2d pose = 2;
|
||||
// 通用运动约束和执行选项。
|
||||
cmvr.msgs.AgvMotionOptions options = 3;
|
||||
// AGV 适配器扩展参数，用于传递厂商特有选项。
|
||||
cmvr.msgs.AgvAdapterParams adapter_params = 4;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// 导航到指定站点命令。
|
||||
message AgvNavigateToStationCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
// 目标站点 id。
|
||||
string station_id = 2;
|
||||
// 通用运动约束和执行选项。
|
||||
cmvr.msgs.AgvMotionOptions options = 3;
|
||||
// AGV 适配器扩展参数，用于传递厂商特有选项。
|
||||
cmvr.msgs.AgvAdapterParams adapter_params = 4;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// 按显式站点路径导航命令。
|
||||
message AgvFollowPathCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
// 路径段列表。每段包含起点站点 id 和终点站点 id。
|
||||
repeated cmvr.msgs.AgvPathSegment path = 2;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// 下发底盘速度命令。
|
||||
message AgvSetVelocityCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
// 目标车体速度。vx/vy 单位：米/秒，wz 单位：弧度/秒。
|
||||
cmvr.msgs.AgvVelocity velocity = 2;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// 查询可用地图列表命令。
|
||||
message AgvListMapsCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
// 地图名称列表。
|
||||
repeated string maps = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// 查询当前地图站点列表命令。
|
||||
message AgvListStationsCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
// 当前地图中的站点列表。
|
||||
repeated cmvr.msgs.AgvStation stations = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// 地图上传、下载、切换等通用地图命令。
|
||||
message AgvMapCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
// 地图名称。切换/下载时表示目标地图，上传时表示写入的地图名称。
|
||||
string map_name = 2;
|
||||
// 地图内容。上传地图时使用；下载或切换地图时可为空。
|
||||
string content = 3;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
// 地图内容。下载地图时返回；其他命令通常为空。
|
||||
string content = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// 开始建图/扫图命令。
|
||||
message AgvStartMappingCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
// 请求建图维度：2D、3D 或二者都要。未指定时由适配器选择最合适模式。
|
||||
cmvr.msgs.AgvMapDimension dimension = 2;
|
||||
// 目标地图名称。为空表示由 AGV 或适配器创建/选择默认地图名。
|
||||
string map_name = 3;
|
||||
// 是否请求实时建图更新。true 表示希望实时推送；false 表示允许离线/批处理。
|
||||
bool real_time = 4;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
// 建图会话 id。该值由服务端生成，作为调试和日志关联标识。
|
||||
string session_id = 2;
|
||||
}
|
||||
}
|
||||
|
||||
// 地图流命令。用于获取当前地图、最近地图、全量快照和后续增量更新。
|
||||
message AgvMapStreamCommand {
|
||||
// 请求体。
|
||||
message Request {
|
||||
// 通用请求头。header.device_id 指定目标 AGV 设备。
|
||||
CommandHeader.Request header = 1;
|
||||
// 请求地图维度：2D、3D 或二者都要。该字段不是厂商格式选择器。
|
||||
cmvr.msgs.AgvMapDimension dimension = 2;
|
||||
// 地图名称。为空表示当前加载地图；若无当前地图，适配器应尝试使用最近保存/创建的地图。
|
||||
string map_name = 3;
|
||||
// 断点续传令牌。为空表示上位机没有缓存，服务端应先发送全量快照。
|
||||
string resume_token = 4;
|
||||
// 是否请求全量快照。首次请求通常应为 true。
|
||||
bool snapshot = 5;
|
||||
// 是否在快照之后保持流并发送增量更新。若适配器不支持增量，可继续发送全量并标记 update_type。
|
||||
bool incremental = 6;
|
||||
// 单条流消息建议最大载荷大小，单位：字节。小于等于 0 表示使用服务端默认值。
|
||||
int32 max_chunk_bytes = 7;
|
||||
}
|
||||
// 反馈体。
|
||||
message Feedback {
|
||||
// 通用反馈头。包含成功标志、错误信息和反馈时间戳。
|
||||
CommandHeader.Feedback header = 1;
|
||||
// 统一地图更新。payload 只会是 map_2d 或 map_3d，不暴露厂商原始格式。
|
||||
cmvr.msgs.AgvUnifiedMapUpdate update = 2;
|
||||
}
|
||||
}
|
||||
|
||||
@ -5,22 +5,68 @@ package cmvr.api;
|
||||
import "cmvr/api/common.proto";
|
||||
import "cmvr/api/agv_command.proto";
|
||||
|
||||
// AGV 通用服务。该服务只暴露控制器无关的能力，
|
||||
// 厂商协议、地图文件格式和控制器特有参数由具体 AGV 适配器内部处理。
|
||||
service AgvService {
|
||||
// 获取 AGV 当前运行状态快照。
|
||||
rpc getRuntimeState(AgvRuntimeStateCommand.Request) returns (AgvRuntimeStateCommand.Feedback);
|
||||
|
||||
// 获取当前导航任务状态。
|
||||
rpc getNavigationStatus(AgvNavigationStatusCommand.Request) returns (AgvNavigationStatusCommand.Feedback);
|
||||
|
||||
// 执行急停或等效安全停止动作。
|
||||
rpc emergencyStop(CommandHeader.Request) returns (CommandHeader.Feedback);
|
||||
|
||||
// 清除可恢复故障或告警。
|
||||
rpc clearFault(CommandHeader.Request) returns (CommandHeader.Feedback);
|
||||
|
||||
// 导航到指定地图位姿。目标位姿 x/y 单位为米，theta 单位为弧度。
|
||||
rpc navigateToPose(AgvNavigateToPoseCommand.Request) returns (AgvNavigateToPoseCommand.Feedback);
|
||||
|
||||
// 导航到指定地图站点。
|
||||
rpc navigateToStation(AgvNavigateToStationCommand.Request) returns (AgvNavigateToStationCommand.Feedback);
|
||||
|
||||
// 按显式站点路径执行导航。
|
||||
rpc followPath(AgvFollowPathCommand.Request) returns (AgvFollowPathCommand.Feedback);
|
||||
|
||||
// 暂停当前导航任务。
|
||||
rpc pauseNavigation(CommandHeader.Request) returns (CommandHeader.Feedback);
|
||||
|
||||
// 恢复已暂停的导航任务。
|
||||
rpc resumeNavigation(CommandHeader.Request) returns (CommandHeader.Feedback);
|
||||
|
||||
// 取消当前导航任务。
|
||||
rpc cancelNavigation(CommandHeader.Request) returns (CommandHeader.Feedback);
|
||||
|
||||
// 下发底盘速度控制指令。vx/vy 单位为米/秒，wz 单位为弧度/秒。
|
||||
rpc setVelocity(AgvSetVelocityCommand.Request) returns (AgvSetVelocityCommand.Feedback);
|
||||
|
||||
// 停止底盘速度控制。该接口不等价于取消导航任务。
|
||||
rpc stopVelocityControl(CommandHeader.Request) returns (CommandHeader.Feedback);
|
||||
|
||||
// 查询 AGV 控制器可用地图名称列表。
|
||||
rpc listMaps(AgvListMapsCommand.Request) returns (AgvListMapsCommand.Feedback);
|
||||
|
||||
// 查询当前地图中的站点列表。
|
||||
rpc listStations(AgvListStationsCommand.Request) returns (AgvListStationsCommand.Feedback);
|
||||
|
||||
// 切换当前使用地图。
|
||||
rpc switchMap(AgvMapCommand.Request) returns (AgvMapCommand.Feedback);
|
||||
|
||||
// 上传地图内容到 AGV 控制器。地图内容字段由适配器解释。
|
||||
rpc uploadMap(AgvMapCommand.Request) returns (AgvMapCommand.Feedback);
|
||||
|
||||
// 下载指定地图内容。
|
||||
rpc downloadMap(AgvMapCommand.Request) returns (AgvMapCommand.Feedback);
|
||||
|
||||
// 开始建图/扫图会话。请求只选择 2D、3D 或二者都要，
|
||||
// 控制器特有地图格式由 AGV 适配器内部转换。
|
||||
rpc startMapping(AgvStartMappingCommand.Request) returns (AgvStartMappingCommand.Feedback);
|
||||
|
||||
// 以服务端流方式发送统一地图。resume_token 为空时应先发送全量快照；
|
||||
// 后续是否发送增量由 AGV 适配器能力决定，并通过 update_type 标记。
|
||||
rpc streamMap(AgvMapStreamCommand.Request) returns (stream AgvMapStreamCommand.Feedback);
|
||||
|
||||
// 停止当前建图/扫图会话。
|
||||
rpc stopMapping(CommandHeader.Request) returns (CommandHeader.Feedback);
|
||||
}
|
||||
|
||||
@ -1,39 +1,78 @@
|
||||
syntax = "proto3";
|
||||
package cmvr.config;
|
||||
|
||||
// 示例/测试 AGV 后端配置。
|
||||
message MyAgvConfig {
|
||||
// 设备 id。为空时通常由外层 AGVDeviceConfig.id 补齐。
|
||||
string id = 1;
|
||||
// AGV 控制器 IP 地址或主机名。
|
||||
string ip = 2;
|
||||
// AGV 控制器端口号。
|
||||
int32 port = 3;
|
||||
}
|
||||
|
||||
// 仙工 SRC1100 AGV 后端配置。
|
||||
message Src1100AgvConfig {
|
||||
// 设备 id。为空时通常由外层 AGVDeviceConfig.id 补齐。
|
||||
string id = 1;
|
||||
// SRC1100 控制器 IP 地址。
|
||||
string ip = 2;
|
||||
// 是否启用该后端配置。当前设备是否创建仍以设备管理器配置为准。
|
||||
bool enable = 3;
|
||||
// 状态查询端口，默认 19204。
|
||||
int32 port_status = 4;
|
||||
// 控制命令端口，默认 19205。
|
||||
int32 port_control = 5;
|
||||
// 导航任务端口，默认 19206。
|
||||
int32 port_nav = 6;
|
||||
// 地图/配置文件端口，默认 19207。
|
||||
int32 port_config = 7;
|
||||
// 其他功能端口，默认 19210，例如扫图开始/停止。
|
||||
int32 port_other = 8;
|
||||
// 状态推送端口，默认 19301。
|
||||
int32 port_push = 9;
|
||||
// 连接超时时间，单位：毫秒。0 表示使用适配器默认值。
|
||||
int32 connect_timeout_ms = 10;
|
||||
// 接收超时时间，单位：毫秒。0 表示使用适配器默认值。
|
||||
int32 recv_timeout_ms = 11;
|
||||
// 是否启用机器人状态实时推送。
|
||||
bool enable_state_push = 12;
|
||||
// 状态推送间隔，单位：毫秒。0 表示不修改控制器默认间隔。
|
||||
int32 state_push_interval_ms = 13;
|
||||
// 状态推送中显式包含的字段列表。该字段不能与 state_push_excluded_fields 同时配置。
|
||||
repeated string state_push_included_fields = 14;
|
||||
// 状态推送中排除的字段列表。该字段不能与 state_push_included_fields 同时配置。
|
||||
repeated string state_push_excluded_fields = 15;
|
||||
// 是否启用地图后台更新线程。启用后适配器会周期性抓取并解析地图，供 gRPC 地图流直接读取。
|
||||
bool enable_map_update = 16;
|
||||
// 地图后台更新间隔，单位：毫秒。0 表示使用适配器默认值。
|
||||
int32 map_update_interval_ms = 17;
|
||||
// 统一地图更新缓存条数。0 表示使用适配器默认值；缓存满后会丢弃最旧更新。
|
||||
uint32 map_update_history_size = 18;
|
||||
}
|
||||
|
||||
// 单个 AGV 设备配置。
|
||||
message AGVDeviceConfig {
|
||||
// 设备 id。必须与设备管理器中的 AGV 设备 id 对应。
|
||||
string id = 1;
|
||||
|
||||
// 具体 AGV 后端配置。同一设备只能选择一个后端。
|
||||
oneof backend {
|
||||
// 示例/测试 AGV 后端。
|
||||
MyAgvConfig my_agv = 10;
|
||||
// 仙工 SRC1100 AGV 后端。
|
||||
Src1100AgvConfig src1100_agv = 11;
|
||||
}
|
||||
}
|
||||
|
||||
// AGV 设备配置集合。
|
||||
message AGVConfig {
|
||||
// AGV 设备列表。
|
||||
repeated AGVDeviceConfig agvs = 1;
|
||||
}
|
||||
|
||||
// AGV 配置文件根节点。
|
||||
message AGVRootConfig {
|
||||
// AGV 配置集合。
|
||||
AGVConfig agv = 1;
|
||||
}
|
||||
|
||||
@ -2,72 +2,309 @@ syntax = "proto3";
|
||||
|
||||
package cmvr.msgs;
|
||||
|
||||
// AGV 在地图平面坐标系中的二维位姿。
|
||||
message AgvPose2d {
|
||||
// X 坐标，单位：米。
|
||||
double x = 1;
|
||||
// Y 坐标，单位：米。
|
||||
double y = 2;
|
||||
// 航向角，单位：弧度，逆时针为正。
|
||||
double theta = 3;
|
||||
}
|
||||
|
||||
// AGV 车体坐标系下的平面速度。
|
||||
message AgvVelocity {
|
||||
// 车体 X 方向线速度，单位：米/秒。
|
||||
double vx = 1;
|
||||
// 车体 Y 方向线速度，单位：米/秒。
|
||||
double vy = 2;
|
||||
// 绕 Z 轴角速度，单位：弧度/秒。
|
||||
double wz = 3;
|
||||
}
|
||||
|
||||
// AGV 电池状态。
|
||||
message AgvBatteryState {
|
||||
// 电量比例，范围：[0, 1]，例如 0.8 表示 80%。
|
||||
double percentage = 1;
|
||||
// 电池电压，单位：伏特。
|
||||
double voltage = 2;
|
||||
// 电池电流，单位：安培；正负号含义由具体 AGV 适配器保持一致。
|
||||
double current = 3;
|
||||
// 电池温度，单位：摄氏度。
|
||||
double temperature = 4;
|
||||
// 是否正在充电。
|
||||
bool charging = 5;
|
||||
}
|
||||
|
||||
// 导航任务的通用运动约束和执行选项。
|
||||
message AgvMotionOptions {
|
||||
// 最大线速度，单位：米/秒；0 表示使用 AGV 默认值。
|
||||
double max_speed = 1;
|
||||
// 最大角速度，单位：弧度/秒；0 表示使用 AGV 默认值。
|
||||
double max_angular_speed = 2;
|
||||
// 最大线加速度，单位：米/秒^2；0 表示使用 AGV 默认值。
|
||||
double max_acceleration = 3;
|
||||
// 最大角加速度，单位：弧度/秒^2；0 表示使用 AGV 默认值。
|
||||
double max_angular_acceleration = 4;
|
||||
// 到达目标点的距离容差，单位：米；0 表示使用 AGV 默认值。
|
||||
double reach_distance = 5;
|
||||
// 到达目标角度的角度容差，单位：弧度；0 表示使用 AGV 默认值。
|
||||
double reach_angle = 6;
|
||||
// 速度比例，范围通常为 [0, 1]；1 表示不降速。
|
||||
double speed_ratio = 7;
|
||||
// 是否异步执行；true 表示下发任务后立即返回。
|
||||
bool asynchronous = 8;
|
||||
}
|
||||
|
||||
// AGV 适配器扩展参数。用于传递厂商或控制器特有的参数。
|
||||
message AgvAdapterParams {
|
||||
// 参数键值表。键和值都使用字符串，具体含义由 AGV 适配器解释。
|
||||
map<string, string> values = 1;
|
||||
}
|
||||
|
||||
// AGV 当前运行状态快照。
|
||||
message AgvRuntimeState {
|
||||
// 状态采样时间，Unix 时间戳，单位：秒。
|
||||
double timestamp = 1;
|
||||
// 运行模式，取值对应服务端内部 AgvMode 枚举的整数值。
|
||||
int32 mode = 2;
|
||||
// 是否已连接 AGV 控制器。
|
||||
bool connected = 3;
|
||||
// 是否已定位成功。
|
||||
bool localized = 4;
|
||||
// AGV 是否处于运动状态。
|
||||
bool moving = 5;
|
||||
// AGV 是否处于故障状态。
|
||||
bool fault = 6;
|
||||
// AGV 是否处于急停状态。
|
||||
bool emergency_stopped = 7;
|
||||
// 当前地图坐标系下的二维位姿。
|
||||
AgvPose2d pose = 8;
|
||||
// 当前车体速度。
|
||||
AgvVelocity velocity = 9;
|
||||
// 当前电池状态。
|
||||
AgvBatteryState battery = 10;
|
||||
// 当前加载地图名称；为空表示未知或控制器未返回。
|
||||
string current_map = 11;
|
||||
// 当前或最近站点 id；为空表示未知或当前不在站点附近。
|
||||
string current_station = 12;
|
||||
// 最近一次错误信息；为空表示无错误或未知。
|
||||
string last_error = 13;
|
||||
}
|
||||
|
||||
// 地图中的站点信息。
|
||||
message AgvStation {
|
||||
// 站点唯一 id。
|
||||
string id = 1;
|
||||
// 站点类型；具体枚举由地图或控制器定义。
|
||||
string type = 2;
|
||||
// 站点在地图坐标系下的二维位姿。
|
||||
AgvPose2d pose = 3;
|
||||
// 站点描述或备注。
|
||||
string description = 4;
|
||||
}
|
||||
|
||||
// 显式站点路径中的一段路径。
|
||||
message AgvPathSegment {
|
||||
// 起点站点 id。
|
||||
string source_station = 1;
|
||||
// 目标站点 id。
|
||||
string target_station = 2;
|
||||
}
|
||||
|
||||
// 地图维度请求。这里只描述上位机想要 2D、3D 还是二者都要，
|
||||
// 不用于指定厂商文件格式；厂商格式必须在 AGV 适配器内部转换。
|
||||
enum AgvMapDimension {
|
||||
// 未指定。服务端应选择最有用的默认地图，通常是当前加载地图。
|
||||
AGV_MAP_DIMENSION_UNSPECIFIED = 0;
|
||||
// 只请求统一 2D 地图。
|
||||
AGV_MAP_2D = 1;
|
||||
// 只请求统一 3D 地图。
|
||||
AGV_MAP_3D = 2;
|
||||
// 在同一个流中请求统一 2D 和统一 3D 地图。
|
||||
AGV_MAP_2D_AND_3D = 3;
|
||||
}
|
||||
|
||||
// 地图流更新类型。上位机应根据该字段判断是全量、增量还是缓存重置。
|
||||
enum AgvMapUpdateType {
|
||||
// 未指定。
|
||||
AGV_MAP_UPDATE_UNSPECIFIED = 0;
|
||||
// 全量地图快照。首次请求或无法增量续传时应发送该类型。
|
||||
AGV_MAP_UPDATE_SNAPSHOT = 1;
|
||||
// 全量快照之后的增量更新。仅当适配器能够可靠生成差异时发送。
|
||||
AGV_MAP_UPDATE_INCREMENTAL = 2;
|
||||
// 上位机本地缓存已失效，应丢弃缓存并等待后续全量快照。
|
||||
AGV_MAP_UPDATE_RESET = 3;
|
||||
}
|
||||
|
||||
// 2D/3D 地图共用的语义对象类型。
|
||||
enum AgvMapObjectType {
|
||||
// 未指定。
|
||||
AGV_MAP_OBJECT_UNSPECIFIED = 0;
|
||||
// 导航站点或路径点。
|
||||
AGV_MAP_OBJECT_STATION = 1;
|
||||
// 路径线、禁行线、引导线等线对象。
|
||||
AGV_MAP_OBJECT_LINE = 2;
|
||||
// 多边形区域，例如禁行区、限速区、作业区。
|
||||
AGV_MAP_OBJECT_AREA = 3;
|
||||
// 二维码、天码或其他标签地标。
|
||||
AGV_MAP_OBJECT_QR_TAG = 4;
|
||||
// 反光板或反光柱地标。
|
||||
AGV_MAP_OBJECT_REFLECTOR = 5;
|
||||
// 库位、货位或储位。
|
||||
AGV_MAP_OBJECT_BIN_LOCATION = 6;
|
||||
// 门、电梯、充电桩等外部设备。
|
||||
AGV_MAP_OBJECT_EXTERNAL_DEVICE = 7;
|
||||
}
|
||||
|
||||
// 地图坐标系下的三维点。单位：米。
|
||||
message AgvMapPoint3D {
|
||||
// X 坐标，单位：米。
|
||||
double x = 1;
|
||||
// Y 坐标，单位：米。
|
||||
double y = 2;
|
||||
// Z 坐标，单位：米；纯 2D 几何可置为 0。
|
||||
double z = 3;
|
||||
}
|
||||
|
||||
// 统一 2D 地图。栅格数据按行优先排列：index = y * width + x。
|
||||
message AgvUnifiedMap2D {
|
||||
// 坐标系名称，例如 "map"。
|
||||
string frame_id = 1;
|
||||
// 地图采样或更新时间，Unix 时间戳，单位：秒。
|
||||
double timestamp = 2;
|
||||
// 栅格分辨率，单位：米/格。
|
||||
double resolution = 3;
|
||||
// 栅格宽度，单位：格。
|
||||
uint32 width = 4;
|
||||
// 栅格高度，单位：格。
|
||||
uint32 height = 5;
|
||||
// 栅格 (0, 0) 在世界/地图坐标系下的位姿；x/y 单位：米，theta 单位：弧度。
|
||||
AgvPose2d origin = 6;
|
||||
// 占据值：-1 表示未知，0 表示空闲，100 表示占据。
|
||||
repeated int32 data = 7;
|
||||
// 地图中的统一语义对象，例如站点、线、区域、标签、反光板等。
|
||||
repeated AgvMapObject objects = 8;
|
||||
}
|
||||
|
||||
// 统一 3D 点样本。坐标单位：米。
|
||||
message AgvMapPointSample3D {
|
||||
// X 坐标，单位：米。
|
||||
double x = 1;
|
||||
// Y 坐标，单位：米。
|
||||
double y = 2;
|
||||
// Z 坐标，单位：米。
|
||||
double z = 3;
|
||||
// 激光强度；无强度信息时置为 0。
|
||||
float intensity = 4;
|
||||
// 激光雷达线束/通道编号；无该信息时置为 0。
|
||||
uint32 ring = 5;
|
||||
// 相对地图时间戳的时间偏移，单位：秒；无该信息时置为 0。
|
||||
double time_offset = 6;
|
||||
}
|
||||
|
||||
// 统一 3D 体素。体素索引基于 AgvUnifiedMap3D.voxel_resolution。
|
||||
message AgvMapVoxel3D {
|
||||
// 体素 X 索引。
|
||||
int32 x = 1;
|
||||
// 体素 Y 索引。
|
||||
int32 y = 2;
|
||||
// 体素 Z 索引。
|
||||
int32 z = 3;
|
||||
// 占据概率，范围：[0, 1]；未知时置为 -1。
|
||||
float probability = 4;
|
||||
}
|
||||
|
||||
// 统一 3D 平面特征。平面方程：
|
||||
// normal.x * x + normal.y * y + normal.z * z + d = 0。
|
||||
message AgvMapPlane3D {
|
||||
// 平面中心点，单位：米。
|
||||
AgvMapPoint3D center = 1;
|
||||
// 平面单位法向量。
|
||||
AgvMapPoint3D normal = 2;
|
||||
// 平面方程偏移量，单位：米。
|
||||
double d = 3;
|
||||
// 平面特征近似半径，单位：米。
|
||||
double radius = 4;
|
||||
}
|
||||
|
||||
// 统一语义对象。几何点均使用地图坐标系，单位：米。
|
||||
message AgvMapObject {
|
||||
// 对象稳定 id 或名称。
|
||||
string id = 1;
|
||||
// 对象类型。
|
||||
AgvMapObjectType type = 2;
|
||||
// 对象几何点。点对象使用 1 个点，线对象使用多个点，区域对象使用多边形顶点。
|
||||
repeated AgvMapPoint3D points = 3;
|
||||
// 朝向角，单位：弧度；不适用时置为 0。
|
||||
double heading = 4;
|
||||
// 额外归一化属性。为兼容不同 AGV，属性值统一使用字符串。
|
||||
map<string, string> properties = 5;
|
||||
}
|
||||
|
||||
// 统一 3D 地图。虽然内部包含点、体素、平面和语义对象，
|
||||
// 但对上位机来说它仍然是唯一的 3D 地图格式。
|
||||
message AgvUnifiedMap3D {
|
||||
// 坐标系名称，例如 "map"。
|
||||
string frame_id = 1;
|
||||
// 地图采样或更新时间，Unix 时间戳，单位：秒。
|
||||
double timestamp = 2;
|
||||
// 体素分辨率，单位：米/体素；没有体素数据时置为 0。
|
||||
double voxel_resolution = 3;
|
||||
// 统一点云点样本。
|
||||
repeated AgvMapPointSample3D points = 4;
|
||||
// 统一占据体素。
|
||||
repeated AgvMapVoxel3D voxels = 5;
|
||||
// 统一平面特征。
|
||||
repeated AgvMapPlane3D planes = 6;
|
||||
// 统一 3D 语义对象，例如站点、区域、标签、地标等。
|
||||
repeated AgvMapObject objects = 7;
|
||||
}
|
||||
|
||||
// 地图流中的单条更新。AGV 适配器必须先把厂商地图转换成 map_2d 或 map_3d，
|
||||
// 再通过该消息发送给上位机。
|
||||
message AgvUnifiedMapUpdate {
|
||||
// 实际发送的地图 id 或名称。请求 map_name 为空时，AGV 可选择当前或最近地图。
|
||||
string map_id = 1;
|
||||
// 建图或地图流会话 id。
|
||||
string session_id = 2;
|
||||
// 会话内单调递增序号，从 0 或 1 开始均可，但同一会话内必须保持递增。
|
||||
uint64 sequence = 3;
|
||||
// 用于断点续传或增量订阅的不透明令牌。
|
||||
string resume_token = 4;
|
||||
// 本条更新的数据维度。
|
||||
AgvMapDimension dimension = 5;
|
||||
// 本条更新是全量、增量还是缓存重置。
|
||||
AgvMapUpdateType update_type = 6;
|
||||
// 坐标系名称，例如 "map"。
|
||||
string frame_id = 7;
|
||||
// 本条更新产生时间，Unix 时间戳，单位：秒。
|
||||
double timestamp = 8;
|
||||
// 是否为全量快照的第一条消息。
|
||||
bool snapshot_begin = 9;
|
||||
// 是否为全量快照的最后一条消息。
|
||||
bool snapshot_end = 10;
|
||||
// 大地图分片发送时的分片序号，从 0 开始。
|
||||
uint32 chunk_index = 11;
|
||||
// 大地图分片总数；0 表示未知或连续流。
|
||||
uint32 chunk_count = 12;
|
||||
|
||||
oneof payload {
|
||||
// 统一 2D 地图或 2D 地图增量。
|
||||
AgvUnifiedMap2D map_2d = 20;
|
||||
// 统一 3D 地图或 3D 地图增量。
|
||||
AgvUnifiedMap3D map_3d = 21;
|
||||
}
|
||||
}
|
||||
|
||||
// 当前导航任务状态。
|
||||
message AgvNavigationStatus {
|
||||
// 导航状态，取值对应服务端内部 AgvTaskState 枚举的整数值。
|
||||
int32 state = 1;
|
||||
// 导航任务类型，取值对应服务端内部 AgvTaskType 枚举的整数值。
|
||||
int32 type = 2;
|
||||
// 当前任务进度，范围：[0, 1]；未知时置为 0。
|
||||
double progress = 3;
|
||||
// 状态描述或错误信息。
|
||||
string message = 4;
|
||||
}
|
||||
|
||||
124
protos/rbk/protocol/src1100_map3d.proto
Normal file
124
protos/rbk/protocol/src1100_map3d.proto
Normal file
@ -0,0 +1,124 @@
|
||||
syntax = "proto3";
|
||||
|
||||
package rbk.protocol;
|
||||
|
||||
// 仙工 SRC1100 3D 地图文件 0.3dsmap 的最小解析结构。
|
||||
// 这里只保留转换统一地图所需字段，未声明字段由 protobuf 作为未知字段跳过。
|
||||
|
||||
// 地图坐标系下的三维位置，单位：米。
|
||||
message Message_MapPos {
|
||||
// X 坐标，单位：米。
|
||||
double x = 1;
|
||||
// Y 坐标，单位：米。
|
||||
double y = 2;
|
||||
// Z 坐标，单位：米。
|
||||
double z = 3;
|
||||
}
|
||||
|
||||
// 仙工地图头信息。
|
||||
message Message_MapHeader {
|
||||
// 地图类型，例如 2D-Map 或 3D-Map。
|
||||
string map_type = 1;
|
||||
// 地图名称，通常对应地图文件名。
|
||||
string map_name = 2;
|
||||
// 地图最小边界点，单位：米。
|
||||
Message_MapPos min_pos = 3;
|
||||
// 地图最大边界点，单位：米。
|
||||
Message_MapPos max_pos = 4;
|
||||
// 地图分辨率，单位：米。
|
||||
double resolution = 5;
|
||||
// 地图格式版本号。
|
||||
string version = 8;
|
||||
}
|
||||
|
||||
// 三维浮点向量。
|
||||
message Vec3f {
|
||||
// X 分量。
|
||||
float x = 1;
|
||||
// Y 分量。
|
||||
float y = 2;
|
||||
// Z 分量。
|
||||
float z = 3;
|
||||
}
|
||||
|
||||
// 三维整数向量。
|
||||
message Vec3i {
|
||||
// X 分量。
|
||||
int32 x = 1;
|
||||
// Y 分量。
|
||||
int32 y = 2;
|
||||
// Z 分量。
|
||||
int32 z = 3;
|
||||
}
|
||||
|
||||
// 仙工 3D 特征地图参数。
|
||||
message FeatureMapParams {
|
||||
// 激光测距标准差，单位：米。
|
||||
float ranging_sigma = 1;
|
||||
// 激光测角标准差，单位：度。
|
||||
float angle_sigma = 2;
|
||||
// 最大体素边长，单位：米。
|
||||
float max_voxel_size = 3;
|
||||
// 八叉树最大层数。
|
||||
uint32 max_layer = 4;
|
||||
// 平面协方差停止更新的点数阈值。
|
||||
uint32 cov_fixed_pts_num = 5;
|
||||
// 平面停止更新的点数阈值。
|
||||
uint32 plane_fixed_pts_num = 6;
|
||||
// 有效平面协方差最小特征值阈值。
|
||||
float plane_min_eigen_value = 7;
|
||||
// 每层评估平面所需的最少点数。
|
||||
repeated int32 each_layer_least_pts_num = 8;
|
||||
}
|
||||
|
||||
// 仙工 3D 平面特征。
|
||||
message FeatureMapPlane {
|
||||
// 平面中心点，单位：米。
|
||||
Vec3f center = 1;
|
||||
// 平面法向量。
|
||||
Vec3f normal = 2;
|
||||
// 平面方程 Ax + By + Cz + D = 0 中的 D。
|
||||
float d = 3;
|
||||
// 平面特征近似半径，单位：米。
|
||||
float radius = 4;
|
||||
// 平面协方差矩阵，按 6x6 展平。
|
||||
repeated float plane_cov = 5;
|
||||
}
|
||||
|
||||
// 仙工特征地图中的八叉树节点。
|
||||
message OctoTree {
|
||||
// 对应的平面 ID。
|
||||
uint32 plane_id = 1;
|
||||
// 子节点序列，-1 表示当前层无其他子节点。
|
||||
repeated int32 child_id_list = 2;
|
||||
}
|
||||
|
||||
// 同一外层体素位置下的八叉树节点集合。
|
||||
message OctoTrees {
|
||||
// 八叉树节点列表。
|
||||
repeated OctoTree octo_tree = 1;
|
||||
}
|
||||
|
||||
// 仙工 3D 特征地图。
|
||||
message FeatureMap3D {
|
||||
// 特征地图参数。
|
||||
FeatureMapParams params = 1;
|
||||
// 平面特征列表。
|
||||
repeated FeatureMapPlane planes = 2;
|
||||
// 每个外层体素对应的八叉树节点集合。
|
||||
repeated OctoTrees octo_trees = 3;
|
||||
// 最外层八叉树体素坐标。
|
||||
repeated Vec3i voxel_locs = 4;
|
||||
}
|
||||
|
||||
// 0.3dsmap 顶层消息。
|
||||
message Message_Map3D {
|
||||
// 地图目录或地图包内部目录名。
|
||||
string map_directory = 1;
|
||||
// 地图头信息。
|
||||
Message_MapHeader header = 2;
|
||||
// 普通 3D 点云点。
|
||||
repeated Message_MapPos normal_pos3d_list = 3;
|
||||
// 3D 特征地图。
|
||||
FeatureMap3D feature_map_3d = 4;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user