From 2fa161f1cc9cf606ce2848cd9c6514a4e1c674e4 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Tue, 7 Jul 2026 15:54:09 +0800 Subject: [PATCH] update src1100 module --- cmvr-es/common/types/agv/agv_types.h | 181 +++ cmvr-es/config/devices/agv/src1100.pb.txt | 23 +- cmvr-es/config/manager/device_manager.pb.txt | 9 +- cmvr-es/devices/agv/abstract_agv.h | 49 + .../devices/agv/src1100/include/src1100_agv.h | 76 + .../devices/agv/src1100/src/src1100_agv.cpp | 1380 ++++++++++++++++- .../service/grpc/include/grpc_agv_service.h | 9 + cmvr-es/service/grpc/src/grpc_agv_service.cpp | 271 ++++ protos/cmvr/api/agv_command.proto | 110 ++ protos/cmvr/api/agv_service.proto | 46 + .../cmvr/config/agv_config/agv_config.proto | 39 + protos/cmvr/msgs/agv.proto | 237 +++ protos/rbk/protocol/src1100_map3d.proto | 124 ++ 13 files changed, 2488 insertions(+), 66 deletions(-) create mode 100644 protos/rbk/protocol/src1100_map3d.proto diff --git a/cmvr-es/common/types/agv/agv_types.h b/cmvr-es/common/types/agv/agv_types.h index 4c9986e1..5c7ef69c 100644 --- a/cmvr-es/common/types/agv/agv_types.h +++ b/cmvr-es/common/types/agv/agv_types.h @@ -1,6 +1,7 @@ #ifndef CMVR_ES_AGV_TYPES_H #define CMVR_ES_AGV_TYPES_H +#include #include #include #include @@ -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 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 points; + double heading{0.0}; + std::unordered_map 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 data; + std::vector 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 points; + std::vector voxels; + std::vector planes; + std::vector 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 map_2d; + std::optional map_3d; +}; + /** * @brief 当前导航任务状态。 * diff --git a/cmvr-es/config/devices/agv/src1100.pb.txt b/cmvr-es/config/devices/agv/src1100.pb.txt index 64790217..833bde58 100644 --- a/cmvr-es/config/devices/agv/src1100.pb.txt +++ b/cmvr-es/config/devices/agv/src1100.pb.txt @@ -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 } } } diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index fc3a4e1e..21dd6ee9 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -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 diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 0e2af167..b8ad4a61 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -6,6 +6,7 @@ #define CMVR_ES_ABSTRACT_AGV_H #pragma once +#include #include #include @@ -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 diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h index d86a92f8..ab5bcb92 100644 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -1,8 +1,13 @@ #ifndef CMVR_ES_SRC1100_AGV_H #define CMVR_ES_SRC1100_AGV_H +#include +#include +#include +#include #include #include +#include #include #include @@ -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& updates) const; + AgvResult parseSrc1100MapArchive_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + std::vector& 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 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 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 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 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 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 diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp index 9eb5d216..7c1b1b2b 100644 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -3,14 +3,25 @@ #include #include #include +#include +#include #include #include +#include +#include +#include +#include #include +#include #include #include #include +#include + +#include #include "common/base/logging/logger.h" +#include "rbk/protocol/src1100_map3d.pb.h" namespace cmvr::device { @@ -21,6 +32,8 @@ constexpr std::uint16_t kRobotStatusBattery = 1007; constexpr std::uint16_t kRobotStatusTask = 1020; constexpr std::uint16_t kRobotStatusMap = 1300; constexpr std::uint16_t kRobotStatusStation = 1301; +constexpr std::uint16_t kRobotStatusMappingFileList = 1780; +constexpr std::uint16_t kRobotStatusDownloadFile = 1800; constexpr std::uint16_t kRobotControlStop = 2000; constexpr std::uint16_t kRobotControlMotion = 2010; constexpr std::uint16_t kRobotControlLoadMap = 2022; @@ -31,12 +44,99 @@ constexpr std::uint16_t kRobotTaskGoTarget = 3051; constexpr std::uint16_t kRobotTaskGoTargetList = 3066; constexpr std::uint16_t kRobotConfigUploadMap = 4010; constexpr std::uint16_t kRobotConfigDownloadMap = 4011; +constexpr std::uint16_t kRobotOtherStartMapping = 6100; +constexpr std::uint16_t kRobotOtherStopMapping = 6101; +constexpr std::uint16_t kRobotPushConfigReq = 9300; +constexpr std::uint16_t kRobotPushConfigRes = 19300; +constexpr std::uint16_t kRobotPush = 19301; +constexpr std::uint32_t kMaxFramePayloadBytes = 512U * 1024U * 1024U; +constexpr int kDefaultMapUpdateIntervalMs = 1000; +constexpr std::size_t kDefaultMapUpdateHistorySize = 8; +constexpr std::uint64_t kMapSnapshotSequenceStart = 1; + +namespace fs = std::filesystem; std::string systemError() { return std::strerror(errno); } +bool wants2D(const AgvMapDimension dimension) +{ + return dimension == AgvMapDimension::Unspecified + || dimension == AgvMapDimension::Map2D + || dimension == AgvMapDimension::Map2DAnd3D; +} + +bool wants3D(const AgvMapDimension dimension) +{ + return dimension == AgvMapDimension::Unspecified + || dimension == AgvMapDimension::Map3D + || dimension == AgvMapDimension::Map2DAnd3D; +} + +bool contentLooksLikeZip(const std::string& content) +{ + return content.size() >= 4 + && static_cast(content[0]) == 0x50U + && static_cast(content[1]) == 0x4BU + && static_cast(content[2]) == 0x03U + && static_cast(content[3]) == 0x04U; +} + +bool contentLooksLikeJson(const std::string& content) +{ + const auto pos = content.find_first_not_of(" \t\r\n"); + return pos != std::string::npos && (content[pos] == '{' || content[pos] == '['); +} + +std::string shellQuote(const std::string& value) +{ + std::string quoted = "'"; + for (const char ch : value) { + if (ch == '\'') { + quoted += "'\\''"; + } else { + quoted += ch; + } + } + quoted += "'"; + return quoted; +} + +bool writeBinaryFile(const fs::path& path, const std::string& content) +{ + std::ofstream output(path, std::ios::binary); + if (!output) { + return false; + } + output.write(content.data(), static_cast(content.size())); + return output.good(); +} + +bool readBinaryFile(const fs::path& path, std::string& content) +{ + std::ifstream input(path, std::ios::binary); + if (!input) { + return false; + } + std::ostringstream buffer; + buffer << input.rdbuf(); + content = buffer.str(); + return true; +} + +fs::path makeTempDirectory() +{ + auto pattern = fs::temp_directory_path() / "cmvr_src1100_map_XXXXXX"; + std::string path = pattern.string(); + char* created = ::mkdtemp(path.data()); + if (!created) { + return {}; + } + return fs::path(created); +} + Json::Value& jsonMember(Json::Value& value, const char* key) { return *value.demand(key, key + std::strlen(key)); @@ -58,6 +158,129 @@ Json::Value jsonGet(const Json::Value& value, const char* key, const Json::Value return found ? *found : fallback; } +double nowSeconds() +{ + const auto now = std::chrono::system_clock::now().time_since_epoch(); + return std::chrono::duration(now).count(); +} + +bool jsonHas(const Json::Value& value, const char* key) +{ + return jsonFind(value, key) != nullptr; +} + +bool hasFaultArray(const Json::Value& value, const char* key) +{ + const auto* found = jsonFind(value, key); + return found && found->isArray() && !found->empty(); +} + +std::string jsonValueToString(const Json::Value& value) +{ + if (value.isString()) return value.asString(); + if (value.isBool()) return value.asBool() ? "true" : "false"; + if (value.isInt64() || value.isInt()) return std::to_string(value.asInt64()); + if (value.isUInt64() || value.isUInt()) return std::to_string(value.asUInt64()); + if (value.isDouble()) return std::to_string(value.asDouble()); + if (value.isNull()) return {}; + + Json::StreamWriterBuilder builder; + builder["indentation"] = ""; + return Json::writeString(builder, value); +} + +void putPropertyIfPresent( + std::unordered_map& properties, + const Json::Value& value, + const char* json_key, + const char* property_key) +{ + const auto* found = jsonFind(value, json_key); + if (!found || found->isNull()) { + return; + } + properties[property_key] = jsonValueToString(*found); +} + +void appendMapProperties( + std::unordered_map& properties, + const Json::Value& value, + const char* key) +{ + const auto* list = jsonFind(value, key); + if (!list || !list->isArray()) { + return; + } + + for (const auto& item : *list) { + const std::string property_key = jsonGet(item, "key", "").asString(); + if (property_key.empty()) { + continue; + } + + const char* value_keys[] = { + "string_value", + "bool_value", + "int32_value", + "uint32_value", + "int64_value", + "uint64_value", + "float_value", + "double_value", + "bytes_value", + "value" + }; + for (const char* value_key : value_keys) { + const auto* found = jsonFind(item, value_key); + if (found && !found->isNull()) { + properties[property_key] = jsonValueToString(*found); + break; + } + } + } +} + +AgvMapPoint3D jsonPoint3D(const Json::Value& value) +{ + AgvMapPoint3D point; + point.x = jsonGet(value, "x", 0.0).asDouble(); + point.y = jsonGet(value, "y", 0.0).asDouble(); + point.z = jsonGet(value, "z", 0.0).asDouble(); + return point; +} + +void appendObject( + AgvUnifiedMap2D& map, + std::string id, + const AgvMapObjectType type, + std::vector points, + const double heading, + const Json::Value& source) +{ + AgvMapObject object; + object.id = std::move(id); + object.type = type; + object.points = std::move(points); + object.heading = heading; + putPropertyIfPresent(object.properties, source, "class_name", "class_name"); + putPropertyIfPresent(object.properties, source, "type", "type"); + putPropertyIfPresent(object.properties, source, "description", "description"); + appendMapProperties(object.properties, source, "property"); + map.objects.push_back(std::move(object)); +} + +void appendStringArray(Json::Value& value, const char* key, const google::protobuf::RepeatedPtrField& strings) +{ + if (strings.empty()) { + return; + } + Json::Value array(Json::arrayValue); + for (const auto& item : strings) { + array.append(item); + } + jsonMember(value, key) = array; +} + AgvMode modeFromTaskState(const int state) { switch (state) { @@ -116,7 +339,11 @@ AgvTaskType toTaskType(const int value) Src1100Agv::Src1100Agv(const config::Src1100AgvConfig& cfg) : config_(cfg), ip_(cfg.ip()), - recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000) + recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000), + state_push_enabled_(cfg.enable_state_push()), + map_update_enabled_(cfg.enable_map_update()), + map_update_interval_ms_(cfg.map_update_interval_ms() > 0 ? cfg.map_update_interval_ms() : kDefaultMapUpdateIntervalMs), + map_update_history_size_(cfg.map_update_history_size() > 0 ? cfg.map_update_history_size() : kDefaultMapUpdateHistorySize) { id_ = cfg.id(); if (cfg.port_status() > 0) ports_.status = cfg.port_status(); @@ -161,6 +388,24 @@ bool Src1100Agv::update() } AgvRuntimeState Src1100Agv::runtimeState() const +{ + if (state_push_enabled_) { + std::lock_guard lock(runtime_state_mutex_); + if (cached_runtime_state_valid_) { + auto state = cached_runtime_state_; + state.connected = connected_(); + state.last_error = last_error_; + if (!state.connected) { + state.mode = AgvMode::Disconnected; + } + return state; + } + } + + return queryRuntimeState_(); +} + +AgvRuntimeState Src1100Agv::queryRuntimeState_() const { AgvRuntimeState state; state.connected = connected_(); @@ -222,49 +467,83 @@ AgvNavigationStatus Src1100Agv::navigationStatus() const AgvResult Src1100Agv::connect_() { - std::lock_guard lock(mutex_); - closeSocket_(sock_status_); - closeSocket_(sock_control_); - closeSocket_(sock_navigation_); - closeSocket_(sock_config_); - closeSocket_(sock_other_); - closeSocket_(sock_push_); + stopPushThread_(); + stopMapUpdateThread_(); - if (ip_.empty()) { - return AgvResult::failure(AgvErrorCode::InvalidArgument, "SRC1100 AGV ip is empty"); - } - - const auto close_all = [this]() { + { + std::lock_guard lock(mutex_); closeSocket_(sock_status_); closeSocket_(sock_control_); closeSocket_(sock_navigation_); closeSocket_(sock_config_); closeSocket_(sock_other_); closeSocket_(sock_push_); - }; - if (auto result = connectSocket_(sock_status_, ports_.status); !result.ok()) { - close_all(); - return result; + if (ip_.empty()) { + return AgvResult::failure(AgvErrorCode::InvalidArgument, "SRC1100 AGV ip is empty"); + } + + const auto close_all = [this]() { + closeSocket_(sock_status_); + closeSocket_(sock_control_); + closeSocket_(sock_navigation_); + closeSocket_(sock_config_); + closeSocket_(sock_other_); + closeSocket_(sock_push_); + }; + + if (auto result = connectSocket_(sock_status_, ports_.status); !result.ok()) { + close_all(); + return result; + } + if (auto result = connectSocket_(sock_control_, ports_.control); !result.ok()) { + close_all(); + return result; + } + if (auto result = connectSocket_(sock_navigation_, ports_.navigation); !result.ok()) { + close_all(); + return result; + } + if (auto result = connectSocket_(sock_config_, ports_.config); !result.ok()) { + close_all(); + return result; + } + + if (state_push_enabled_) { + const auto result = connectSocket_(sock_push_, ports_.push); + if (!result.ok()) { + CMVR_LOG(ERROR) << "[Src1100Agv] Connect push port failed" + << ", id=" << id_ + << ", port=" << ports_.push + << ", error=" << result.message; + closeSocket_(sock_push_); + } + } + last_error_.clear(); } - if (auto result = connectSocket_(sock_control_, ports_.control); !result.ok()) { - close_all(); - return result; + + if (state_push_enabled_ && sock_push_ >= 0) { + const auto result = configurePush_(); + if (result.ok()) { + startPushThread_(); + } else { + CMVR_LOG(ERROR) << "[Src1100Agv] Configure push failed" + << ", id=" << id_ + << ", error=" << result.message; + std::lock_guard lock(mutex_); + closeSocket_(sock_push_); + } } - if (auto result = connectSocket_(sock_navigation_, ports_.navigation); !result.ok()) { - close_all(); - return result; + if (map_update_enabled_) { + startMapUpdateThread_(); } - if (auto result = connectSocket_(sock_config_, ports_.config); !result.ok()) { - close_all(); - return result; - } - last_error_.clear(); return AgvResult::success(); } AgvResult Src1100Agv::disconnect_() { + stopMapUpdateThread_(); + stopPushThread_(); std::lock_guard lock(mutex_); closeSocket_(sock_status_); closeSocket_(sock_control_); @@ -436,6 +715,735 @@ AgvResult Src1100Agv::downloadMap(const std::string& map_name, std::string& cont return resultFromResponse_(response); } +AgvResult Src1100Agv::startMapping(const AgvMappingOptions& options) +{ + auto result = ensureOtherSocket_(); + if (!result.ok()) return result; + + Json::Value payload(Json::objectValue); + jsonMember(payload, "slam_type") = options.dimension == AgvMapDimension::Map2D ? 2 : 4; + jsonMember(payload, "real_time") = options.real_time; + if (!options.map_name.empty()) { + jsonMember(payload, "map_name") = options.map_name; + } + + Json::Value response; + result = sendCommand_(sock_other_, kRobotOtherStartMapping, payload, &response); + result = result.ok() ? resultFromResponse_(response) : result; + if (result.ok()) { + { + std::lock_guard lock(map_update_mutex_); + cached_map_updates_.clear(); + next_mapping_index_ = 0; + last_map_content_hash_ = 0; + map_sequence_ = 0; + map_session_id_ = id_ + "_mapping_" + std::to_string(static_cast(nowSeconds() * 1000.0)); + } + if (map_update_enabled_ || options.real_time) { + startMapUpdateThread_(); + } + } + return result; +} + +AgvResult Src1100Agv::getMappingData(const int start_index, AgvMappingData& data) const +{ + if (start_index < 0) { + return AgvResult::failure(AgvErrorCode::InvalidArgument, "mapping data start_index must be >= 0"); + } + + Json::Value list_payload(Json::objectValue); + jsonMember(list_payload, "index") = start_index; + + Json::Value list_response; + auto result = sendCommand_(sock_status_, kRobotStatusMappingFileList, list_payload, &list_response); + if (!result.ok()) return result; + result = resultFromResponse_(list_response); + if (!result.ok()) return result; + + data = {}; + data.start_index = start_index; + data.next_index = start_index; + + const auto* list = jsonFind(list_response, "list"); + if (!list || !list->isArray()) { + return AgvResult::success(); + } + + for (const auto& item : *list) { + const std::string file_name = item.asString(); + if (file_name.empty()) { + continue; + } + + Json::Value download_payload(Json::objectValue); + jsonMember(download_payload, "type") = "users"; + jsonMember(download_payload, "file_path") = file_name; + + std::string content; + result = sendCommandRaw_(sock_status_, kRobotStatusDownloadFile, download_payload, &content); + if (!result.ok()) return result; + + Json::Value maybe_error; + std::string parse_error; + if (parseJson_(content, maybe_error, parse_error) && maybe_error.isObject()) { + result = resultFromResponse_(maybe_error); + if (!result.ok()) return result; + content = jsonGet(maybe_error, "content", jsonGet(maybe_error, "file_content", content)).asString(); + } + + AgvMappingDataFile file; + file.name = file_name; + file.content = std::move(content); + data.files.push_back(std::move(file)); + } + + data.next_index = data.start_index + static_cast(data.files.size()); + return AgvResult::success(); +} + +AgvResult Src1100Agv::getUnifiedMapUpdate( + const std::uint64_t after_sequence, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + if (findCachedMapUpdate_(after_sequence, options, update)) { + return AgvResult::success(); + } + + const auto refresh_result = refreshMapCacheOnce_(options); + if (findCachedMapUpdate_(after_sequence, options, update)) { + return AgvResult::success(); + } + if (!refresh_result.ok() && refresh_result.code != AgvErrorCode::Timeout) { + return refresh_result; + } + + const auto wait_ms = options.wait_timeout_ms > 0 ? options.wait_timeout_ms : 1000; + std::unique_lock lock(map_update_mutex_); + const auto effective_after = [&]() { + if (after_sequence != 0 || options.resume_token.empty()) { + return after_sequence; + } + try { + return static_cast(std::stoull(options.resume_token)); + } catch (...) { + return std::uint64_t{0}; + } + }(); + const auto find_locked = [&]() { + for (const auto& candidate : cached_map_updates_) { + if (candidate.sequence > effective_after && mapUpdateMatches_(candidate, options)) { + update = candidate; + return true; + } + } + return false; + }; + + if (find_locked()) { + return AgvResult::success(); + } + const bool ready = map_update_cv_.wait_for( + lock, + std::chrono::milliseconds(wait_ms), + find_locked); + if (ready) { + return AgvResult::success(); + } + return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 unified map update timeout"); +} + +void Src1100Agv::startMapUpdateThread_() +{ + if (map_update_running_.exchange(true)) { + return; + } + map_update_thread_ = std::thread(&Src1100Agv::mapUpdateLoop_, this); +} + +void Src1100Agv::stopMapUpdateThread_() +{ + const bool was_running = map_update_running_.exchange(false); + if (was_running) { + map_update_cv_.notify_all(); + } + if (map_update_thread_.joinable()) { + map_update_thread_.join(); + } +} + +void Src1100Agv::mapUpdateLoop_() +{ + while (map_update_running_) { + AgvMapStreamOptions options; + options.dimension = AgvMapDimension::Map2DAnd3D; + options.snapshot = true; + options.incremental = true; + options.wait_timeout_ms = 0; + + const auto result = refreshMapCacheOnce_(options); + if (!result.ok() && result.code != AgvErrorCode::Timeout) { + std::lock_guard lock(mutex_); + last_error_ = result.message; + } + + std::unique_lock lock(map_update_mutex_); + map_update_cv_.wait_for( + lock, + std::chrono::milliseconds(map_update_interval_ms_), + [this]() { return !map_update_running_; }); + } +} + +AgvResult Src1100Agv::refreshMapCacheOnce_(const AgvMapStreamOptions& options) const +{ + int start_index = 0; + { + std::lock_guard lock(map_update_mutex_); + start_index = next_mapping_index_; + } + + AgvMappingData mapping_data; + auto result = getMappingData(start_index, mapping_data); + if (result.ok() && !mapping_data.files.empty()) { + std::vector updates; + for (const auto& file : mapping_data.files) { + std::vector file_updates; + const auto parse_result = parseMapFileToUpdates_(file.name, file.content, options, file_updates); + if (!parse_result.ok()) { + CMVR_LOG(ERROR) << "[Src1100Agv] Parse mapping file failed" + << ", id=" << id_ + << ", file=" << file.name + << ", error=" << parse_result.message; + continue; + } + updates.insert( + updates.end(), + std::make_move_iterator(file_updates.begin()), + std::make_move_iterator(file_updates.end())); + } + { + std::lock_guard lock(map_update_mutex_); + next_mapping_index_ = std::max(next_mapping_index_, mapping_data.next_index); + } + if (!updates.empty()) { + cacheMapUpdates_(std::move(updates)); + return AgvResult::success(); + } + } + + std::string map_name = options.map_name; + if (map_name.empty()) { + const auto state = runtimeState(); + map_name = state.current_map; + } + if (map_name.empty()) { + std::vector maps; + if (listMaps(maps).ok() && !maps.empty()) { + map_name = maps.back(); + } + } + if (map_name.empty()) { + return result.ok() + ? AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 no map file is available") + : result; + } + + std::string content; + result = downloadMap(map_name, content); + if (!result.ok()) { + return result; + } + const auto content_hash = std::hash{}(content); + std::size_t last_map_content_hash = 0; + { + std::lock_guard lock(map_update_mutex_); + last_map_content_hash = last_map_content_hash_; + } + AgvUnifiedMapUpdate cached; + if (content_hash == last_map_content_hash && findCachedMapUpdate_(0, options, cached)) { + return AgvResult::success(); + } + + std::vector updates; + result = parseMapFileToUpdates_(map_name, content, options, updates); + if (!result.ok()) { + return result; + } + if (updates.empty()) { + return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 map file has no requested dimension"); + } + + { + std::lock_guard lock(map_update_mutex_); + last_map_content_hash_ = content_hash; + } + cacheMapUpdates_(std::move(updates)); + return AgvResult::success(); +} + +AgvResult Src1100Agv::parseMapFileToUpdates_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + std::vector& updates) const +{ + if (content.empty()) { + return AgvResult::failure(AgvErrorCode::InvalidArgument, "SRC1100 map file is empty: " + file_name); + } + + if (contentLooksLikeZip(content)) { + return parseSrc1100MapArchive_(file_name, content, options, updates); + } + + if (contentLooksLikeJson(content)) { + if (wants2D(options.dimension)) { + AgvUnifiedMapUpdate update; + const auto result = parseSrc1100Map2D_(file_name, content, options, update); + if (!result.ok()) { + return result; + } + updates.push_back(std::move(update)); + } + return AgvResult::success(); + } + + if (wants3D(options.dimension)) { + AgvUnifiedMapUpdate update; + const auto result = parseSrc1100Map3D_(file_name, content, options, update); + if (!result.ok()) { + return result; + } + updates.push_back(std::move(update)); + return AgvResult::success(); + } + + return AgvResult::success(); +} + +AgvResult Src1100Agv::parseSrc1100MapArchive_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + std::vector& updates) const +{ + const auto temp_dir = makeTempDirectory(); + if (temp_dir.empty()) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "create temporary map directory failed: " + systemError()); + } + + const auto archive_path = temp_dir / "map.smap"; + if (!writeBinaryFile(archive_path, content)) { + fs::remove_all(temp_dir); + return AgvResult::failure(AgvErrorCode::CommandFailed, "write temporary map archive failed"); + } + + const std::string command = "unzip -qq -o " + + shellQuote(archive_path.string()) + + " -d " + + shellQuote(temp_dir.string()); + const int unzip_result = std::system(command.c_str()); + if (unzip_result != 0) { + fs::remove_all(temp_dir); + return AgvResult::failure(AgvErrorCode::CommandFailed, "unzip SRC1100 smap archive failed: " + file_name); + } + + if (wants2D(options.dimension)) { + std::string map2d_content; + if (readBinaryFile(temp_dir / "0.smap", map2d_content)) { + AgvUnifiedMapUpdate update; + const auto result = parseSrc1100Map2D_(file_name, map2d_content, options, update); + if (result.ok()) { + updates.push_back(std::move(update)); + } else { + CMVR_LOG(ERROR) << "[Src1100Agv] Parse 0.smap failed" + << ", id=" << id_ + << ", file=" << file_name + << ", error=" << result.message; + } + } + } + + if (wants3D(options.dimension)) { + std::string map3d_content; + if (readBinaryFile(temp_dir / "0.3dsmap", map3d_content)) { + AgvUnifiedMapUpdate update; + const auto result = parseSrc1100Map3D_(file_name, map3d_content, options, update); + if (result.ok()) { + updates.push_back(std::move(update)); + } else { + CMVR_LOG(ERROR) << "[Src1100Agv] Parse 0.3dsmap failed" + << ", id=" << id_ + << ", file=" << file_name + << ", error=" << result.message; + } + } + } + + fs::remove_all(temp_dir); + return updates.empty() + ? AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 smap archive has no requested map data: " + file_name) + : AgvResult::success(); +} + +AgvResult Src1100Agv::parseSrc1100Map2D_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + Json::Value root; + std::string error; + if (!parseJson_(content, root, error)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "parse SRC1100 2D map json failed: " + error); + } + if (!root.isObject()) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 2D map json root is not object"); + } + + const auto* header_ptr = jsonFind(root, "header"); + const Json::Value& header = header_ptr && header_ptr->isObject() ? *header_ptr : root; + + AgvUnifiedMap2D map; + map.frame_id = "map"; + map.timestamp = nowSeconds(); + map.resolution = jsonGet(header, "resolution", 0.0).asDouble(); + if (const auto* min_pos = jsonFind(header, "min_pos")) { + map.origin.x = jsonGet(*min_pos, "x", 0.0).asDouble(); + map.origin.y = jsonGet(*min_pos, "y", 0.0).asDouble(); + map.origin.theta = 0.0; + } + if (const auto* max_pos = jsonFind(header, "max_pos"); + max_pos && map.resolution > 0.0) { + const double width_m = jsonGet(*max_pos, "x", map.origin.x).asDouble() - map.origin.x; + const double height_m = jsonGet(*max_pos, "y", map.origin.y).asDouble() - map.origin.y; + if (width_m > 0.0 && height_m > 0.0) { + map.width = static_cast(std::ceil(width_m / map.resolution)); + map.height = static_cast(std::ceil(height_m / map.resolution)); + } + } + + const auto make_id = [](const Json::Value& value, const char* prefix, const int index) { + std::string id = jsonGet(value, "instance_name", "").asString(); + if (id.empty()) id = jsonGet(value, "id", "").asString(); + if (id.empty()) id = jsonGet(value, "name", "").asString(); + if (id.empty()) id = jsonGet(value, "point_name", "").asString(); + if (id.empty() && jsonHas(value, "tag_value")) { + id = std::to_string(jsonGet(value, "tag_value", 0).asUInt()); + } + if (id.empty()) id = std::string(prefix) + "_" + std::to_string(index); + return id; + }; + + if (const auto* list = jsonFind(root, "advanced_point_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + const auto* pos = jsonFind(item, "pos"); + appendObject( + map, + make_id(item, "station", index++), + AgvMapObjectType::Station, + pos ? std::vector{jsonPoint3D(*pos)} : std::vector{}, + jsonGet(item, "dir", 0.0).asDouble(), + item); + } + } + + if (const auto* list = jsonFind(root, "normal_line_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* start = jsonFind(item, "start_pos")) points.push_back(jsonPoint3D(*start)); + if (const auto* end = jsonFind(item, "end_pos")) points.push_back(jsonPoint3D(*end)); + appendObject(map, make_id(item, "normal_line", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); + } + } + + if (const auto* list = jsonFind(root, "advanced_line_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* line = jsonFind(item, "line")) { + if (const auto* start = jsonFind(*line, "start_pos")) points.push_back(jsonPoint3D(*start)); + if (const auto* end = jsonFind(*line, "end_pos")) points.push_back(jsonPoint3D(*end)); + } + appendObject(map, make_id(item, "line", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); + } + } + + if (const auto* list = jsonFind(root, "advanced_curve_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* start = jsonFind(item, "start_pos")) { + if (const auto* pos = jsonFind(*start, "pos")) points.push_back(jsonPoint3D(*pos)); + } + if (const auto* control = jsonFind(item, "control_pos1")) points.push_back(jsonPoint3D(*control)); + if (const auto* control = jsonFind(item, "control_pos2")) points.push_back(jsonPoint3D(*control)); + if (const auto* control = jsonFind(item, "control_pos3")) points.push_back(jsonPoint3D(*control)); + if (const auto* control = jsonFind(item, "control_pos4")) points.push_back(jsonPoint3D(*control)); + if (const auto* end = jsonFind(item, "end_pos")) { + if (const auto* pos = jsonFind(*end, "pos")) points.push_back(jsonPoint3D(*pos)); + } + appendObject(map, make_id(item, "curve", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); + } + } + + if (const auto* list = jsonFind(root, "advanced_area_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* pos_group = jsonFind(item, "pos_group"); pos_group && pos_group->isArray()) { + for (const auto& pos : *pos_group) points.push_back(jsonPoint3D(pos)); + } + appendObject( + map, + make_id(item, "area", index++), + AgvMapObjectType::Area, + std::move(points), + jsonGet(item, "dir", 0.0).asDouble(), + item); + } + } + + if (const auto* list = jsonFind(root, "reflector_pos_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + appendObject( + map, + make_id(item, "reflector", index++), + AgvMapObjectType::Reflector, + {jsonPoint3D(item)}, + 0.0, + item); + } + } + + if (const auto* list = jsonFind(root, "tag_pos_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + appendObject( + map, + make_id(item, "tag", index++), + AgvMapObjectType::QrTag, + {jsonPoint3D(item)}, + jsonGet(item, "angle", 0.0).asDouble(), + item); + } + } + + if (const auto* list = jsonFind(root, "external_device_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + appendObject( + map, + make_id(item, "external_device", index++), + AgvMapObjectType::ExternalDevice, + {}, + 0.0, + item); + } + } + + if (const auto* groups = jsonFind(root, "bin_locations_list"); groups && groups->isArray()) { + int index = 0; + for (const auto& group : *groups) { + const auto* list = jsonFind(group, "bin_location_list"); + if (!list || !list->isArray()) { + continue; + } + for (const auto& item : *list) { + const auto* pos = jsonFind(item, "pos"); + appendObject( + map, + make_id(item, "bin_location", index++), + AgvMapObjectType::BinLocation, + pos ? std::vector{jsonPoint3D(*pos)} : std::vector{}, + 0.0, + item); + } + } + } + + std::string map_id = options.map_name; + if (map_id.empty()) map_id = jsonGet(header, "map_name", "").asString(); + if (map_id.empty()) map_id = file_name; + + update = {}; + update.map_id = map_id; + update.dimension = AgvMapDimension::Map2D; + update.update_type = AgvMapUpdateType::Snapshot; + update.frame_id = map.frame_id; + update.timestamp = map.timestamp; + update.snapshot_begin = true; + update.snapshot_end = true; + update.chunk_index = 0; + update.chunk_count = 1; + update.map_2d = std::move(map); + return AgvResult::success(); +} + +AgvResult Src1100Agv::parseSrc1100Map3D_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + rbk::protocol::Message_Map3D src; + if (!src.ParseFromString(content)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "parse SRC1100 3D map protobuf failed: " + file_name); + } + + AgvUnifiedMap3D map; + map.frame_id = "map"; + map.timestamp = nowSeconds(); + if (src.has_feature_map_3d() && src.feature_map_3d().has_params()) { + map.voxel_resolution = src.feature_map_3d().params().max_voxel_size(); + } else if (src.has_header()) { + map.voxel_resolution = src.header().resolution(); + } + + map.points.reserve(static_cast(src.normal_pos3d_list_size())); + for (const auto& point : src.normal_pos3d_list()) { + AgvMapPointSample3D sample; + sample.x = point.x(); + sample.y = point.y(); + sample.z = point.z(); + map.points.push_back(sample); + } + + if (src.has_feature_map_3d()) { + const auto& feature_map = src.feature_map_3d(); + map.planes.reserve(static_cast(feature_map.planes_size())); + for (const auto& plane : feature_map.planes()) { + AgvMapPlane3D dst; + dst.center = {plane.center().x(), plane.center().y(), plane.center().z()}; + dst.normal = {plane.normal().x(), plane.normal().y(), plane.normal().z()}; + dst.d = plane.d(); + dst.radius = plane.radius(); + map.planes.push_back(dst); + } + + map.voxels.reserve(static_cast(feature_map.voxel_locs_size())); + for (const auto& voxel : feature_map.voxel_locs()) { + AgvMapVoxel3D dst; + dst.x = voxel.x(); + dst.y = voxel.y(); + dst.z = voxel.z(); + dst.probability = 1.0F; + map.voxels.push_back(dst); + } + } + + std::string map_id = options.map_name; + if (map_id.empty() && src.has_header()) map_id = src.header().map_name(); + if (map_id.empty()) map_id = src.map_directory(); + if (map_id.empty()) map_id = file_name; + + update = {}; + update.map_id = map_id; + update.dimension = AgvMapDimension::Map3D; + update.update_type = AgvMapUpdateType::Snapshot; + update.frame_id = map.frame_id; + update.timestamp = map.timestamp; + update.snapshot_begin = true; + update.snapshot_end = true; + update.chunk_index = 0; + update.chunk_count = 1; + update.map_3d = std::move(map); + return AgvResult::success(); +} + +void Src1100Agv::cacheMapUpdates_(std::vector updates) const +{ + if (updates.empty()) { + return; + } + + { + std::lock_guard lock(map_update_mutex_); + if (map_session_id_.empty()) { + map_session_id_ = id_ + "_map"; + } + if (map_sequence_ == 0) { + map_sequence_ = kMapSnapshotSequenceStart - 1; + } + for (auto& update : updates) { + update.sequence = ++map_sequence_; + update.session_id = map_session_id_; + update.resume_token = std::to_string(update.sequence); + if (update.timestamp <= 0.0) update.timestamp = nowSeconds(); + if (update.frame_id.empty()) update.frame_id = "map"; + if (update.map_id.empty()) update.map_id = id_; + if (update.update_type == AgvMapUpdateType::Unspecified) { + update.update_type = AgvMapUpdateType::Snapshot; + } + cached_map_updates_.push_back(std::move(update)); + } + while (cached_map_updates_.size() > map_update_history_size_) { + cached_map_updates_.pop_front(); + } + } + map_update_cv_.notify_all(); +} + +bool Src1100Agv::findCachedMapUpdate_( + const std::uint64_t after_sequence, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + std::uint64_t effective_after = after_sequence; + if (effective_after == 0 && !options.resume_token.empty()) { + try { + effective_after = static_cast(std::stoull(options.resume_token)); + } catch (...) { + effective_after = 0; + } + } + + std::lock_guard lock(map_update_mutex_); + for (const auto& candidate : cached_map_updates_) { + if (candidate.sequence > effective_after && mapUpdateMatches_(candidate, options)) { + update = candidate; + return true; + } + } + return false; +} + +bool Src1100Agv::mapUpdateMatches_( + const AgvUnifiedMapUpdate& update, + const AgvMapStreamOptions& options) const +{ + if (!options.map_name.empty() && update.map_id != options.map_name) { + return false; + } + + switch (options.dimension) { + case AgvMapDimension::Map2D: + return update.dimension == AgvMapDimension::Map2D && update.map_2d.has_value(); + case AgvMapDimension::Map3D: + return update.dimension == AgvMapDimension::Map3D && update.map_3d.has_value(); + case AgvMapDimension::Map2DAnd3D: + return (update.dimension == AgvMapDimension::Map2D && update.map_2d.has_value()) + || (update.dimension == AgvMapDimension::Map3D && update.map_3d.has_value()); + case AgvMapDimension::Unspecified: + default: + return update.map_2d.has_value() || update.map_3d.has_value(); + } +} + +AgvResult Src1100Agv::stopMapping() +{ + auto result = ensureOtherSocket_(); + if (!result.ok()) return result; + + Json::Value response; + result = sendCommand_(sock_other_, kRobotOtherStopMapping, Json::Value(Json::objectValue), &response); + return result.ok() ? resultFromResponse_(response) : result; +} + AgvResult Src1100Agv::connectSocket_(int& sock, const int port) { sock = ::socket(AF_INET, SOCK_STREAM, 0); @@ -466,6 +1474,15 @@ AgvResult Src1100Agv::connectSocket_(int& sock, const int port) return AgvResult::success(); } +AgvResult Src1100Agv::ensureOtherSocket_() +{ + std::lock_guard lock(mutex_); + if (sock_other_ >= 0) { + return AgvResult::success(); + } + return connectSocket_(sock_other_, ports_.other); +} + void Src1100Agv::closeSocket_(int& sock) const { if (sock >= 0) { @@ -484,6 +1501,34 @@ AgvResult Src1100Agv::sendCommand_( const std::uint16_t command, const Json::Value& payload, Json::Value* response) const +{ + std::string response_payload; + auto result = sendCommandRaw_(sock, command, payload, &response_payload); + if (!result.ok()) { + return result; + } + if (!response) { + return AgvResult::success(); + } + + Json::Value parsed; + std::string error; + if (!parseJson_(response_payload, parsed, error)) { + const std::string json_text = extractJson_(response_payload); + if (json_text.empty() || !parseJson_(json_text, parsed, error)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, error); + } + } + + *response = std::move(parsed); + return AgvResult::success(); +} + +AgvResult Src1100Agv::sendCommandRaw_( + const int sock, + const std::uint16_t command, + const Json::Value& payload, + std::string* response_payload) const { std::lock_guard lock(mutex_); if (sock < 0) { @@ -496,36 +1541,15 @@ AgvResult Src1100Agv::sendCommand_( return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send command failed: " + systemError()); } - std::string raw; - std::vector buffer(65536); - while (true) { - const ssize_t count = ::recv(sock, buffer.data(), buffer.size(), 0); - if (count <= 0) { - break; - } - raw.append(buffer.data(), static_cast(count)); - if (raw.find('}') != std::string::npos) { - break; - } + std::uint16_t response_command = 0; + std::string payload_text_response; + const auto result = receiveFrame_(sock, response_command, payload_text_response); + if (!result.ok()) { + return result; } - - if (raw.empty()) { - return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 receive timeout or empty response"); - } - - const std::string json_text = extractJson_(raw); - if (json_text.empty()) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 response does not contain JSON"); - } - - Json::Value parsed; - std::string error; - if (!parseJson_(json_text, parsed, error)) { - return AgvResult::failure(AgvErrorCode::CommandFailed, error); - } - - if (response) { - *response = std::move(parsed); + (void)response_command; + if (response_payload) { + *response_payload = std::move(payload_text_response); } return AgvResult::success(); } @@ -538,6 +1562,193 @@ AgvResult Src1100Agv::sendCommandNoResponse_( return sendCommand_(sock, command, payload, nullptr); } +AgvResult Src1100Agv::configurePush_() +{ + if (config_.state_push_included_fields_size() > 0 && config_.state_push_excluded_fields_size() > 0) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SRC1100 push included_fields and excluded_fields cannot both be set"); + } + + Json::Value payload(Json::objectValue); + if (config_.state_push_interval_ms() > 0) { + jsonMember(payload, "interval") = config_.state_push_interval_ms(); + } + appendStringArray(payload, "included_fields", config_.state_push_included_fields()); + appendStringArray(payload, "excluded_fields", config_.state_push_excluded_fields()); + + if (payload.empty()) { + return AgvResult::success(); + } + + const std::string payload_text = toJsonString_(payload); + const auto frame = buildFrame_(kRobotPushConfigReq, payload_text); + + std::lock_guard lock(mutex_); + if (sock_push_ < 0) { + return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 push socket not connected"); + } + if (::send(sock_push_, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast(frame.size())) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send push config failed: " + systemError()); + } + + while (true) { + std::uint16_t command = 0; + std::string response_payload; + const auto result = receiveFrame_(sock_push_, command, response_payload); + if (!result.ok()) { + return result; + } + + Json::Value response; + std::string error; + if (!response_payload.empty() && !parseJson_(response_payload, response, error)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, error); + } + + if (command == kRobotPushConfigRes) { + return resultFromResponse_(response); + } + if (command == kRobotPush && response.isObject()) { + updateCachedRuntimeState_(response); + } + } +} + +void Src1100Agv::startPushThread_() +{ + if (!state_push_enabled_) { + return; + } + if (push_running_.exchange(true)) { + return; + } + if (sock_push_ < 0) { + push_running_ = false; + return; + } + push_thread_ = std::thread(&Src1100Agv::pushLoop_, this); +} + +void Src1100Agv::stopPushThread_() +{ + const bool was_running = push_running_.exchange(false); + if (was_running) { + int sock = -1; + { + std::lock_guard lock(mutex_); + sock = sock_push_; + } + if (sock >= 0) { + ::shutdown(sock, SHUT_RDWR); + } + } + if (push_thread_.joinable()) { + push_thread_.join(); + } +} + +void Src1100Agv::pushLoop_() +{ + while (push_running_) { + int sock = -1; + { + std::lock_guard lock(mutex_); + sock = sock_push_; + } + if (sock < 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + + std::uint16_t command = 0; + std::string payload; + const auto result = receiveFrame_(sock, command, payload); + if (!push_running_) { + break; + } + if (!result.ok()) { + if (result.code != AgvErrorCode::Timeout) { + std::lock_guard lock(mutex_); + last_error_ = result.message; + } + continue; + } + if (command != kRobotPush || payload.empty()) { + continue; + } + + Json::Value parsed; + std::string error; + if (!parseJson_(payload, parsed, error)) { + std::lock_guard lock(mutex_); + last_error_ = error; + continue; + } + updateCachedRuntimeState_(parsed); + } +} + +void Src1100Agv::updateCachedRuntimeState_(const Json::Value& payload) +{ + std::lock_guard lock(runtime_state_mutex_); + auto state = cached_runtime_state_valid_ ? cached_runtime_state_ : AgvRuntimeState{}; + state.timestamp = nowSeconds(); + state.connected = true; + state.last_error.clear(); + + if (jsonHas(payload, "x")) state.pose.x = jsonGet(payload, "x", state.pose.x).asDouble(); + if (jsonHas(payload, "y")) state.pose.y = jsonGet(payload, "y", state.pose.y).asDouble(); + if (jsonHas(payload, "angle")) state.pose.theta = jsonGet(payload, "angle", state.pose.theta).asDouble(); + if (jsonHas(payload, "vx")) state.velocity.vx = jsonGet(payload, "vx", state.velocity.vx).asDouble(); + if (jsonHas(payload, "vy")) state.velocity.vy = jsonGet(payload, "vy", state.velocity.vy).asDouble(); + if (jsonHas(payload, "w")) state.velocity.wz = jsonGet(payload, "w", state.velocity.wz).asDouble(); + if (jsonHas(payload, "battery_level")) { + state.battery.percentage = jsonGet(payload, "battery_level", state.battery.percentage).asDouble(); + } + if (jsonHas(payload, "battery_temp")) { + state.battery.temperature = jsonGet(payload, "battery_temp", state.battery.temperature).asDouble(); + } + if (jsonHas(payload, "charging")) { + state.battery.charging = jsonGet(payload, "charging", state.battery.charging).asBool(); + } + if (jsonHas(payload, "voltage")) { + state.battery.voltage = jsonGet(payload, "voltage", state.battery.voltage).asDouble(); + } + if (jsonHas(payload, "current")) { + state.battery.current = jsonGet(payload, "current", state.battery.current).asDouble(); + } + if (jsonHas(payload, "current_map")) { + state.current_map = jsonGet(payload, "current_map", state.current_map).asString(); + } + if (jsonHas(payload, "current_station")) { + state.current_station = jsonGet(payload, "current_station", state.current_station).asString(); + } + if (jsonHas(payload, "confidence")) { + state.localized = jsonGet(payload, "confidence", 0.0).asDouble() > 0.0; + } + if (jsonHas(payload, "emergency")) { + state.emergency_stopped = jsonGet(payload, "emergency", state.emergency_stopped).asBool(); + } + + state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4; + state.fault = hasFaultArray(payload, "fatals") || hasFaultArray(payload, "errors"); + if (state.emergency_stopped) { + state.mode = AgvMode::EmergencyStop; + } else if (state.fault) { + state.mode = AgvMode::Fault; + } else if (state.battery.charging) { + state.mode = AgvMode::Charging; + } else if (state.moving) { + state.mode = AgvMode::Auto; + } else { + state.mode = AgvMode::Idle; + } + + cached_runtime_state_ = state; + cached_runtime_state_valid_ = true; +} + std::vector Src1100Agv::buildFrame_( const std::uint16_t command, const std::string& payload) @@ -582,6 +1793,61 @@ std::string Src1100Agv::extractJson_(const std::string& raw) return raw.substr(begin, end - begin + 1); } +AgvResult Src1100Agv::receiveFrame_(const int sock, std::uint16_t& command, std::string& payload) +{ + const auto recv_exact = [](const int fd, std::uint8_t* data, const std::size_t size) -> AgvResult { + std::size_t offset = 0; + while (offset < size) { + const ssize_t count = ::recv(fd, data + offset, size - offset, 0); + if (count > 0) { + offset += static_cast(count); + continue; + } + if (count == 0) { + return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 socket closed"); + } + if (errno == EINTR) { + continue; + } + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 receive timeout"); + } + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 receive failed: " + systemError()); + } + return AgvResult::success(); + }; + + std::uint8_t header[16]{}; + auto result = recv_exact(sock, header, sizeof(header)); + if (!result.ok()) { + return result; + } + if (header[0] != 0x5A) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 frame header is invalid"); + } + + const auto length = (static_cast(header[4]) << 24U) + | (static_cast(header[5]) << 16U) + | (static_cast(header[6]) << 8U) + | static_cast(header[7]); + command = static_cast((static_cast(header[8]) << 8U) | header[9]); + payload.clear(); + if (length == 0) { + return AgvResult::success(); + } + if (length > kMaxFramePayloadBytes) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 frame payload is too large"); + } + + std::vector buffer(length); + result = recv_exact(sock, buffer.data(), buffer.size()); + if (!result.ok()) { + return result; + } + payload.assign(reinterpret_cast(buffer.data()), buffer.size()); + return AgvResult::success(); +} + int Src1100Agv::optionalInt_(const AgvAdapterParams& params, const std::string& key, const int fallback) { const auto value = params.getDouble(key); diff --git a/cmvr-es/service/grpc/include/grpc_agv_service.h b/cmvr-es/service/grpc/include/grpc_agv_service.h index 26195986..6ec4e65d 100644 --- a/cmvr-es/service/grpc/include/grpc_agv_service.h +++ b/cmvr-es/service/grpc/include/grpc_agv_service.h @@ -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* writer) override; + grpc::Status stopMapping(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; private: device::DeviceManager& dmgr_; diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp index 200adf6e..1f8d160f 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -1,5 +1,6 @@ #include "service/grpc/include/grpc_agv_service.h" +#include #include #include #include @@ -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_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* writer) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(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::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(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 diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto index a7e7f76b..267c88f3 100644 --- a/protos/cmvr/api/agv_command.proto +++ b/protos/cmvr/api/agv_command.proto @@ -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; + } +} diff --git a/protos/cmvr/api/agv_service.proto b/protos/cmvr/api/agv_service.proto index a460e146..f0bdc5b2 100644 --- a/protos/cmvr/api/agv_service.proto +++ b/protos/cmvr/api/agv_service.proto @@ -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); } diff --git a/protos/cmvr/config/agv_config/agv_config.proto b/protos/cmvr/config/agv_config/agv_config.proto index fdea6bbd..c8ec8c56 100644 --- a/protos/cmvr/config/agv_config/agv_config.proto +++ b/protos/cmvr/config/agv_config/agv_config.proto @@ -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; } diff --git a/protos/cmvr/msgs/agv.proto b/protos/cmvr/msgs/agv.proto index cc5c1dde..54439eeb 100644 --- a/protos/cmvr/msgs/agv.proto +++ b/protos/cmvr/msgs/agv.proto @@ -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 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 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; } diff --git a/protos/rbk/protocol/src1100_map3d.proto b/protos/rbk/protocol/src1100_map3d.proto new file mode 100644 index 00000000..b33474f1 --- /dev/null +++ b/protos/rbk/protocol/src1100_map3d.proto @@ -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; +}