diff --git a/CMakeLists.txt b/CMakeLists.txt index 78131e29..961b77e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,11 +9,29 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) #set(CMAKE_CXX_STANDARD_REQUIRED True) set(CMAKE_POSITION_INDEPENDENT_CODE ON) -# Tests are opt-in so production builds keep the existing footprint. +# Preserve the project's production-build behavior: tests are opt-in via +# -DBUILD_TESTING=ON, while still registering them with CTest when requested. option(BUILD_TESTING "Build the test targets" OFF) include(CTest) - +if(BUILD_TESTING AND UNIX AND NOT APPLE) + # Test executables can still inherit the AUBO imported target's build-tree + # RUNPATH. Keep the active toolchain runtime ahead of that vendor path. + execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so.6 + OUTPUT_VARIABLE CMVR_TEST_SYSTEM_LIBSTDCXX + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + if(EXISTS "${CMVR_TEST_SYSTEM_LIBSTDCXX}") + get_filename_component( + CMVR_TEST_SYSTEM_LIBSTDCXX + "${CMVR_TEST_SYSTEM_LIBSTDCXX}" + REALPATH + ) + else() + unset(CMVR_TEST_SYSTEM_LIBSTDCXX) + endif() +endif() # Install to /output set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE) @@ -32,38 +50,9 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") include(FindExternalLib) set(ARCH "x86") setup_external_libs(${ARCH}) - -# Intel oneVPL / VA-API runtime. The shared libraries in lib/ are installed -# by setup_external_libs(); the VA-API driver plugin directory is installed -# separately because it must retain its dri layout. -set(INTEL_MEDIA_STACK_ROOT - "${PROJECT_SOURCE_DIR}/dependency/${ARCH}/third_party/intel-media-stack/vpl-2.17" -) -set(INTEL_MEDIA_DRIVER_DIR "${INTEL_MEDIA_STACK_ROOT}/lib/dri") -set(INTEL_IHD_DRIVER "${INTEL_MEDIA_DRIVER_DIR}/iHD_drv_video.so") - -if(NOT EXISTS "${INTEL_IHD_DRIVER}") - message(FATAL_ERROR "Intel iHD VA-API driver not found: ${INTEL_IHD_DRIVER}") +if(BUILD_TESTING AND CMVR_EXTERNAL_LIBRARY_DIRS) + list(JOIN CMVR_EXTERNAL_LIBRARY_DIRS ":" CMVR_TEST_EXTERNAL_LIBRARY_PATH) endif() - -message(STATUS "Intel media stack: ${INTEL_MEDIA_STACK_ROOT}") -install( - DIRECTORY "${INTEL_MEDIA_DRIVER_DIR}/" - DESTINATION lib/dri -) -install(CODE [=[ - find_program(CMVR_PATCHELF_EXECUTABLE patchelf REQUIRED) - set(_cmvr_ihd_driver - "${CMAKE_INSTALL_PREFIX}/lib/dri/iHD_drv_video.so" - ) - execute_process( - COMMAND "${CMVR_PATCHELF_EXECUTABLE}" - --set-rpath "$ORIGIN/.." - "${_cmvr_ihd_driver}" - COMMAND_ERROR_IS_FATAL ANY - ) -]=]) - # 在调用 setup_external_libs 之后 message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}") message(STATUS "CMAKE_SHARED_LINKER_FLAGS: ${CMAKE_SHARED_LINKER_FLAGS}") @@ -81,8 +70,9 @@ file(GLOB_RECURSE PROTO_FILES ${PROTO_IMPORT_DIR}/*.proto) set(Protobuf_PROTOC_EXECUTABLE "${CMAKE_INSTALL_PREFIX}/bin/protoc" CACHE FILEPATH "" FORCE) -set_property(TARGET gRPC::grpc_cpp_plugin - PROPERTY IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin" +set_target_properties(gRPC::grpc_cpp_plugin PROPERTIES + IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin" + IMPORTED_LOCATION_RELEASE "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin" ) # 1) 先做 OBJECT：只负责生成/编译 pb.cc @@ -161,17 +151,6 @@ target_link_libraries(cmvr_es PRIVATE ) install(TARGETS cmvr_es RUNTIME DESTINATION bin) - -add_executable(cmvr_config_server - cmvr-es/config_server/config_server_main.cpp - cmvr-es/config_server/config_file_service.cpp -) -target_link_libraries(cmvr_config_server PRIVATE - cmvr_es::proto - protobuf::libprotobuf - gRPC::grpc++ -) -install(TARGETS cmvr_config_server RUNTIME DESTINATION bin) install(CODE [[ file(REMOVE_RECURSE "${CMAKE_INSTALL_PREFIX}/bin/config" diff --git a/README.md b/README.md index 1bcb8565..5913f0fd 100644 --- a/README.md +++ b/README.md @@ -138,3 +138,14 @@ $IGH_ETHERCAT_ROOT/bin/ethercat pdos sudo script/ethercat/stop_ethercat.sh eno1 sudo script/ethercat/stop_ethercat.sh eno1 --restore-network ``` + +## 组件文档 + +具体能力、配置、协议和安全边界由对应代码目录下的 README 维护： + +- [MotorService gRPC 接口](cmvr-es/service/README.md#motorservice) +- [电机设备模块](cmvr-es/devices/motor/README.md) +- [AUBO 控制柜 Standard 数字 IO](cmvr-es/devices/arm/aubo_arm/README.md) +- [配置与部署规则](cmvr-es/config/README.md) + +AUBO JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。 diff --git a/cmake/FindExternalLib.cmake b/cmake/FindExternalLib.cmake index 44a25ea7..3e2a0b2c 100644 --- a/cmake/FindExternalLib.cmake +++ b/cmake/FindExternalLib.cmake @@ -139,6 +139,7 @@ function(setup_external_libs ARCH) list(REMOVE_DUPLICATES LIBRARY_DIRS) link_directories(${LIBRARY_DIRS}) endif() + set(CMVR_EXTERNAL_LIBRARY_DIRS "${LIBRARY_DIRS}" PARENT_SCOPE) # ---- install third-party shared libs into /lib ---- if(INSTALL_SO_FILES) diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index ad988823..1d6f8080 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -10,7 +10,6 @@ add_subdirectory(manager/control_authority_manager) add_subdirectory(manager/safety_manager) add_subdirectory(manager/device_manager) add_subdirectory(service/grpc/stop_all) -add_subdirectory(manager/media_source_hub) add_subdirectory(manager/media_source_manager) add_subdirectory(service/quic_edge) add_subdirectory(task) diff --git a/cmvr-es/common/CMakeLists.txt b/cmvr-es/common/CMakeLists.txt index baee96be..b34192ff 100644 --- a/cmvr-es/common/CMakeLists.txt +++ b/cmvr-es/common/CMakeLists.txt @@ -28,9 +28,3 @@ target_link_libraries(common PUBLIC add_library(cmvr_es::common ALIAS common) install(TARGETS common LIBRARY DESTINATION lib) - -add_executable(support_functions_test - math/support_functions_test.cpp -) -target_include_directories(support_functions_test PRIVATE ${CMAKE_SOURCE_DIR}/cmvr-es) -target_link_libraries(support_functions_test PRIVATE gtest gtest_main glog) diff --git a/cmvr-es/common/README.md b/cmvr-es/common/README.md index cf60bb8d..a8a21010 100644 --- a/cmvr-es/common/README.md +++ b/cmvr-es/common/README.md @@ -54,7 +54,7 @@ AGV 通用类型应参考 [`types/agv/agv_types.h`](types/agv/agv_types.h)，机 - AAC、Opus、PCM 明确 payload format、采样率和声道数； - 不把 QUIC、gRPC 或浏览器专有字段加入通用帧。 -设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceHub 章节。 +设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceManager 章节。 ## 环形队列选择 diff --git a/cmvr-es/common/base/logging/logger.cpp b/cmvr-es/common/base/logging/logger.cpp index 0a6355ee..594b95c1 100644 --- a/cmvr-es/common/base/logging/logger.cpp +++ b/cmvr-es/common/base/logging/logger.cpp @@ -167,6 +167,15 @@ void Logger::shutdown() initialized_ = false; } +void Logger::flush() +{ + std::lock_guard lock(mutex_); + if (log_file_.is_open()) { + log_file_.flush(); + last_flush_ = std::chrono::steady_clock::now(); + } +} + bool Logger::enabled(const Level level) const { std::lock_guard lock(mutex_); @@ -200,7 +209,8 @@ void Logger::write(const Level level, rotateIfNeeded_(); log_file_ << line << '\n'; const auto now = std::chrono::steady_clock::now(); - if (level == Level::ERROR || level == Level::FATAL || now - last_flush_ >= flush_interval_) { + if (level == Level::ERROR || level == Level::FATAL || + now - last_flush_ >= flush_interval_) { log_file_.flush(); last_flush_ = now; } diff --git a/cmvr-es/common/base/logging/logger.h b/cmvr-es/common/base/logging/logger.h index a7ee3a74..d6bd17c5 100644 --- a/cmvr-es/common/base/logging/logger.h +++ b/cmvr-es/common/base/logging/logger.h @@ -43,6 +43,7 @@ public: const std::string& application_name, const std::filesystem::path& executable_directory); void shutdown(); + void flush(); bool enabled(Level level) const; void write(Level level, const char* source_file, int source_line, const std::string& message); diff --git a/cmvr-es/devices/agv/src1100/CMakeLists.txt b/cmvr-es/devices/agv/src1100/CMakeLists.txt deleted file mode 100644 index 6ad1f3fe..00000000 --- a/cmvr-es/devices/agv/src1100/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -add_library(src1100_agv SHARED src/src1100_agv.cpp) - -target_include_directories(src1100_agv PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - -target_link_libraries(src1100_agv - PUBLIC - cmvr_es::proto - jsoncpp -) - -add_library(cmvr_es::device::src1100_agv ALIAS src1100_agv) -install(TARGETS src1100_agv LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h deleted file mode 100644 index ab5bcb92..00000000 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ /dev/null @@ -1,179 +0,0 @@ -#ifndef CMVR_ES_SRC1100_AGV_H -#define CMVR_ES_SRC1100_AGV_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "cmvr/config/agv_config/agv_config.pb.h" -#include "devices/agv/abstract_agv.h" - -namespace cmvr::device { - -class Src1100Agv final : public AbstractAGV { -public: - explicit Src1100Agv(const config::Src1100AgvConfig& cfg); - ~Src1100Agv() override; - - std::string typeName() const override { return "Src1100Agv"; } - - bool init() override; - bool start() override; - bool stop() override; - bool update() override; - - AgvRuntimeState runtimeState() const override; - AgvNavigationStatus navigationStatus() const override; - - AgvResult emergencyStop() override; - AgvResult clearFault() override; - - AgvResult navigateToPose( - const math::Pose2d& pose, - const AgvMotionOptions& options = {}, - const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override; - AgvResult navigateToStation( - const std::string& station_id, - const AgvMotionOptions& options = {}, - const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override; - AgvResult followPath(const std::vector& path) override; - AgvResult pauseNavigation() override; - AgvResult resumeNavigation() override; - AgvResult cancelNavigation() override; - - AgvResult setVelocity(const AgvVelocity& velocity) override; - - AgvResult listMaps(std::vector& maps) const override; - AgvResult listStations(std::vector& stations) const override; - 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 { - int status{19204}; - int control{19205}; - int navigation{19206}; - int config{19207}; - int other{19210}; - int push{19301}; - }; - - AgvResult connect_(); - AgvResult disconnect_(); - AgvResult connectSocket_(int& sock, int port); - AgvResult ensureOtherSocket_(); - void closeSocket_(int& sock) const; - bool connected_() const; - - AgvResult sendCommand_(int sock, - 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); - static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params); - static AgvResult resultFromResponse_(const Json::Value& response); - - config::Src1100AgvConfig config_; - 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}; - int sock_control_{-1}; - int sock_navigation_{-1}; - int sock_config_{-1}; - 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 - -#endif // CMVR_ES_SRC1100_AGV_H diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp deleted file mode 100644 index 7c1b1b2b..00000000 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ /dev/null @@ -1,1898 +0,0 @@ -#include "devices/agv/src1100/include/src1100_agv.h" - -#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 { - -namespace { - -constexpr std::uint16_t kRobotStatusLoc = 1004; -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; -constexpr std::uint16_t kRobotTaskPause = 3001; -constexpr std::uint16_t kRobotTaskResume = 3002; -constexpr std::uint16_t kRobotTaskCancel = 3003; -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)); -} - -Json::Value& jsonMember(Json::Value& value, const std::string& key) -{ - return *value.demand(key.data(), key.data() + key.size()); -} - -const Json::Value* jsonFind(const Json::Value& value, const char* key) -{ - return value.find(key, key + std::strlen(key)); -} - -Json::Value jsonGet(const Json::Value& value, const char* key, const Json::Value& fallback) -{ - const auto* found = jsonFind(value, key); - 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) { - case 2: - return AgvMode::Auto; - case 3: - return AgvMode::Paused; - case 5: - return AgvMode::Fault; - case 6: - return AgvMode::Stopped; - default: - return AgvMode::Idle; - } -} - -AgvTaskState toTaskState(const int value) -{ - switch (value) { - case 1: - return AgvTaskState::Waiting; - case 2: - return AgvTaskState::Running; - case 3: - return AgvTaskState::Paused; - case 4: - return AgvTaskState::Completed; - case 5: - return AgvTaskState::Failed; - case 6: - return AgvTaskState::Canceled; - case 0: - default: - return AgvTaskState::None; - } -} - -AgvTaskType toTaskType(const int value) -{ - switch (value) { - case 1: - return AgvTaskType::NavigateToPose; - case 2: - return AgvTaskType::NavigateToStation; - case 3: - return AgvTaskType::FollowPath; - case 100: - return AgvTaskType::Custom; - default: - return AgvTaskType::None; - } -} - -} // namespace - -Src1100Agv::Src1100Agv(const config::Src1100AgvConfig& cfg) - : config_(cfg), - ip_(cfg.ip()), - 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(); - if (cfg.port_control() > 0) ports_.control = cfg.port_control(); - if (cfg.port_nav() > 0) ports_.navigation = cfg.port_nav(); - if (cfg.port_config() > 0) ports_.config = cfg.port_config(); - if (cfg.port_other() > 0) ports_.other = cfg.port_other(); - if (cfg.port_push() > 0) ports_.push = cfg.port_push(); - - const auto result = connect_(); - if (!result.ok()) { - CMVR_LOG(ERROR) << "[Src1100Agv] Auto connect failed" - << ", id=" << id_ - << ", ip=" << ip_ - << ", error=" << result.message; - } -} - -Src1100Agv::~Src1100Agv() -{ - (void)disconnect_(); -} - -bool Src1100Agv::init() -{ - return !id_.empty() && !ip_.empty(); -} - -bool Src1100Agv::start() -{ - return true; -} - -bool Src1100Agv::stop() -{ - return true; -} - -bool Src1100Agv::update() -{ - return true; -} - -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_(); - state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected; - state.last_error = last_error_; - - Json::Value loc; - if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) { - state.pose.x = jsonGet(loc, "x", 0.0).asDouble(); - state.pose.y = jsonGet(loc, "y", 0.0).asDouble(); - state.pose.theta = jsonGet(loc, "angle", 0.0).asDouble(); - state.localized = jsonGet(loc, "confidence", 0.0).asDouble() > 0.0; - state.current_station = jsonGet(loc, "current_station", "").asString(); - } - - Json::Value battery; - if (sendCommand_(sock_status_, kRobotStatusBattery, Json::Value(Json::objectValue), &battery).ok()) { - state.battery.percentage = jsonGet(battery, "battery_level", 0.0).asDouble(); - state.battery.temperature = jsonGet(battery, "battery_temp", 0.0).asDouble(); - state.battery.charging = jsonGet(battery, "charging", false).asBool(); - state.battery.voltage = jsonGet(battery, "voltage", 0.0).asDouble(); - state.battery.current = jsonGet(battery, "current", 0.0).asDouble(); - } - - Json::Value map; - if (sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &map).ok()) { - state.current_map = jsonGet(map, "current_map", "").asString(); - } - - const auto nav = navigationStatus(); - state.moving = nav.state == AgvTaskState::Running; - state.fault = nav.state == AgvTaskState::Failed; - state.mode = state.fault ? AgvMode::Fault : modeFromTaskState(static_cast(nav.state)); - return state; -} - -AgvNavigationStatus Src1100Agv::navigationStatus() const -{ - AgvNavigationStatus status; - Json::Value payload(Json::objectValue); - jsonMember(payload, "simple") = false; - - Json::Value response; - const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response); - if (!result.ok()) { - status.state = AgvTaskState::Failed; - status.message = result.message; - return status; - } - - status.state = toTaskState(jsonGet(response, "task_status", 0).asInt()); - status.type = toTaskType(jsonGet(response, "task_type", 0).asInt()); - status.message = jsonGet(response, "move_status_info", jsonGet(response, "err_msg", "")).asString(); - if (const auto* task_status_package = jsonFind(response, "task_status_package")) { - status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble(); - } - return status; -} - -AgvResult Src1100Agv::connect_() -{ - stopPushThread_(); - stopMapUpdateThread_(); - - { - std::lock_guard lock(mutex_); - closeSocket_(sock_status_); - closeSocket_(sock_control_); - closeSocket_(sock_navigation_); - closeSocket_(sock_config_); - closeSocket_(sock_other_); - closeSocket_(sock_push_); - - 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 (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 (map_update_enabled_) { - startMapUpdateThread_(); - } - return AgvResult::success(); -} - -AgvResult Src1100Agv::disconnect_() -{ - stopMapUpdateThread_(); - stopPushThread_(); - std::lock_guard lock(mutex_); - closeSocket_(sock_status_); - closeSocket_(sock_control_); - closeSocket_(sock_navigation_); - closeSocket_(sock_config_); - closeSocket_(sock_other_); - closeSocket_(sock_push_); - return AgvResult::success(); -} - -AgvResult Src1100Agv::emergencyStop() -{ - return cancelNavigation(); -} - -AgvResult Src1100Agv::clearFault() -{ - return AgvResult::success(); -} - -AgvResult Src1100Agv::navigateToPose( - const math::Pose2d& pose, - const AgvMotionOptions& options, - const AgvAdapterParams& adapter_params) -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); - jsonMember(payload, "id") = adapter_params.getString("target_id").value_or(""); - jsonMember(payload, "skill_name") = adapter_params.getString("skill_name").value_or("GotoSpecifiedPose"); - auto& free_go = jsonMember(payload, "freeGo"); - jsonMember(free_go, "x") = pose.x; - jsonMember(free_go, "y") = pose.y; - jsonMember(free_go, "theta") = pose.theta; - applyMotionOptions_(payload, options); - applyAdapterParams_(payload, adapter_params); - Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult Src1100Agv::navigateToStation( - const std::string& station_id, - const AgvMotionOptions& options, - const AgvAdapterParams& adapter_params) -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); - jsonMember(payload, "id") = station_id; - applyMotionOptions_(payload, options); - applyAdapterParams_(payload, adapter_params); - Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult Src1100Agv::followPath(const std::vector& path) -{ - Json::Value payload(Json::objectValue); - Json::Value tasks(Json::arrayValue); - int index = 0; - for (const auto& segment : path) { - Json::Value task(Json::objectValue); - jsonMember(task, "task_id") = id_ + "_path_" + std::to_string(index++); - jsonMember(task, "source_id") = segment.source_station; - jsonMember(task, "id") = segment.target_station; - tasks.append(task); - } - jsonMember(payload, "move_task_list") = tasks; - Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult Src1100Agv::pauseNavigation() -{ - Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskPause, Json::Value(Json::objectValue), &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult Src1100Agv::resumeNavigation() -{ - Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskResume, Json::Value(Json::objectValue), &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult Src1100Agv::cancelNavigation() -{ - Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskCancel, Json::Value(Json::objectValue), &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "vx") = velocity.vx; - jsonMember(payload, "vy") = velocity.vy; - jsonMember(payload, "w") = velocity.wz; - jsonMember(payload, "duration") = -1; - Json::Value response; - auto result = sendCommand_(sock_control_, kRobotControlMotion, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult Src1100Agv::listMaps(std::vector& maps) const -{ - Json::Value response; - auto result = sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &response); - if (!result.ok()) return result; - maps.clear(); - if (const auto* values = jsonFind(response, "maps"); values && values->isArray()) { - for (const auto& value : *values) { - maps.push_back(value.asString()); - } - } - return resultFromResponse_(response); -} - -AgvResult Src1100Agv::listStations(std::vector& stations) const -{ - Json::Value response; - auto result = sendCommand_(sock_status_, kRobotStatusStation, Json::Value(Json::objectValue), &response); - if (!result.ok()) return result; - stations.clear(); - if (const auto* values = jsonFind(response, "stations"); values && values->isArray()) { - for (const auto& value : *values) { - AgvStation station; - station.id = jsonGet(value, "id", "").asString(); - station.type = jsonGet(value, "type", "").asString(); - station.pose.x = jsonGet(value, "x", 0.0).asDouble(); - station.pose.y = jsonGet(value, "y", 0.0).asDouble(); - station.pose.theta = jsonGet(value, "r", 0.0).asDouble(); - station.description = jsonGet(value, "desc", "").asString(); - stations.push_back(station); - } - } - return resultFromResponse_(response); -} - -AgvResult Src1100Agv::switchMap(const std::string& map_name) -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "map_name") = map_name; - Json::Value response; - auto result = sendCommand_(sock_control_, kRobotControlLoadMap, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult Src1100Agv::uploadMap(const std::string& map_name, const std::string& content) -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "map_name") = map_name; - jsonMember(payload, "map_content") = content; - Json::Value response; - auto result = sendCommand_(sock_config_, kRobotConfigUploadMap, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult Src1100Agv::downloadMap(const std::string& map_name, std::string& content) const -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "map_name") = map_name; - Json::Value response; - auto result = sendCommand_(sock_config_, kRobotConfigDownloadMap, payload, &response); - if (!result.ok()) return result; - content = jsonGet(response, "map_content", jsonGet(response, "content", "")).asString(); - 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); - if (sock < 0) { - last_error_ = "create socket failed: " + systemError(); - return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_); - } - - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_port = htons(static_cast(port)); - if (::inet_pton(AF_INET, ip_.c_str(), &address.sin_addr) <= 0) { - closeSocket_(sock); - last_error_ = "invalid SRC1100 ip: " + ip_; - return AgvResult::failure(AgvErrorCode::InvalidArgument, last_error_); - } - - if (::connect(sock, reinterpret_cast(&address), sizeof(address)) < 0) { - closeSocket_(sock); - last_error_ = "connect SRC1100 port " + std::to_string(port) + " failed: " + systemError(); - return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_); - } - - timeval timeout{}; - timeout.tv_sec = recv_timeout_ms_ / 1000; - timeout.tv_usec = (recv_timeout_ms_ % 1000) * 1000; - ::setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); - 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) { - ::close(sock); - sock = -1; - } -} - -bool Src1100Agv::connected_() const -{ - return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0; -} - -AgvResult Src1100Agv::sendCommand_( - const int sock, - 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) { - return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 socket not connected"); - } - - const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload); - const auto frame = buildFrame_(command, payload_text); - if (::send(sock, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast(frame.size())) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send command failed: " + systemError()); - } - - 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; - } - (void)response_command; - if (response_payload) { - *response_payload = std::move(payload_text_response); - } - return AgvResult::success(); -} - -AgvResult Src1100Agv::sendCommandNoResponse_( - const int sock, - const std::uint16_t command, - const Json::Value& payload) const -{ - 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) -{ - std::vector frame(16 + payload.size(), 0); - frame[0] = 0x5A; - frame[1] = 0x01; - frame[2] = 0x00; - frame[3] = 0x01; - const auto length = static_cast(payload.size()); - frame[4] = static_cast((length >> 24U) & 0xFFU); - frame[5] = static_cast((length >> 16U) & 0xFFU); - frame[6] = static_cast((length >> 8U) & 0xFFU); - frame[7] = static_cast(length & 0xFFU); - frame[8] = static_cast((command >> 8U) & 0xFFU); - frame[9] = static_cast(command & 0xFFU); - std::copy(payload.begin(), payload.end(), frame.begin() + 16); - return frame; -} - -std::string Src1100Agv::toJsonString_(const Json::Value& value) -{ - Json::StreamWriterBuilder builder; - builder["indentation"] = ""; - return Json::writeString(builder, value); -} - -bool Src1100Agv::parseJson_(const std::string& input, Json::Value& output, std::string& error) -{ - Json::CharReaderBuilder builder; - std::unique_ptr reader(builder.newCharReader()); - return reader->parse(input.data(), input.data() + input.size(), &output, &error); -} - -std::string Src1100Agv::extractJson_(const std::string& raw) -{ - const auto begin = raw.find('{'); - const auto end = raw.rfind('}'); - if (begin == std::string::npos || end == std::string::npos || end < begin) { - return {}; - } - 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); - return value ? static_cast(*value) : fallback; -} - -double Src1100Agv::optionalDouble_(const AgvAdapterParams& params, const std::string& key, const double fallback) -{ - const auto value = params.getDouble(key); - return value ? *value : fallback; -} - -void Src1100Agv::applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options) -{ - if (options.max_speed > 0.0) jsonMember(payload, "max_speed") = options.max_speed; - if (options.max_angular_speed > 0.0) jsonMember(payload, "max_wspeed") = options.max_angular_speed; - if (options.max_acceleration > 0.0) jsonMember(payload, "max_acc") = options.max_acceleration; - if (options.max_angular_acceleration > 0.0) jsonMember(payload, "max_wacc") = options.max_angular_acceleration; - if (options.reach_distance > 0.0) jsonMember(payload, "reach_dist") = options.reach_distance; - if (options.reach_angle > 0.0) jsonMember(payload, "reach_angle") = options.reach_angle; -} - -void Src1100Agv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params) -{ - for (const auto& [key, value] : params.values) { - if (key.rfind("port_", 0) == 0) { - continue; - } - jsonMember(payload, key) = value; - } - jsonMember(payload, "jack_height") = optionalDouble_( - params, - "jack_height", - jsonGet(payload, "jack_height", 0.0).asDouble()); -} - -AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response) -{ - const int ret_code = jsonGet(response, "ret_code", 0).asInt(); - const std::string message = jsonGet(response, "err_msg", "").asString(); - if (ret_code == 0) { - return AgvResult::success(); - } - return AgvResult::failure(AgvErrorCode::CommandFailed, - message.empty() ? "SRC1100 command failed: " + std::to_string(ret_code) : message); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/biohead/abstract_biohead.h b/cmvr-es/devices/biohead/abstract_biohead.h index aaf28658..26ac0d35 100644 --- a/cmvr-es/devices/biohead/abstract_biohead.h +++ b/cmvr-es/devices/biohead/abstract_biohead.h @@ -1,6 +1,11 @@ #ifndef ABSTRACT_BIOHEAD_H #define ABSTRACT_BIOHEAD_H #pragma once + +#include +#include +#include + #include "../abstract_device.h" namespace cmvr::device { @@ -58,6 +63,8 @@ namespace cmvr::device { // 抽象头部类 class AbstractBiohead : public AbstractDevice { public: + using OperationalToken = std::uint64_t; + AbstractBiohead() = default; ~AbstractBiohead() override = default; @@ -76,20 +83,140 @@ namespace cmvr::device { virtual void expressionSadness() {}; virtual void expressionYawn() {}; + // Capture under the process-wide StopAll admission gate. Commands + // from an older generation are rejected after operational stop. + OperationalToken beginOperationalActivity() const noexcept + { + std::lock_guard lock(operational_mutex_); + return operational_generation_; + } + + virtual bool setExpressionPoseIfCurrent( + OperationalToken token, + FacialExpressionState& expression_state, + double vel = 0.5, + double acc = 0.1) + { + return runIfOperationalActivityCurrent_(token, [&] { + setExpressionPose(expression_state, vel, acc); + }); + } + + virtual bool streamFacialPoseIfCurrent( + OperationalToken token, + FacialExpressionState& expression_state, + double vel, + double acc) + { + return runIfOperationalActivityCurrent_(token, [&] { + streamFacialPose(expression_state, vel, acc); + }); + } + + virtual bool speakStartIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + speakstart(); + }); + } + + virtual bool expressionHappyIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionHappy(); + }); + } + + virtual bool expressionSurprisedIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionSurprised(); + }); + } + + virtual bool expressionTiredIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionTired(); + }); + } + + virtual bool expressionAngryIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionAngry(); + }); + } + + virtual bool expressionSadnessIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionSadness(); + }); + } + + virtual bool expressionYawnIfCurrent(OperationalToken token) + { + return runIfOperationalActivityCurrent_(token, [&] { + expressionYawn(); + }); + } + + // Stops expression motion and speaking without closing the device. + // True confirms that old activity was fenced and the hold completed. + virtual bool stopOperationalActivity() + { + invalidateOperationalActivities_(); + speakstop(); + (void)runOperationalStop_([&] { eStop(); }); + return false; + } + FacialExpressionState expression_state_; - std::atomic emergency_stop_requested = false; - + protected: + template + bool runIfOperationalActivityCurrent_( + const OperationalToken token, + Operation&& operation) + { + std::lock_guard lock(operational_mutex_); + if (token == 0U || token != operational_generation_) { + return false; + } + std::forward(operation)(); + return true; + } + template + bool runOperationalStop_(Operation&& operation) + { + std::lock_guard lock(operational_mutex_); + std::forward(operation)(); + return true; + } + bool operationalActivityCurrent_( + const OperationalToken token) const noexcept + { + std::lock_guard lock(operational_mutex_); + return token != 0U && token == operational_generation_; + } + void invalidateOperationalActivities_() noexcept + { + std::lock_guard lock(operational_mutex_); + ++operational_generation_; + if (operational_generation_ == 0U) { + ++operational_generation_; + } + } + private: + mutable std::mutex operational_mutex_; + OperationalToken operational_generation_{1U}; }; } // namespace cmvr::device #endif // ABSTRACT_BIOHEAD_H - - - - diff --git a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h index 61d301ba..dd0d31a1 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h +++ b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h @@ -4,10 +4,12 @@ #include "../../abstract_biohead.h" #include "../../../../hardware/include/esp32_serial_port.h" #include "cmvr/config/biohead_config/biohead_config.pb.h" +#include #include #include #include #include +#include namespace cmvr::device { @@ -19,7 +21,7 @@ namespace cmvr::device { class BioHeadRobot : public AbstractBiohead { public: explicit BioHeadRobot(const config::BioHeadRobotConfig &config); - ~BioHeadRobot() override = default; + ~BioHeadRobot() override; std::string typeName() const override { return "BioHeadRobot"; } bool init() override; @@ -30,7 +32,25 @@ namespace cmvr::device { void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) override; void speakstart() override; void speakstop() override; - void speakthread(); + bool stopOperationalActivity() override; + + bool setExpressionPoseIfCurrent( + OperationalToken token, + FacialExpressionState& expression_state, + double vel = 0.5, + double acc = 0.1) override; + bool streamFacialPoseIfCurrent( + OperationalToken token, + FacialExpressionState& expression_state, + double vel, + double acc) override; + bool speakStartIfCurrent(OperationalToken token) override; + bool expressionHappyIfCurrent(OperationalToken token) override; + bool expressionSurprisedIfCurrent(OperationalToken token) override; + bool expressionTiredIfCurrent(OperationalToken token) override; + bool expressionAngryIfCurrent(OperationalToken token) override; + bool expressionSadnessIfCurrent(OperationalToken token) override; + bool expressionYawnIfCurrent(OperationalToken token) override; void expressionHappy()override; void expressionSurprised()override; @@ -43,11 +63,23 @@ namespace cmvr::device { private: // 内部方法 void parseConfig(const config::BioHeadRobotConfig &config); - void sendServoCommands( const std::vector& targets, uint16_t duration_ms); + bool sendServoCommands( + const std::vector& targets, + uint16_t duration_ms, + bool force = false); + bool sendRawIfCurrent( + OperationalToken token, + const std::vector& raw_data); uint16_t angleToRaw(double angle); double normalizeToAngle(double normalized, size_t index); - void sendExpression(const std::vector& device_64_angles, const std::vector& device_65_angles, int step_ms); + bool sendExpression( + OperationalToken token, + const std::vector& device_64_angles, + const std::vector& device_65_angles, + int step_ms); + bool startSpeaking(OperationalToken token); + void speakthread(OperationalToken token); @@ -69,8 +101,9 @@ namespace cmvr::device { std::shared_ptr speak_thread_; std::atomic speak_running_{false}; - - + std::mutex speak_mutex_; + std::mutex expression_wait_mutex_; + std::condition_variable expression_wait_cv_; }; diff --git a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp index 5e86e722..7fd504a6 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp @@ -21,6 +21,11 @@ BioHeadRobot::BioHeadRobot(const config::BioHeadRobotConfig &config) { } +BioHeadRobot::~BioHeadRobot() +{ + speakstop(); +} + bool BioHeadRobot::init() { @@ -112,13 +117,36 @@ double BioHeadRobot::normalizeToAngle(double normalized, size_t index) { } void BioHeadRobot::getState(RobotState &state) { + std::lock_guard lock(stateMutex_); state.error = false; state.joint_positions = current_joints_; } void BioHeadRobot::eStop() { CMVR_LOG(WARNING) << "[BioHeadRobot] Emergency stop: hold current joint positions."; - sendServoCommands(current_joints_, 100); // 快速下发当前角度 + (void)sendServoCommands(last_joints_, 0, true); +} + +bool BioHeadRobot::setExpressionPoseIfCurrent( + const OperationalToken token, + FacialExpressionState& expression_state, + const double vel, + const double acc) +{ + return runIfOperationalActivityCurrent_(token, [&] { + setExpressionPose(expression_state, vel, acc); + }); +} + +bool BioHeadRobot::streamFacialPoseIfCurrent( + const OperationalToken token, + FacialExpressionState& expression_state, + const double vel, + const double acc) +{ + return runIfOperationalActivityCurrent_(token, [&] { + streamFacialPose(expression_state, vel, acc); + }); } void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, double vel, double acc) { @@ -160,8 +188,10 @@ void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, do for (size_t i = 0; i < joints.size(); ++i) { CMVR_LOG(INFO) << "Joint[" << i << "] = " << joints[i]; // 打印每个关节的角度 } - uint16_t duration = static_cast(1000.0 / vel); - sendServoCommands(joints, duration); + const uint16_t duration = vel > 0.0 + ? static_cast(1000.0 / vel) + : 0U; + (void)sendServoCommands(joints, duration); } @@ -208,17 +238,30 @@ void BioHeadRobot::streamFacialPose(FacialExpressionState& expression_state, dou CMVR_LOG(INFO) << "嘴角3=： " << ": " << joints[15]; CMVR_LOG(INFO) << "嘴角4=： " << ": " << joints[16]; - uint16_t duration = static_cast(1000.0 / vel); - sendServoCommands(joints, duration); + const uint16_t duration = vel > 0.0 + ? static_cast(1000.0 / vel) + : 0U; + (void)sendServoCommands(joints, duration); } void BioHeadRobot::speakstart() { + (void)speakStartIfCurrent(beginOperationalActivity()); +} + +bool BioHeadRobot::speakStartIfCurrent(const OperationalToken token) +{ + return startSpeaking(token); +} + +bool BioHeadRobot::startSpeaking(const OperationalToken token) +{ + std::lock_guard lock(speak_mutex_); if (speak_running_.load()) { CMVR_LOG(INFO) << "[BioHeadRobot] speak thread already running."; - return; + return operationalActivityCurrent_(token); } // 检查 channels 中是否有 65:8 和 65:9 @@ -229,12 +272,9 @@ void BioHeadRobot::speakstart() { } if (!found8 || !found9) { CMVR_LOG(ERROR) << "[BioHeadRobot] Required servo channels not found (addr 65 ch 8/9). speakstart aborted."; - return; + return false; } - // 启动线程 - speak_running_.store(true); - // 清理旧线程（若有） if (speak_thread_ && speak_thread_->joinable()) { try { @@ -245,19 +285,24 @@ void BioHeadRobot::speakstart() { speak_thread_.reset(); } - speak_thread_ = std::make_shared(&BioHeadRobot::speakthread, this); + bool started = false; + const bool current = runIfOperationalActivityCurrent_(token, [&] { + speak_running_.store(true, std::memory_order_release); + speak_thread_ = std::make_shared( + &BioHeadRobot::speakthread, this, token); + started = true; + }); + if (!current || !started) { + speak_running_.store(false, std::memory_order_release); + return false; + } CMVR_LOG(INFO) << "[BioHeadRobot] speak thread started."; + return true; } void BioHeadRobot::speakstop() { - { - if (!speak_running_.load()) { - CMVR_LOG(INFO) << "[BioHeadRobot] speak thread not running."; - return; - } - speak_running_.store(false); - } - // 唤醒线程（如果在 wait 中） + std::lock_guard lock(speak_mutex_); + speak_running_.store(false, std::memory_order_release); // join 并清理线程对象 if (speak_thread_) { @@ -275,7 +320,20 @@ void BioHeadRobot::speakstop() { CMVR_LOG(INFO) << "[BioHeadRobot] speak thread stopped."; } -void BioHeadRobot::speakthread() { +bool BioHeadRobot::stopOperationalActivity() +{ + invalidateOperationalActivities_(); + expression_wait_cv_.notify_all(); + speakstop(); + + bool hold_confirmed = false; + (void)runOperationalStop_([&] { + hold_confirmed = sendServoCommands(last_joints_, 0, true); + }); + return hold_confirmed; +} + +void BioHeadRobot::speakthread(const OperationalToken token) { CMVR_LOG(INFO) << "[BioHeadRobot] speakthread running."; // 固定参数 @@ -313,7 +371,11 @@ void BioHeadRobot::speakthread() { } // 以当前角度为基准 - std::vector base = current_joints_; + std::vector base; + { + std::lock_guard lock(stateMutex_); + base = current_joints_; + } if (base.size() != channels_.size()) { base.resize(channels_.size(), 90.0); } @@ -346,7 +408,8 @@ void BioHeadRobot::speakthread() { double current_random_factor = 0.0; const double random_update_interval = 0.2; // 每0.2秒更新一次随机扰动 - while (speak_running_.load()) { + while (speak_running_.load(std::memory_order_acquire) && + operationalActivityCurrent_(token)) { auto now = std::chrono::steady_clock::now(); double t = std::chrono::duration_cast>(now - start).count(); @@ -452,7 +515,9 @@ void BioHeadRobot::speakthread() { } // 下发 - serial_->sendRawServoData(raw_data); + if (!sendRawIfCurrent(token, raw_data)) { + break; + } // 控制循环频率 std::this_thread::sleep_for(std::chrono::milliseconds(step_ms)); @@ -483,12 +548,23 @@ void BioHeadRobot::speakthread() { } } - serial_->sendRawServoData(restore_data); - CMVR_LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose."; + if (sendRawIfCurrent(token, restore_data)) { + CMVR_LOG(INFO) + << "[BioHeadRobot] speakthread exiting and restored base pose."; + } else { + CMVR_LOG(INFO) + << "[BioHeadRobot] speakthread stopped without a stale restore."; + } + speak_running_.store(false, std::memory_order_release); } -void BioHeadRobot::sendExpression(const std::vector& device_64_angles, const std::vector& device_65_angles, int step_ms) { +bool BioHeadRobot::sendExpression( + const OperationalToken token, + const std::vector& device_64_angles, + const std::vector& device_65_angles, + const int step_ms) +{ std::vector raw_data; // 处理设备64角度 @@ -511,8 +587,20 @@ void BioHeadRobot::sendExpression(const std::vector& device_64_angles, c raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节 } - serial_->sendRawServoData(raw_data); - std::this_thread::sleep_for(std::chrono::seconds(5)); + if (!sendRawIfCurrent(token, raw_data)) { + return false; + } + { + std::unique_lock lock(expression_wait_mutex_); + if (expression_wait_cv_.wait_for( + lock, + std::chrono::seconds(5), + [this, token] { + return !operationalActivityCurrent_(token); + })) { + return false; + } + } // 恢复到原始角度 // 设备64角度（10通道） @@ -542,50 +630,78 @@ void BioHeadRobot::sendExpression(const std::vector& device_64_angles, c raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 } - serial_->sendRawServoData(raw_data_neutral); + return sendRawIfCurrent(token, raw_data_neutral); } //高兴 void BioHeadRobot::expressionHappy() { + (void)expressionHappyIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionHappyIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 90, 90, 90, 80, 125, 100, 60, 90, 90}; const std::vector device_65_angles = {100, 80, 125, 135, 100, 105, 110, 90, 90, 90}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } //惊讶 void BioHeadRobot::expressionSurprised() { + (void)expressionSurprisedIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionSurprisedIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 100, 100, 70, 20, 140, 130, 50, 90, 90}; const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 70, 110}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } //睡觉 void BioHeadRobot::expressionTired() { + (void)expressionTiredIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionTiredIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90}; const std::vector device_65_angles = {90, 90, 90, 90, 90, 105, 110, 90, 85, 95}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } //愤怒 void BioHeadRobot::expressionAngry() { + (void)expressionAngryIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionAngryIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 70, 90, 110, 70, 125, 110, 80, 70, 90}; const std::vector device_65_angles = {100, 80, 130, 130, 70, 55, 50, 125, 90, 90}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } //悲伤 void BioHeadRobot::expressionSadness() { + (void)expressionSadnessIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionSadnessIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 70, 90, 110, 70, 125, 110, 80, 90, 90}; const std::vector device_65_angles = {100, 80, 130, 130, 70, 55, 50, 125, 90, 90}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } //打哈欠 void BioHeadRobot::expressionYawn() { + (void)expressionYawnIfCurrent(beginOperationalActivity()); +} +bool BioHeadRobot::expressionYawnIfCurrent(const OperationalToken token) { const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90}; const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90}; - sendExpression(device_64_angles, device_65_angles, 0); + return sendExpression(token, device_64_angles, device_65_angles, 0); } -void BioHeadRobot::sendServoCommands(const std::vector& targets, uint16_t duration_ms) { +bool BioHeadRobot::sendServoCommands( + const std::vector& targets, + const uint16_t duration_ms, + const bool force) +{ + if (!serial_ || targets.size() != channels_.size() || + targets.size() != min_angles_.size() || + targets.size() != max_angles_.size() || + targets.size() != last_joints_.size()) { + return false; + } std::vector addrs, chs; std::vector raws; @@ -598,17 +714,14 @@ void BioHeadRobot::sendServoCommands(const std::vector& targets, uint16_ continue; } - // 更新 last_joints_，只有当角度变化较大时才更新 - last_joints_[i] = tgt; - // 准备打包数据 addrs.push_back(channels_[i].addr); chs.push_back(channels_[i].channel); raws.push_back(angleToRaw(tgt)); } // 2. 如果没有任何通道需要更新，就直接返回 - if (raws.empty()) { - return; + if (raws.empty() && !force) { + return true; } std::vector raw_data; // 原始格式处理 @@ -640,7 +753,41 @@ void BioHeadRobot::sendServoCommands(const std::vector& targets, uint16_ - serial_->sendRawServoData(raw_data); + const bool sent = serial_->sendRawServoData(raw_data); + if (sent) { + for (std::size_t i = 0; i < targets.size(); ++i) { + last_joints_[i] = + std::clamp(targets[i], min_angles_[i], max_angles_[i]); + } + std::lock_guard lock(stateMutex_); + current_joints_ = last_joints_; + } + return sent; +} + +bool BioHeadRobot::sendRawIfCurrent( + const OperationalToken token, + const std::vector& raw_data) +{ + bool sent = false; + const bool current = runIfOperationalActivityCurrent_(token, [&] { + sent = serial_ && serial_->sendRawServoData(raw_data); + if (!sent || raw_data.size() % 5U != 0U) { + return; + } + for (std::size_t offset = 0; offset < raw_data.size(); offset += 5U) { + for (std::size_t index = 0; index < channels_.size(); ++index) { + if (channels_[index].addr == raw_data[offset] && + channels_[index].channel == raw_data[offset + 1U]) { + last_joints_[index] = raw_data[offset + 2U]; + break; + } + } + } + std::lock_guard lock(stateMutex_); + current_joints_ = last_joints_; + }); + return current && sent; } @@ -651,4 +798,3 @@ uint16_t BioHeadRobot::angleToRaw(double angle) { } // namespace cmvr::device - diff --git a/cmvr-es/devices/canbus/CMakeLists.txt b/cmvr-es/devices/canbus/CMakeLists.txt index 76b2a460..0ce2cd40 100644 --- a/cmvr-es/devices/canbus/CMakeLists.txt +++ b/cmvr-es/devices/canbus/CMakeLists.txt @@ -31,6 +31,21 @@ target_link_libraries(socket_can_client_raw_test glog cmvr_es::proto ) +add_test( + NAME socket_can_client_raw_test + COMMAND socket_can_client_raw_test +) +set(_socket_can_client_raw_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" +) +if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _socket_can_client_raw_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") +endif() +set_tests_properties(socket_can_client_raw_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_socket_can_client_raw_test_environment}" +) add_executable(protocol_data_test @@ -93,4 +108,3 @@ target_link_libraries(can_receiver_test glog cmvr_es::proto ) - diff --git a/cmvr-es/devices/canbus/abstract_canbus.h b/cmvr-es/devices/canbus/abstract_canbus.h index b8649125..dc683f4b 100644 --- a/cmvr-es/devices/canbus/abstract_canbus.h +++ b/cmvr-es/devices/canbus/abstract_canbus.h @@ -3,6 +3,14 @@ // #pragma once +#include +#include +#include +#include +#include +#include +#include + #include "../abstract_device.h" #include "cmvr/msgs/error_code.pb.h" #include "canbus/common/byte.h" @@ -14,20 +22,26 @@ namespace cmvr::device { */ struct CanFrame { /// Message id - uint32_t id; + uint32_t id{0}; /// Message length - uint8_t len; - /// Message content - uint8_t data[8]; - /// Time stamp - struct timeval timestamp; + uint8_t len{0}; + /// Message content. Classic CAN uses at most the first 8 bytes. + uint8_t data[64]{}; + bool is_extended_id{false}; + bool is_remote_frame{false}; + bool is_error_frame{false}; + bool is_fd{false}; + bool bitrate_switch{false}; + bool error_state_indicator{false}; + /// Local host receive time used for freshness and watchdog checks. + int64_t rx_monotonic_ns{0}; + /// Legacy wall-clock field retained for source compatibility. + struct timeval timestamp{0, 0}; /** * @brief Constructor */ - CanFrame() : id(0), len(0), timestamp{0} { - std::memset(data, 0, sizeof(data)); - } + CanFrame() = default; /** * @brief CanFrame string including essential information about the message. @@ -37,10 +51,15 @@ namespace cmvr::device { std::stringstream output_stream(""); output_stream << "id:0x" << Byte::byte_to_hex(id) << ",len:" << static_cast(len) << ",data:"; - for (uint8_t i = 0; i < len; ++i) { + const auto printable_len = + std::min(len, sizeof(data)); + for (std::size_t i = 0; i < printable_len; ++i) { output_stream << Byte::byte_to_hex(data[i]); } - output_stream << ","; + output_stream << ",fd:" << is_fd + << ",brs:" << bitrate_switch + << ",extended:" << is_extended_id + << ",error:" << is_error_frame << ","; return output_stream.str(); } }; @@ -67,6 +86,28 @@ namespace cmvr::device { virtual cmvr::msgs::ErrorCode send(const std::vector &frames, int32_t *const frame_num) = 0; + /** + * @brief Send messages without starting a batch after an absolute + * local deadline. + * + * Deadline-aware transports should override this method so their + * internal blocking budget is also capped by @p deadline. The default + * preserves source compatibility and at least rejects an already + * expired request before calling send(). + */ + virtual cmvr::msgs::ErrorCode sendUntil( + const std::vector& frames, + int32_t* const frame_num, + const std::chrono::steady_clock::time_point deadline) { + if (std::chrono::steady_clock::now() >= deadline) { + if (frame_num) { + *frame_num = 0; + } + return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + return send(frames, frame_num); + } + /** * @brief Send a single message. * @param frames A single-element vector containing only one message. @@ -75,7 +116,9 @@ namespace cmvr::device { virtual cmvr::msgs::ErrorCode sendSingleFrame( const std::vector &frames) { if (frames.size() != 1U) { - CMVR_LOG(FATAL) << "frames size not equal to 1, actual frame size: " << frames.size(); + CMVR_LOG(ERROR) << "frames size not equal to 1, actual frame size: " + << frames.size(); + return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } int32_t n = 1; return send(frames, &n); @@ -91,6 +134,17 @@ namespace cmvr::device { virtual cmvr::msgs::ErrorCode receive(std::vector *const frames, int32_t *const frame_num) = 0; + /** + * @brief Discard frames already queued by the transport. + * + * Command/response protocols without a sequence field can use this + * immediately before sending a new request to reduce the risk that a + * response from an older cycle is accepted as fresh. Implementations + * must keep this call bounded. The conservative default reports that + * the transport cannot provide this guarantee. + */ + virtual bool discardPendingFrames() { return false; } + /** * @brief Get the error string. * @param status The status to get the error string. diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc index 21d9340e..61b7157f 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc @@ -12,9 +12,13 @@ #include "socket_can_client_raw.h" #include "absl/strings/str_cat.h" +#include +#include +#include +#include + namespace cmvr { namespace device { -#define CAN_ID_MASK 0x1FFFF800U // can_filter mask #define CAN_STANDARD_MAX_ID 0x7FFU using cmvr::msgs::ErrorCode; @@ -24,8 +28,25 @@ namespace cmvr { auto channel_id = cfg.channel_id(); port_ = static_cast(channel_id); interface_ = CANCardParameter::NATIVE; - - enable_can_err_check_ = false; + interface_name_ = + cfg.has_interface_name() && !cfg.interface_name().empty() + ? cfg.interface_name() + : cfg.dev_id(); + enable_fd_ = cfg.has_enable_fd() && cfg.enable_fd(); + default_bitrate_switch_ = + cfg.has_bitrate_switch() && cfg.bitrate_switch(); + receive_own_messages_ = + cfg.has_receive_own_messages() && cfg.receive_own_messages(); + receive_timeout_us_ = + cfg.has_receive_timeout_us() && cfg.receive_timeout_us() > 0 + ? cfg.receive_timeout_us() + : 100000U; + send_timeout_us_ = + cfg.has_send_timeout_us() && cfg.send_timeout_us() > 0 + ? cfg.send_timeout_us() + : 100000U; + enable_can_err_check_ = + cfg.has_enable_error_frames() && cfg.enable_error_frames(); } @@ -49,7 +70,7 @@ namespace cmvr { } SocketCanClientRaw::~SocketCanClientRaw() { - if (dev_handler_) { + if (dev_handler_ >= 0) { stop(); } } @@ -59,8 +80,8 @@ namespace cmvr { status_ = ErrorCode::OK; return true; } - struct sockaddr_can addr; - struct ifreq ifr; + struct sockaddr_can addr {}; + struct ifreq ifr {}; // open device // guss net is the device minor number, if one card is 0,1 @@ -91,17 +112,71 @@ namespace cmvr { if (ret < 0) { CMVR_LOG(ERROR) << "add receive msg id filter error code: " << ret; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } } - // 2. enable reception of can frames. - int enable = 1; - ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable, - sizeof(enable)); - if (ret < 0) { - CMVR_LOG(ERROR) << "enable reception of can frame error code: " << ret; + // 2. Explicitly opt into CAN-FD only when configured. This socket + // option does not configure the physical link bitrate or state. + if (enable_fd_) { + int enable = 1; + ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, + CAN_RAW_FD_FRAMES, &enable, sizeof(enable)); + if (ret < 0) { + CMVR_LOG(ERROR) << "enable CAN-FD frames failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + } + + const int receive_own = receive_own_messages_ ? 1 : 0; + if (::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, + &receive_own, sizeof(receive_own)) < 0) { + CMVR_LOG(ERROR) << "configure receive-own-messages failed: " + << std::strerror(errno); status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + + if (enable_can_err_check_) { + const can_err_mask_t error_mask = CAN_ERR_MASK; + if (::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, + &error_mask, sizeof(error_mask)) < 0) { + CMVR_LOG(ERROR) << "configure CAN error filter failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + } + + struct timeval receive_timeout { + static_cast(receive_timeout_us_ / 1000000U), + static_cast(receive_timeout_us_ % 1000000U) + }; + if (::setsockopt(dev_handler_, SOL_SOCKET, SO_RCVTIMEO, + &receive_timeout, sizeof(receive_timeout)) < 0) { + CMVR_LOG(ERROR) << "configure CAN receive timeout failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + + struct timeval send_timeout { + static_cast(send_timeout_us_ / 1000000U), + static_cast(send_timeout_us_ % 1000000U) + }; + if (::setsockopt(dev_handler_, SOL_SOCKET, SO_SNDTIMEO, + &send_timeout, sizeof(send_timeout)) < 0) { + CMVR_LOG(ERROR) << "configure CAN send timeout failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } @@ -115,13 +190,39 @@ namespace cmvr { interface_prefix = "can"; } - const std::string can_name = absl::StrCat(interface_prefix, port_); - std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ); - if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) { - CMVR_LOG(ERROR) << "ioctl error"; + const std::string can_name = + interface_name_.empty() + ? absl::StrCat(interface_prefix, port_) + : interface_name_; + if (can_name.size() >= IFNAMSIZ) { + CMVR_LOG(ERROR) << "CAN interface name is too long: " << can_name; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } + std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) { + CMVR_LOG(ERROR) << "CAN interface not found: " << can_name + << ", error=" << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + + if (enable_fd_) { + struct ifreq mtu_request {}; + std::strncpy(mtu_request.ifr_name, can_name.c_str(), IFNAMSIZ); + mtu_request.ifr_name[IFNAMSIZ - 1] = '\0'; + if (::ioctl(dev_handler_, SIOCGIFMTU, &mtu_request) < 0 || + mtu_request.ifr_mtu != CANFD_MTU) { + CMVR_LOG(ERROR) << "CAN-FD requested but interface MTU is not CANFD_MTU: " + << can_name; + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + } // bind socket to network interface @@ -131,8 +232,10 @@ namespace cmvr { sizeof(addr)); if (ret < 0) { - CMVR_LOG(ERROR) << "bind socket to network interface error code: " << ret; + CMVR_LOG(ERROR) << "bind socket to CAN interface failed: " + << std::strerror(errno); status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } @@ -142,10 +245,11 @@ namespace cmvr { } bool SocketCanClientRaw::stop() { - if (is_started_) { - is_started_ = false; - - int ret = close(dev_handler_); + is_started_ = false; + if (dev_handler_ >= 0) { + const int fd = dev_handler_; + dev_handler_ = -1; + int ret = close(fd); if (ret < 0) { CMVR_LOG(ERROR) << "close error code:" << ret << ", " << getErrorString(ret); return false; @@ -159,48 +263,190 @@ namespace cmvr { // Synchronous transmission of CAN messages ErrorCode SocketCanClientRaw::send(const std::vector &frames, int32_t *const frame_num) { + return sendWithDeadline_( + frames, frame_num, + std::chrono::steady_clock::now() + + std::chrono::microseconds(send_timeout_us_)); + } + + ErrorCode SocketCanClientRaw::sendUntil( + const std::vector& frames, + int32_t* const frame_num, + const std::chrono::steady_clock::time_point deadline) { + return sendWithDeadline_( + frames, frame_num, + std::min( + deadline, + std::chrono::steady_clock::now() + + std::chrono::microseconds(send_timeout_us_))); + } + + ErrorCode SocketCanClientRaw::sendWithDeadline_( + const std::vector& frames, + int32_t* const frame_num, + const std::chrono::steady_clock::time_point send_deadline) { if (frame_num == nullptr) { - CMVR_LOG(FATAL) << "frame_num is null"; + CMVR_LOG(ERROR) << "frame_num is null"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } - if (frames.size() != static_cast(*frame_num)) { - CMVR_LOG(FATAL) << "frames size does not match frame_num"; + if (*frame_num < 0 || + frames.size() != static_cast(*frame_num) || + frames.size() > static_cast(MAX_CAN_SEND_FRAME_LEN)) { + CMVR_LOG(ERROR) << "frames size does not match a valid frame_num"; + return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM; } if (!is_started_) { CMVR_LOG(ERROR) << "Nvidia can client has not been initiated! Please init first!"; return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } - for (size_t i = 0; i < frames.size() && i < MAX_CAN_SEND_FRAME_LEN; ++i) { - if (frames[i].len > CANBUS_MESSAGE_LENGTH || frames[i].len < 0) { - CMVR_LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len - << ", which is not equal to can message data length (" - << CANBUS_MESSAGE_LENGTH << ")."; - return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; - } - if (frames[i].id > CAN_STANDARD_MAX_ID) { - send_frames_[i].can_id = (frames[i].id & CAN_EFF_MASK) | CAN_EFF_FLAG; - } else { - send_frames_[i].can_id = (frames[i].id & CAN_SFF_MASK); - } - // CMVR_LOG(INFO) << "send can id is " << send_frames_[i].can_id; - send_frames_[i].can_dlc = frames[i].len; - std::memcpy(send_frames_[i].data, frames[i].data, frames[i].len); + if (std::chrono::steady_clock::now() >= send_deadline) { + *frame_num = 0; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } - // Synchronous transmission of CAN messages - int ret = static_cast( - write(dev_handler_, &send_frames_[i], sizeof(send_frames_[i]))); - if (ret <= 0) { - CMVR_LOG(ERROR) << "can " << port_ << " send message failed, error code: " << ret; - return ErrorCode::CAN_CLIENT_ERROR_BASE; + // Validate the complete batch before committing its first frame. + // This prevents a malformed later element from causing a valid + // prefix of a cyclic command batch to reach the bus. + for (size_t i = 0; i < frames.size(); ++i) { + const auto& source = frames[i]; + const auto max_length = + source.is_fd ? CANFD_MESSAGE_LENGTH + : CANBUS_MESSAGE_LENGTH; + if (source.len > max_length || + (source.is_remote_frame && source.is_fd) || + (source.is_fd && !enable_fd_)) { + *frame_num = 0; + CMVR_LOG(ERROR) << "invalid CAN frame at index " << i + << ", len=" << static_cast(source.len) + << ", fd=" << source.is_fd; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } } + int32_t sent_count = 0; + for (size_t i = 0; i < frames.size(); ++i) { + const auto& source = frames[i]; + if (std::chrono::steady_clock::now() >= send_deadline) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send batch timed out before frame " << i; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + + canid_t can_id = source.is_extended_id || + source.id > CAN_STANDARD_MAX_ID + ? (source.id & CAN_EFF_MASK) | CAN_EFF_FLAG + : (source.id & CAN_SFF_MASK); + if (source.is_remote_frame) { + can_id |= CAN_RTR_FLAG; + } + if (source.is_error_frame) { + can_id = (source.id & CAN_ERR_MASK) | CAN_ERR_FLAG; + } + + const void* payload = nullptr; + std::size_t expected = 0; + struct canfd_frame fd_frame {}; + struct can_frame classic_frame {}; + if (source.is_fd) { + fd_frame.can_id = can_id; + fd_frame.len = source.len; + if (source.bitrate_switch || default_bitrate_switch_) { + fd_frame.flags |= CANFD_BRS; + } + if (source.error_state_indicator) { + fd_frame.flags |= CANFD_ESI; + } + std::memcpy(fd_frame.data, source.data, source.len); + expected = CANFD_MTU; + payload = &fd_frame; + } else { + classic_frame.can_id = can_id; + classic_frame.can_dlc = source.len; + std::memcpy( + classic_frame.data, source.data, source.len); + expected = CAN_MTU; + payload = &classic_frame; + } + + while (true) { + const auto written = ::send( + dev_handler_, payload, expected, + MSG_DONTWAIT | MSG_NOSIGNAL); + if (written == static_cast(expected)) { + ++sent_count; + break; + } + if (written >= 0) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " sent a partial frame"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + if (errno == EINTR) { + continue; + } + if (errno != EAGAIN && errno != EWOULDBLOCK) { + *frame_num = sent_count; + CMVR_LOG(ERROR) << "can " << port_ + << " send message failed: " + << std::strerror(errno); + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + + const auto now = std::chrono::steady_clock::now(); + if (now >= send_deadline) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send batch timed out"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + const auto remaining = + std::chrono::duration_cast( + send_deadline - now); + struct timespec timeout { + static_cast( + remaining.count() / 1000000000LL), + static_cast( + remaining.count() % 1000000000LL) + }; + struct pollfd writable { + dev_handler_, POLLOUT, 0 + }; + const int ready = + ::ppoll(&writable, 1, &timeout, nullptr); + if (ready == 0) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send batch timed out"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + if (ready < 0 && errno != EINTR) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send poll failed: " + << std::strerror(errno); + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + } + } + + *frame_num = sent_count; return ErrorCode::OK; } // buf size must be 8 bytes, every time, we receive only one frame ErrorCode SocketCanClientRaw::receive(std::vector *const frames, int32_t *const frame_num) { + if (frames == nullptr || frame_num == nullptr) { + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } if (!is_started_) { CMVR_LOG(ERROR) << "Nvidia can client is not init! Please init first!"; return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; @@ -213,39 +459,109 @@ namespace cmvr { return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM; } - for (int32_t i = 0; i < *frame_num && i < MAX_CAN_RECV_FRAME_LEN; ++i) { + frames->clear(); + const int32_t requested = *frame_num; + *frame_num = 0; + for (int32_t i = 0; i < requested && i < MAX_CAN_RECV_FRAME_LEN; ++i) { CanFrame cf; - auto ret = read(dev_handler_, &recv_frames_[i], sizeof(recv_frames_[i])); + struct canfd_frame raw {}; + const auto ret = ::read(dev_handler_, &raw, CANFD_MTU); if (ret < 0) { - CMVR_LOG(ERROR) << "receive message failed, error code: " << ret; - return ErrorCode::CAN_CLIENT_ERROR_BASE; - } - if (recv_frames_[i].can_dlc > CANBUS_MESSAGE_LENGTH || - recv_frames_[i].can_dlc < 0) { - CMVR_LOG(ERROR) << "recv_frames_[" << i - << "].can_dlc = " << recv_frames_[i].can_dlc - << ", which is not equal to can message data length (" - << CANBUS_MESSAGE_LENGTH << ")."; + if (errno == EAGAIN || errno == EWOULDBLOCK || + errno == EINTR) { + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } + CMVR_LOG(ERROR) << "receive CAN message failed: " + << std::strerror(errno); return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; } - if (recv_frames_[i].can_id > CAN_STANDARD_MAX_ID) { - cf.id = enable_can_err_check_ - ? recv_frames_[i].can_id & CAN_EFF_MASK | CAN_ERR_FLAG - : recv_frames_[i].can_id & CAN_EFF_MASK; - } else { - cf.id = (recv_frames_[i].can_id & CAN_SFF_MASK); + if (ret != CAN_MTU && ret != CANFD_MTU) { + CMVR_LOG(ERROR) << "unexpected SocketCAN MTU: " << ret; + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; } - // CMVR_LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].can_id; - cf.len = recv_frames_[i].can_dlc; - std::memcpy(cf.data, recv_frames_[i].data, recv_frames_[i].can_dlc); + + const canid_t raw_id = raw.can_id; + cf.is_extended_id = (raw_id & CAN_EFF_FLAG) != 0; + cf.is_remote_frame = (raw_id & CAN_RTR_FLAG) != 0; + cf.is_error_frame = (raw_id & CAN_ERR_FLAG) != 0; + if (cf.is_error_frame) { + cf.id = raw_id & CAN_ERR_MASK; + } else if (cf.is_extended_id) { + cf.id = raw_id & CAN_EFF_MASK; + } else { + cf.id = raw_id & CAN_SFF_MASK; + } + + cf.is_fd = ret == CANFD_MTU; + if (cf.is_fd) { + cf.len = raw.len; + cf.bitrate_switch = (raw.flags & CANFD_BRS) != 0; + cf.error_state_indicator = (raw.flags & CANFD_ESI) != 0; + } else { + const auto* classic = + reinterpret_cast(&raw); + cf.len = classic->can_dlc; + } + const auto max_length = + cf.is_fd ? CANFD_MESSAGE_LENGTH : CANBUS_MESSAGE_LENGTH; + if (cf.len > max_length) { + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } + std::memcpy(cf.data, raw.data, cf.len); + + struct timespec monotonic {}; + if (::clock_gettime(CLOCK_MONOTONIC, &monotonic) == 0) { + cf.rx_monotonic_ns = + static_cast(monotonic.tv_sec) * 1000000000LL + + monotonic.tv_nsec; + } + ::gettimeofday(&cf.timestamp, nullptr); frames->push_back(cf); + ++(*frame_num); } return ErrorCode::OK; } - std::string SocketCanClientRaw::getErrorString(const int32_t /*status*/) { - return ""; + bool SocketCanClientRaw::discardPendingFrames() { + if (!is_started_ || dev_handler_ < 0) { + return false; + } + + constexpr std::size_t kMaximumDrainFrames = 4096; + const auto deadline = + std::chrono::steady_clock::now() + + std::chrono::microseconds(send_timeout_us_); + std::size_t count = 0; + while (count < kMaximumDrainFrames && + std::chrono::steady_clock::now() < deadline) { + struct canfd_frame raw {}; + const auto received = ::recv( + dev_handler_, &raw, CANFD_MTU, MSG_DONTWAIT); + if (received == CAN_MTU || received == CANFD_MTU) { + ++count; + continue; + } + if (received < 0 && + (errno == EAGAIN || errno == EWOULDBLOCK)) { + return true; + } + if (received < 0 && errno == EINTR) { + continue; + } + CMVR_LOG(ERROR) + << "failed while draining pending CAN frames: " + << (received < 0 ? std::strerror(errno) + : "unexpected MTU"); + return false; + } + CMVR_LOG(ERROR) + << "CAN receive queue did not drain within its bound"; + return false; + } + + std::string SocketCanClientRaw::getErrorString(const int32_t status) { + return std::strerror(status < 0 ? -status : status); } } } diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h index 7f478c86..ca2c01ab 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h @@ -12,11 +12,13 @@ #include #include +#include #include #include #include #include +#include #include #include @@ -51,6 +53,10 @@ namespace cmvr { */ cmvr::msgs::ErrorCode send(const std::vector &frames, int32_t *const frame_num) override; + cmvr::msgs::ErrorCode sendUntil( + const std::vector& frames, + int32_t* const frame_num, + std::chrono::steady_clock::time_point deadline) override; /** * @brief Receive messages @@ -60,6 +66,7 @@ namespace cmvr { */ cmvr::msgs::ErrorCode receive(std::vector *const frames, int32_t *const frame_num) override; + bool discardPendingFrames() override; /** * @brief Get the error string. @@ -67,14 +74,23 @@ namespace cmvr { */ std::string getErrorString(const int32_t status) override; private: - int dev_handler_ = 0; + int dev_handler_{-1}; cmvr::msgs::CANCardParameter::CANChannelId port_; cmvr::msgs::CANCardParameter::CANInterface interface_; - can_frame send_frames_[MAX_CAN_SEND_FRAME_LEN]; - can_frame recv_frames_[MAX_CAN_RECV_FRAME_LEN]; + std::string interface_name_; + bool enable_fd_{false}; + bool default_bitrate_switch_{false}; + bool receive_own_messages_{false}; + uint32_t receive_timeout_us_{100000}; + uint32_t send_timeout_us_{100000}; // bool enable_can_err_check_{false}; + + cmvr::msgs::ErrorCode sendWithDeadline_( + const std::vector& frames, + int32_t* frame_num, + std::chrono::steady_clock::time_point deadline); }; } } diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc index 391be9d3..745600eb 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc @@ -1,44 +1,226 @@ -#include "common/base/logging/logger.h" -// -// Created by lgv on 2025/7/16. -// -#include "cmvr/msgs/error_code.pb.h" -#include "cmvr/msgs/can_card_parameter.pb.h" #include "canbus/can_client/socket/socket_can_client_raw.h" -#include "gtest/gtest.h" -namespace cmvr { -namespace device { - using cmvr::msgs::ErrorCode; - using cmvr::msgs::CANCardParameter; - TEST(SocketCanClientRawTest, simple_test) { - CANCardParameter param; - param.set_brand(CANCardParameter::SOCKET_CAN_RAW); - param.set_channel_id(CANCardParameter::CHANNEL_ID_ZERO); +#include +#include +#include +#include +#include +#include +#include - cmvr::config::SocketCanConfig cfg; - cfg.set_channel_id(0); - SocketCanClientRaw socket_can_client(cfg); +#include - // EXPECT_EQ(socket_can_client.start(), ErrorCode::CAN_CLIENT_ERROR_BASE); - socket_can_client.start(); - std::vector frames; - int32_t num = 0; - EXPECT_EQ(socket_can_client.send(frames, &num), - ErrorCode::OK); - ++num; - EXPECT_EQ(socket_can_client.receive(&frames, &num), - ErrorCode::OK); - CMVR_LOG(INFO) << frames.at(0).CanFrameString(); - CanFrame can_frame; - can_frame.id = 0x123; - can_frame.len = 8; - memset(can_frame.data, 0xA3, sizeof(can_frame.data)); - frames.clear(); - frames.push_back(can_frame); - EXPECT_EQ(socket_can_client.sendSingleFrame(frames), - ErrorCode::OK); - socket_can_client.stop(); +namespace cmvr::device { +namespace { + +std::size_t openFileDescriptorCount() +{ + std::error_code error; + std::size_t count = 0; + for (std::filesystem::directory_iterator iterator( + "/proc/self/fd", error); + !error && iterator != std::filesystem::directory_iterator(); + iterator.increment(error)) { + ++count; + } + return error ? 0U : count; +} + +config::SocketCanConfig vcanConfig(const bool enable_fd) +{ + config::SocketCanConfig config; + config.set_interface_name("vcan0"); + config.set_enable_fd(enable_fd); + config.set_bitrate_switch(enable_fd); + config.set_receive_own_messages(false); + config.set_receive_timeout_us(2000U); + config.set_send_timeout_us(2000U); + return config; +} + +bool vcanAvailable() +{ + return ::if_nametoindex("vcan0") != 0U; +} + +TEST(SocketCanClientRawTest, MissingClassicInterfaceFailsWithoutLeakingFd) +{ + config::SocketCanConfig config; + config.set_interface_name("cmvr_no_such_can"); + config.set_enable_fd(false); + config.set_receive_timeout_us(100U); + config.set_send_timeout_us(100U); + SocketCanClientRaw client(config); + + const auto before = openFileDescriptorCount(); + ASSERT_GT(before, 0U); + for (int attempt = 0; attempt < 32; ++attempt) { + EXPECT_FALSE(client.start()); + EXPECT_TRUE(client.stop()); + } + const auto after = openFileDescriptorCount(); + EXPECT_LE(after, before + 1U); +} + +TEST(SocketCanClientRawTest, ClosedClientRejectsClassicSendAndReceive) +{ + config::SocketCanConfig config; + config.set_interface_name("cmvr_no_such_can"); + config.set_enable_fd(false); + SocketCanClientRaw client(config); + + CanFrame frame; + frame.id = 0x123U; + frame.len = 8U; + frame.is_fd = false; + std::fill(std::begin(frame.data), std::end(frame.data), 0xA3U); + std::vector frames{frame}; + int32_t count = 1; + EXPECT_EQ( + client.send(frames, &count), + msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED); + + count = 1; + EXPECT_EQ( + client.receive(&frames, &count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); + EXPECT_NE(frame.CanFrameString().find("fd:0"), std::string::npos); +} + +TEST(SocketCanClientRawTest, VcanTransmitsClassicAndCanFdBatches) +{ + if (!vcanAvailable()) { + GTEST_SKIP() << "vcan0 is not available in this network namespace"; + } + + SocketCanClientRaw classic_tx(vcanConfig(false)); + SocketCanClientRaw fd_rx(vcanConfig(true)); + ASSERT_TRUE(classic_tx.start()); + ASSERT_TRUE(fd_rx.start()); + + CanFrame first; + first.id = 0x123U; + first.len = 8U; + first.data[0] = 0xA1U; + CanFrame second; + second.id = 0x456U; + second.len = 3U; + second.data[0] = 0xB2U; + std::vector classic_frames{first, second}; + int32_t count = 2; + ASSERT_EQ( + classic_tx.send(classic_frames, &count), + msgs::ErrorCode::OK); + ASSERT_EQ(count, 2); + + for (const auto& expected : classic_frames) { + std::vector received; + int32_t receive_count = 1; + ASSERT_EQ( + fd_rx.receive(&received, &receive_count), + msgs::ErrorCode::OK); + ASSERT_EQ(receive_count, 1); + ASSERT_EQ(received.size(), 1U); + EXPECT_FALSE(received.front().is_fd); + EXPECT_EQ(received.front().id, expected.id); + EXPECT_EQ(received.front().len, expected.len); + EXPECT_EQ(received.front().data[0], expected.data[0]); + } + ASSERT_TRUE(classic_tx.stop()); + ASSERT_TRUE(fd_rx.stop()); + + SocketCanClientRaw fd_tx(vcanConfig(true)); + SocketCanClientRaw second_fd_rx(vcanConfig(true)); + ASSERT_TRUE(fd_tx.start()); + ASSERT_TRUE(second_fd_rx.start()); + CanFrame fd_first; + fd_first.id = 0x201U; + fd_first.len = 12U; + fd_first.is_fd = true; + fd_first.bitrate_switch = true; + fd_first.data[11] = 0xC3U; + CanFrame fd_second; + fd_second.id = 0x202U; + fd_second.len = 64U; + fd_second.is_fd = true; + fd_second.bitrate_switch = true; + fd_second.data[63] = 0xD4U; + std::vector fd_frames{fd_first, fd_second}; + count = 2; + ASSERT_EQ(fd_tx.send(fd_frames, &count), msgs::ErrorCode::OK); + ASSERT_EQ(count, 2); + + for (const auto& expected : fd_frames) { + std::vector received; + int32_t receive_count = 1; + ASSERT_EQ( + second_fd_rx.receive(&received, &receive_count), + msgs::ErrorCode::OK); + ASSERT_EQ(received.size(), 1U); + EXPECT_TRUE(received.front().is_fd); + EXPECT_TRUE(received.front().bitrate_switch); + EXPECT_EQ(received.front().id, expected.id); + EXPECT_EQ(received.front().len, expected.len); + EXPECT_EQ( + received.front().data[expected.len - 1U], + expected.data[expected.len - 1U]); } } + +TEST(SocketCanClientRawTest, VcanDrainAndBatchValidationAreFailClosed) +{ + if (!vcanAvailable()) { + GTEST_SKIP() << "vcan0 is not available in this network namespace"; + } + + SocketCanClientRaw tx(vcanConfig(false)); + SocketCanClientRaw rx(vcanConfig(false)); + ASSERT_TRUE(tx.start()); + ASSERT_TRUE(rx.start()); + + CanFrame valid; + valid.id = 0x321U; + valid.len = 8U; + valid.data[0] = 0x5AU; + std::vector one{valid}; + int32_t count = 1; + ASSERT_EQ(tx.send(one, &count), msgs::ErrorCode::OK); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + ASSERT_TRUE(rx.discardPendingFrames()); + + std::vector received; + int32_t receive_count = 1; + EXPECT_EQ( + rx.receive(&received, &receive_count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); + + CanFrame invalid = valid; + invalid.id = 0x322U; + invalid.len = 9U; + std::vector invalid_batch{valid, invalid}; + count = 2; + EXPECT_EQ( + tx.send(invalid_batch, &count), + msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED); + EXPECT_EQ(count, 0); + receive_count = 1; + EXPECT_EQ( + rx.receive(&received, &receive_count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); + + count = 1; + EXPECT_EQ( + tx.sendUntil( + one, &count, + std::chrono::steady_clock::now() - + std::chrono::microseconds(1)), + msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED); + EXPECT_EQ(count, 0); + receive_count = 1; + EXPECT_EQ( + rx.receive(&received, &receive_count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); } + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/canbus/common/canbus_consts.h b/cmvr-es/devices/canbus/common/canbus_consts.h index ed42ba19..d46f9223 100644 --- a/cmvr-es/devices/canbus/common/canbus_consts.h +++ b/cmvr-es/devices/canbus/common/canbus_consts.h @@ -26,9 +26,14 @@ namespace cmvr { namespace device { const int32_t CAN_FRAME_SIZE = 8; - const int32_t MAX_CAN_SEND_FRAME_LEN = 1; + const int32_t CAN_FD_FRAME_SIZE = 64; + // One UME cycle may submit a complete arm worth of frames. The receive + // API intentionally remains one-frame-at-a-time so a caller never + // blocks waiting to fill an artificial batch. + const int32_t MAX_CAN_SEND_FRAME_LEN = 64; const int32_t MAX_CAN_RECV_FRAME_LEN = 1; // 这个暂时改为 1 ,大量数据的时候改为 10 const int32_t CANBUS_MESSAGE_LENGTH = 8; // according to ISO-11891-1 + const int32_t CANFD_MESSAGE_LENGTH = 64; } } diff --git a/cmvr-es/devices/dexhand/abstract_dexhand.h b/cmvr-es/devices/dexhand/abstract_dexhand.h index 9cdb53fb..a68048e8 100644 --- a/cmvr-es/devices/dexhand/abstract_dexhand.h +++ b/cmvr-es/devices/dexhand/abstract_dexhand.h @@ -156,6 +156,17 @@ namespace cmvr::device { lifecycle == Status::STREAMING; } + // Stops command-driven activity without changing the device lifecycle + // or closing its transport. Implementations must return true only after + // no pre-stop activity can continue. Motion-capable hands without a + // reliable hold/idle command deliberately fail closed. + virtual bool stopOperationalActivity() { return false; } + + // Restores an activity paused by stopOperationalActivity(). This is + // called only after a new command has crossed the system admission + // boundary. Most motion-capable hands need no separate resume command. + virtual bool resumeOperationalActivity() { return true; } + virtual void setAngles(const std::vector& finger_joint_angles) = 0; virtual void setTactilePollingRegion(FingerType finger, TactileRegion region) { setTactilePollingRegions({TactileRegionKey{finger, region}}); diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h b/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h index 163f12c8..91cb479f 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h @@ -49,6 +49,8 @@ namespace cmvr::device { Status state() const override; std::string lastError() const override; void getState(DexHandState& state) override; + bool stopOperationalActivity() override; + bool resumeOperationalActivity() override; void setAngles(const std::vector& finger_joint_angles) override; void setTactilePollingRegions(const std::vector& regions) override; @@ -122,6 +124,7 @@ namespace cmvr::device { mutable std::mutex polling_mutex_; std::condition_variable polling_cv_; bool requested_polling_{true}; + bool polling_paused_for_stop_all_{false}; std::thread polling_thread_; std::atomic polling_thread_running_{false}; std::chrono::milliseconds poll_interval_{10}; diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp index d15d4cf6..0d4724d2 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp @@ -450,6 +450,28 @@ void PX6AXGen3::getState(DexHandState& state_out) { state_out = std::move(next_state); } +bool PX6AXGen3::stopOperationalActivity() { + { + std::lock_guard lock(polling_mutex_); + polling_paused_for_stop_all_ = true; + } + polling_cv_.notify_all(); + + // The refresh mutex is the bounded device-I/O dispatch boundary. Once it is + // acquired, a pre-stop sensor transaction cannot still be using the wire. + std::lock_guard refresh_lock(refresh_mutex_); + return true; +} + +bool PX6AXGen3::resumeOperationalActivity() { + { + std::lock_guard lock(polling_mutex_); + polling_paused_for_stop_all_ = false; + } + polling_cv_.notify_all(); + return true; +} + void PX6AXGen3::setAngles(const std::vector&) { CMVR_LOG(ERROR) << "PX6AXGen3 is a tactile sensor only and does not support setAngles."; } @@ -589,6 +611,12 @@ void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_r } std::lock_guard refresh_lock(refresh_mutex_); + { + std::lock_guard polling_lock(polling_mutex_); + if (polling_paused_for_stop_all_) { + return; + } + } const bool had_valid_snapshot = isSnapshotReady(read_distributed, read_resultant); try { @@ -722,9 +750,10 @@ void PX6AXGen3::pollingLoop() { auto next_poll_deadline = std::chrono::steady_clock::now(); std::unique_lock lock(polling_mutex_); while (polling_thread_running_.load(std::memory_order_acquire)) { - if (!requested_polling_) { + if (!requested_polling_ || polling_paused_for_stop_all_) { polling_cv_.wait(lock, [this]() { - return !polling_thread_running_.load(std::memory_order_acquire) || requested_polling_; + return !polling_thread_running_.load(std::memory_order_acquire) || + (requested_polling_ && !polling_paused_for_stop_all_); }); next_poll_deadline = std::chrono::steady_clock::now(); continue; @@ -746,7 +775,8 @@ void PX6AXGen3::pollingLoop() { } polling_cv_.wait_until(lock, next_poll_deadline, [this]() { - return !polling_thread_running_.load(std::memory_order_acquire); + return !polling_thread_running_.load(std::memory_order_acquire) || + polling_paused_for_stop_all_; }); } } @@ -758,6 +788,15 @@ void PX6AXGen3::ensureSensorReady(const bool allow_background, const bool background_covers_request = (!require_tactile || polls_tactile) && (!require_resultant || polls_resultant); + bool polling_paused = false; + { + std::lock_guard lock(polling_mutex_); + polling_paused = polling_paused_for_stop_all_; + } + if (polling_paused) { + return; + } + const bool background_ready = allow_background && background_covers_request && polling_thread_running_.load(std::memory_order_acquire) && diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h index 388790f8..6e19729b 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h @@ -28,14 +28,17 @@ namespace cmvr::device { class ModbusController { public: ModbusController() = default; - ~ModbusController(); + virtual ~ModbusController(); - bool open(const std::string& ip, int port); - void close(); - bool isOpen() const; + virtual bool open(const std::string& ip, int port); + virtual void close(); + virtual bool isOpen() const; - bool writeRegisters(int address, const uint16_t* values, int count); - bool readRegisterBlock(int start_address, int count, std::vector& values); + virtual bool writeRegisters(int address, const uint16_t* values, int count); + virtual bool readRegisterBlock( + int start_address, + int count, + std::vector& values); private: void closeUnlocked(); @@ -58,6 +61,9 @@ namespace cmvr::device { using RegionMask = std::bitset; explicit RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg); + RH56DFTPDexhand( + const config::RH56DFTPDexHandConfig& cfg, + std::unique_ptr controller); ~RH56DFTPDexhand() override; std::string typeName() const override { return "RH56DFTPDexhand"; } @@ -68,6 +74,8 @@ namespace cmvr::device { Status state() const override; std::string lastError() const override; void getState(DexHandState& state) override; + bool stopOperationalActivity() override; + bool resumeOperationalActivity() override; void setAngles(const std::vector& finger_joint_angles) override; void setTactilePollingRegions(const std::vector& regions) override; @@ -110,6 +118,18 @@ namespace cmvr::device { mutable std::mutex command_mutex_; std::array last_commanded_angles_{}; + // Kept separately from last_commanded_angles_: a failed Modbus block + // write may still have changed a prefix of the device registers. Such + // an attempt must remain visible to StopAll without being reported as + // a successfully accepted command. + std::array pending_angle_target_{}; + bool angle_target_unconfirmed_{false}; + + // Normal command and tactile I/O take this gate in shared mode. + // StopAll first closes admission and then takes it exclusively, which + // drains every operation that crossed the boundary before the stop. + mutable std::shared_mutex operational_gate_; + std::atomic operational_paused_{false}; std::array tactile_buffers_; std::array tactile_buffer_masks_{}; diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp index 44737981..3d0875bd 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp @@ -21,6 +21,11 @@ namespace { using Status = DexHand::Status; constexpr int kAngleSetByteAddress = 1486; + constexpr int kAngleActualByteAddress = 1546; + constexpr int kAngleStoppedTolerance = 5; + constexpr int kAngleStableTolerance = 1; + constexpr int kAngleStopConfirmationSamples = 3; + constexpr auto kAngleStopSampleInterval = std::chrono::milliseconds(10); constexpr int kDefaultPort = 6000; constexpr int kMaxRegistersPerRead = 125; @@ -325,8 +330,16 @@ void ModbusController::closeUnlocked() { } RH56DFTPDexhand::RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg) - : controller_(std::make_unique()), - dexhandCfg_(cfg) { + : RH56DFTPDexhand(cfg, std::make_unique()) { +} + +RH56DFTPDexhand::RH56DFTPDexhand( + const config::RH56DFTPDexHandConfig& cfg, + std::unique_ptr controller) + : controller_(std::move(controller)), dexhandCfg_(cfg) { + if (!controller_) { + throw std::invalid_argument("RH56 Modbus controller is required"); + } id_ = dexhandCfg_.id(); ip_address_ = dexhandCfg_.ip(); if (dexhandCfg_.port() > 0) { @@ -352,6 +365,9 @@ bool RH56DFTPDexhand::init() { } bool RH56DFTPDexhand::start() { + if (!resumeOperationalActivity()) { + return false; + } if (tactile_thread_running_.exchange(true, std::memory_order_acq_rel)) { transitionTo(Status::STREAMING); return true; @@ -387,6 +403,7 @@ bool RH56DFTPDexhand::start() { } bool RH56DFTPDexhand::stop() { + operational_paused_.store(true, std::memory_order_release); tactile_thread_running_.store(false, std::memory_order_release); polling_cv_.notify_all(); @@ -394,8 +411,14 @@ bool RH56DFTPDexhand::stop() { tactile_thread_.join(); } - if (controller_) { - controller_->close(); + { + // Drain command and tactile dispatches before closing their transport. + std::unique_lock operational_lock( + operational_gate_); + operational_paused_.store(true, std::memory_order_release); + if (controller_) { + controller_->close(); + } } if (state() != Status::FAULT) { @@ -433,13 +456,124 @@ void RH56DFTPDexhand::getState(DexHandState& state_out) { state_out = std::move(next_state); } +bool RH56DFTPDexhand::stopOperationalActivity() { + operational_paused_.store(true, std::memory_order_release); + polling_cv_.notify_all(); + + // Taking the gate exclusively confirms that every command write and + // tactile read admitted before StopAll has left the Modbus boundary. + std::unique_lock operational_lock(operational_gate_); + operational_paused_.store(true, std::memory_order_release); + + std::array target{}; + { + std::lock_guard command_lock(command_mutex_); + if (!angle_target_unconfirmed_) { + return true; + } + target = pending_angle_target_; + } + + // RH56 exposes no hold/quick-stop command. The actual-angle registers are + // therefore the only physical confirmation available. Require several + // samples both at the requested target and stable over time; a single + // sample can coincide with a joint crossing the target while still moving. + // Otherwise StopAll stays fail-closed and a later round can retry. + if (!controller_ || !controller_->isOpen()) { + CMVR_LOG(ERROR) + << "[RH56DFTPDexhand] cannot confirm the last angle target: " + "Modbus is not connected"; + return false; + } + std::vector previous_actual; + for (int sample = 0; sample < kAngleStopConfirmationSamples; ++sample) { + if (sample != 0) { + std::this_thread::sleep_for(kAngleStopSampleInterval); + } + + std::vector actual; + if (!controller_->readRegisterBlock( + kAngleActualByteAddress, + static_cast(ANGLE_COMMAND_COUNT), + actual) || + actual.size() != ANGLE_COMMAND_COUNT) { + CMVR_LOG(ERROR) + << "[RH56DFTPDexhand] failed to read actual joint angles " + "while confirming operational stop"; + return false; + } + for (std::size_t index = 0; index < target.size(); ++index) { + if (std::abs(static_cast(actual[index]) - target[index]) > + kAngleStoppedTolerance) { + CMVR_LOG(WARNING) + << "[RH56DFTPDexhand] joint " << index + << " has not reached its pending target; target=" + << target[index] << ", actual=" << actual[index]; + return false; + } + if (!previous_actual.empty() && + std::abs(static_cast(actual[index]) - + static_cast(previous_actual[index])) > + kAngleStableTolerance) { + CMVR_LOG(WARNING) + << "[RH56DFTPDexhand] joint " << index + << " is not stable while confirming operational stop; " + "previous=" + << previous_actual[index] << ", actual=" << actual[index]; + return false; + } + } + previous_actual = std::move(actual); + } + + { + std::lock_guard command_lock(command_mutex_); + angle_target_unconfirmed_ = false; + } + return true; +} + +bool RH56DFTPDexhand::resumeOperationalActivity() { + std::unique_lock operational_lock(operational_gate_); + operational_paused_.store(false, std::memory_order_release); + operational_lock.unlock(); + polling_cv_.notify_all(); + return true; +} + void RH56DFTPDexhand::setAngles(const std::vector& finger_joint_angles) { + if (finger_joint_angles.size() != ANGLE_COMMAND_COUNT) { + CMVR_LOG(ERROR) << "RH56DFTPDexhand expects exactly 6 joint angles."; + return; + } + if (operational_paused_.load(std::memory_order_acquire)) { + CMVR_LOG(WARNING) + << "[RH56DFTPDexhand] angle command rejected while operational " + "activity is paused"; + return; + } + std::shared_lock operational_lock(operational_gate_); + if (operational_paused_.load(std::memory_order_acquire)) { + return; + } const auto registers = encodeAngleCommand(finger_joint_angles); try { if (!ensureConnected()) { return; } + + // Mark the write attempt before crossing the Modbus boundary. A false + // return can represent a partial register write, so only StopAll's + // physical confirmation may clear this state. + { + std::lock_guard lock(command_mutex_); + std::copy( + finger_joint_angles.begin(), + finger_joint_angles.end(), + pending_angle_target_.begin()); + angle_target_unconfirmed_ = true; + } if (!controller_->writeRegisters( kAngleSetByteAddress, registers.data(), @@ -538,6 +672,14 @@ void RH56DFTPDexhand::refreshTactileData(const RegionMask& mask) { if (mask.none()) { return; } + if (operational_paused_.load(std::memory_order_acquire)) { + return; + } + + std::shared_lock operational_lock(operational_gate_); + if (operational_paused_.load(std::memory_order_acquire)) { + return; + } try { if (!ensureConnected()) { @@ -586,9 +728,12 @@ void RH56DFTPDexhand::tactilePollingLoop() { auto next_poll_deadline = std::chrono::steady_clock::now(); std::unique_lock lock(polling_mutex_); while (tactile_thread_running_.load(std::memory_order_acquire)) { - if (requested_polling_mask_.none()) { + if (requested_polling_mask_.none() || + operational_paused_.load(std::memory_order_acquire)) { polling_cv_.wait(lock, [this]() { - return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_.any(); + return !tactile_thread_running_.load(std::memory_order_acquire) || + (!operational_paused_.load(std::memory_order_acquire) && + requested_polling_mask_.any()); }); next_poll_deadline = std::chrono::steady_clock::now(); continue; @@ -611,7 +756,9 @@ void RH56DFTPDexhand::tactilePollingLoop() { } polling_cv_.wait_until(lock, next_poll_deadline, [this, mask]() { - return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_ != mask; + return !tactile_thread_running_.load(std::memory_order_acquire) || + operational_paused_.load(std::memory_order_acquire) || + requested_polling_mask_ != mask; }); } } @@ -667,6 +814,9 @@ void RH56DFTPDexhand::ensureTactileMaskReady(const RegionMask& mask, const bool if (mask.none()) { return; } + if (operational_paused_.load(std::memory_order_acquire)) { + return; + } const bool background_ready = allow_background && tactile_thread_running_.load(std::memory_order_acquire) && diff --git a/cmvr-es/devices/gripper/abstract_gripper.h b/cmvr-es/devices/gripper/abstract_gripper.h index 728d094b..d4dcfed1 100644 --- a/cmvr-es/devices/gripper/abstract_gripper.h +++ b/cmvr-es/devices/gripper/abstract_gripper.h @@ -23,6 +23,11 @@ namespace cmvr::device{ virtual void setPosition(float position, float vel) {} virtual void setForce(float value) {} + // Stops command-driven gripper activity while preserving the device + // lifecycle. Backends must explicitly confirm this contract before + // SystemService::StopAll can report success. + virtual bool stopOperationalActivity() { return false; } + protected: GripperState state_; }; diff --git a/cmvr-es/devices/speaker/abstract_speaker.h b/cmvr-es/devices/speaker/abstract_speaker.h index ab24542d..cfdb62d9 100644 --- a/cmvr-es/devices/speaker/abstract_speaker.h +++ b/cmvr-es/devices/speaker/abstract_speaker.h @@ -21,6 +21,11 @@ namespace cmvr::device{ virtual int getVolume() const {return 0;} virtual void pause() {} virtual void resume() {} + // Stops the current file or streamed playback without changing the + // device lifecycle. SystemService StopAll and SpeakerService use this + // typed operation; implementations should return only after their + // playback workers can no longer emit audio. + virtual bool stopPlayback() { return false; } virtual bool pushAudioFrame(const AudioStreamFrameData& frame_data) { return false; } virtual void stopStreaming() {} diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h b/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h index 124213b3..6c95eb37 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h @@ -32,6 +32,7 @@ namespace cmvr::device { bool init() override; bool start() override; bool stop() override; + bool stopPlayback() override; void play(const std::string& audio_path) override; void setVolume(int volume) override; int getVolume() const override; @@ -45,6 +46,7 @@ namespace cmvr::device { bool initPulseDevice_(); bool initAudioParams_(const std::string& audio_path); private: + bool stopPlayback_(bool deinitialize); void decode_audio_(); void play_audio_(); bool startStreamingPlayback_(const AudioStreamFrameData& frame_data); diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp b/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp index 0a6be5fb..a0c25b3b 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp @@ -34,6 +34,7 @@ ffmpegSpeaker::~ffmpegSpeaker() { is_stopping_ = true; { std::lock_guard lock(mtx_); + state_.is_initialized = false; state_.is_running = false; state_.is_decoding = false; state_.is_paused = false; @@ -94,7 +95,6 @@ void ffmpegSpeaker::resetPlayState() // 清空所有帧 } - state_.is_initialized = false; is_streaming_input_ = false; audio_path_.clear(); @@ -102,11 +102,22 @@ void ffmpegSpeaker::resetPlayState() } bool ffmpegSpeaker::stop() { + return stopPlayback_(true); +} + +bool ffmpegSpeaker::stopPlayback() { + return stopPlayback_(false); +} + +bool ffmpegSpeaker::stopPlayback_(const bool deinitialize) { std::lock_guard stop_lock(stop_mtx_); is_stopping_ = true; { lock_guard lock(mtx_); + if (deinitialize) { + state_.is_initialized = false; + } state_.is_running = false; state_.is_decoding = false; state_.is_paused = false; @@ -304,7 +315,7 @@ void ffmpegSpeaker::decode_audio_() { return; } - const AVCodec* decoder = nullptr; + AVCodec* decoder = nullptr; const int stream_index = av_find_best_stream(fmt_ctx.get(), AVMEDIA_TYPE_AUDIO, -1, -1, &decoder, 0); if (stream_index < 0) { { diff --git a/cmvr-es/main.cpp b/cmvr-es/main.cpp index 2e422db8..c1ac7852 100644 --- a/cmvr-es/main.cpp +++ b/cmvr-es/main.cpp @@ -1,10 +1,13 @@ #include +#include +#include +#include #include +#include #include "common/base/logging/logger.h" #include "runtime/include/cmvr_runtime.h" - namespace { bool blockShutdownSignals(sigset_t& shutdown_signals) @@ -15,57 +18,76 @@ bool blockShutdownSignals(sigset_t& shutdown_signals) return pthread_sigmask(SIG_BLOCK, &shutdown_signals, nullptr) == 0; } -} // namespace +struct CommandLineOptions { + std::string config_path; + double control_period_s{0.001}; + bool show_help{false}; +}; -namespace fs = std::filesystem; -static bool setupIntelMediaEnvironment() +void printUsage(const char* program) { - char exe_path_buffer[PATH_MAX] = {}; + std::cout + << "Usage: " << program + << " [--config PATH] [--control-period-s SECONDS]\n" + << "\n" + << "With no --config argument, cmvr_es loads config/cmvr_es.pb.txt " + "beside the executable.\n"; +} - const ssize_t length = readlink( - "/proc/self/exe", - exe_path_buffer, - sizeof(exe_path_buffer) - 1); - - if (length <= 0) { - std::cerr << "Failed to resolve /proc/self/exe\n"; +bool parseCommandLine( + const int argc, + char* argv[], + CommandLineOptions& options) +{ + for (int index = 1; index < argc; ++index) { + const std::string argument = argv[index]; + if (argument == "--help" || argument == "-h") { + options.show_help = true; + return true; + } + if (argument == "--config") { + if (++index >= argc || argv[index][0] == '\0') { + std::cerr << "--config requires a path\n"; + return false; + } + options.config_path = argv[index]; + continue; + } + if (argument == "--control-period-s") { + if (++index >= argc) { + std::cerr + << "--control-period-s requires a numeric value\n"; + return false; + } + char* end = nullptr; + const double value = std::strtod(argv[index], &end); + if (!end || *end != '\0' || !std::isfinite(value) || + value <= 0.0 || value > 1.0) { + std::cerr + << "--control-period-s must be in (0, 1]\n"; + return false; + } + options.control_period_s = value; + continue; + } + std::cerr << "unknown argument: " << argument << '\n'; return false; } - - exe_path_buffer[length] = '\0'; - - const fs::path executable_path(exe_path_buffer); - const fs::path bin_directory = executable_path.parent_path(); - const fs::path install_directory = bin_directory.parent_path(); - const fs::path library_directory = install_directory / "lib"; - const fs::path va_driver_directory = library_directory / "dri"; - const fs::path ihd_driver = va_driver_directory / "iHD_drv_video.so"; - - if (!fs::exists(ihd_driver)) { - std::cerr << "Intel media driver not found: " - << ihd_driver << '\n'; - return false; - } - - // overwrite=1：强制 cmvr-es 使用随项目发布的媒体运行时。 - setenv("LIBVA_DRIVER_NAME", "iHD", 1); - setenv( - "LIBVA_DRIVERS_PATH", - va_driver_directory.c_str(), - 1); - setenv( - "ONEVPL_SEARCH_PATH", - library_directory.c_str(), - 1); - return true; } +} // namespace -int main() +int main(int argc, char* argv[]) { - if (!setupIntelMediaEnvironment()) { - return EXIT_FAILURE; + CommandLineOptions options; + if (!parseCommandLine(argc, argv, options)) { + printUsage(argv[0]); + return 2; + } + if (options.show_help) { + printUsage(argv[0]); + return 0; } sigset_t shutdown_signals; @@ -74,10 +96,13 @@ int main() } cmvr::Runtime runtime; - if (!runtime.init()) { + const bool initialized = options.config_path.empty() + ? runtime.init() + : runtime.init(options.config_path); + if (!initialized) { return 1; } - if (!runtime.startTasks()) { + if (!runtime.startTasks(options.control_period_s)) { return 1; } diff --git a/cmvr-es/manager/device_manager/include/device_manager.h b/cmvr-es/manager/device_manager/include/device_manager.h index b6726f20..a974bce0 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -15,9 +15,16 @@ #include "device_factory.h" #include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "manager/safety_manager/include/safety_manager.h" namespace cmvr::device { + struct DeviceInventoryEntry { + std::string id; + DeviceKind kind = DeviceKind::Unknown; + std::shared_ptr device; + }; + class DeviceManager { public: DeviceManager(const DeviceManager&) = delete; @@ -27,16 +34,29 @@ namespace cmvr::device { static DeviceManager& getInstance(); static void destroyInstance(); - void start(); - void restart(); + bool start(); + bool restart(); void stop(); + bool initialized() const noexcept { return initialized_; } void getDeviceList(std::list> &device_list); void registerDevice(const std::shared_ptr& device); void registerDevice(const std::string& device_id, const std::shared_ptr& device); std::shared_ptr getDeviceBase(const std::string& device_id); + // Copies only manager-owned metadata and shared ownership. No device + // methods are called, so a blocked driver cannot delay this snapshot. + std::vector inventorySnapshot() const; DeviceManagerSnapshot snapshot() const; + safety::SafetyManager& safetyManager() noexcept + { + return *safety_manager_; + } + const safety::SafetyManager& safetyManager() const noexcept + { + return *safety_manager_; + } + std::string version() const; std::string name() const; std::string description() const; @@ -54,14 +74,27 @@ namespace cmvr::device { std::unordered_map devices_; std::unordered_map device_statuses_; std::unique_ptr dev_factory_; + std::unique_ptr safety_manager_; + bool initialized_{false}; explicit DeviceManager(const config::DeviceManagerConfig &cfg); void log_device_plan_() const; - void pre_scan_robot_arm_dependencies_() const; - void init_devices_(); + bool pre_scan_robot_arm_dependencies_() const; + bool init_devices_(); void configure_mujoco_viewer_pip_(); - void start_devices_(); - void stop_devices_(); + void initialize_device_statuses_(); + void mark_initializing_statuses_error_(const std::string& error_message); + void update_device_status_(const std::string& device_id, + ManagedDeviceState state, + const std::string& error_message = {}); + DeviceHealthSnapshot sample_device_health_( + const std::shared_ptr& device) const; + void update_device_health_(const std::string& device_id, + DeviceHealthSnapshot health); + bool register_device_safety_( + const std::shared_ptr& device, + const config::DeviceConfigEntry* config_entry = nullptr); + void stop_devices_(bool update_status = true); }; } // cmvr diff --git a/cmvr-es/manager/device_manager/include/device_safety_adapters.h b/cmvr-es/manager/device_manager/include/device_safety_adapters.h new file mode 100644 index 00000000..359d7f4e --- /dev/null +++ b/cmvr-es/manager/device_manager/include/device_safety_adapters.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +#include "devices/abstract_device.h" +#include "manager/safety_manager/include/safety_participant.h" + +namespace cmvr::device { + +safety::DeviceSafetyRegistration makeDeviceSafetyRegistration( + const std::shared_ptr& device, + std::chrono::milliseconds configured_maximum_age = + std::chrono::milliseconds::zero(), + std::chrono::milliseconds configured_stop_timeout = + std::chrono::milliseconds::zero()); + +} // namespace cmvr::device diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index db84586f..2ffffbb1 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -4,9 +4,13 @@ // #include "../include/device_manager.h" +#include "../include/device_safety_adapters.h" #include +#include #include +#include +#include #include "devices/agv/abstract_agv.h" #include "devices/arm/robot_arm.h" @@ -31,6 +35,113 @@ namespace { using GroupJointSelection = std::unordered_map>; using MotorJointSelections = std::unordered_map; +constexpr std::size_t kMaxDeviceErrorLength = 512; +constexpr auto kSafetyStartupValidationTimeout = std::chrono::seconds(2); + +cmvr::safety::SafetyManagerConfig safetyConfigFrom( + const cmvr::config::DeviceManagerConfig& config) +{ + cmvr::safety::SafetyManagerConfig result; + if (!config.has_safety()) { + result.enforcement_mode = cmvr::safety::EnforcementMode::Shadow; + return result; + } + + const auto& source = config.safety(); + switch (source.mode()) { + case cmvr::config::SafetyManagerConfig::LEGACY: + result.enforcement_mode = cmvr::safety::EnforcementMode::Legacy; + break; + case cmvr::config::SafetyManagerConfig::ENFORCE_SELECTED: + result.enforcement_mode = + cmvr::safety::EnforcementMode::EnforceSelected; + break; + case cmvr::config::SafetyManagerConfig::ENFORCE_ALL: + result.enforcement_mode = cmvr::safety::EnforcementMode::EnforceAll; + break; + case cmvr::config::SafetyManagerConfig::SHADOW: + case cmvr::config::SafetyManagerConfig::ENFORCEMENT_MODE_UNSPECIFIED: + default: + result.enforcement_mode = cmvr::safety::EnforcementMode::Shadow; + break; + } + for (const auto& id : source.enforced_device_ids()) { + if (!id.empty()) { + result.enforced_device_ids.insert(id); + } + } + for (const auto& entry : config.devices()) { + if (entry.safety_enforce() && !entry.id().empty()) { + result.enforced_device_ids.insert(entry.id()); + } + } + if (source.stop_all_timeout_ms() != 0) { + result.stop_all_timeout = + std::chrono::milliseconds(source.stop_all_timeout_ms()); + } + if (source.recovery_timeout_ms() != 0) { + result.recovery_timeout = + std::chrono::milliseconds(source.recovery_timeout_ms()); + } + if (source.command_ledger_result_capacity() != 0) { + result.command_ledger.result_capacity = + source.command_ledger_result_capacity(); + } + if (source.command_ledger_total_id_capacity() != 0) { + result.command_ledger.total_id_capacity = + source.command_ledger_total_id_capacity(); + } + if (source.event_history_capacity() != 0) { + result.event_history_capacity = source.event_history_capacity(); + } + result.fail_startup_on_missing_control_capability = + source.fail_startup_on_missing_control_capability(); + return result; +} + +std::uint64_t unixTimeMs() noexcept +{ + const auto elapsed = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + return elapsed.count() > 0 + ? static_cast(elapsed.count()) + : 1U; +} + +std::string truncateDeviceError(const std::string& message) +{ + return message.substr(0, kMaxDeviceErrorLength); +} + +DeviceKind deviceTypeToKind( + const cmvr::config::DeviceConfigEntry::DeviceType type) +{ + switch (type) { + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_BIO_HEAD_ROBOT: + return DeviceKind::BioHead; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM: + return DeviceKind::MotorSystem; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM: + return DeviceKind::Arm; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA: + return DeviceKind::Camera; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND: + return DeviceKind::DexHand; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MICROPHONE: + return DeviceKind::Microphone; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_SPEAKER: + return DeviceKind::Speaker; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_AGV: + return DeviceKind::AGV; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD: + return DeviceKind::MujocoWorld; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_VIEWER: + return DeviceKind::MujocoViewer; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN: + default: + return DeviceKind::Unknown; + } +} void logSection(const char* title) { @@ -122,16 +233,30 @@ std::shared_ptr DeviceManager::instance_ = nullptr; std::mutex DeviceManager::init_mutex_; -DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) { - cfg_ = cfg; - - dev_factory_ = std::make_unique(); +DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) + : cfg_(cfg), + dev_factory_(std::make_unique()), + safety_manager_(std::make_unique( + safetyConfigFrom(cfg))) +{ + initialize_device_statuses_(); logSection("Device Plan"); log_device_plan_(); - pre_scan_robot_arm_dependencies_(); + const bool dependencies_valid = pre_scan_robot_arm_dependencies_(); logSection("Initialize Devices"); - init_devices_(); - configure_mujoco_viewer_pip_(); + if (!dependencies_valid) { + mark_initializing_statuses_error_( + "device dependency validation failed"); + } + const bool devices_initialized = + dependencies_valid ? init_devices_() : false; + initialized_ = dependencies_valid && devices_initialized; + if (initialized_) { + configure_mujoco_viewer_pip_(); + } else { + CMVR_LOG(ERROR) << "[DeviceManager]: Initialization failed for at " + "least one enabled device"; + } } DeviceManager& DeviceManager::getInstance(const config::DeviceManagerConfig& cfg) { @@ -156,35 +281,159 @@ void DeviceManager::destroyInstance() { MotorManager::clearActiveJoints(); } -void DeviceManager::start(){ - for (auto& [id, record] : devices_) { - if (!record.device) { - CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; - continue; - } - if (record.device->start()) { - CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success"; - } else { - CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed"; +bool DeviceManager::start(){ + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (!initialized_) { + CMVR_LOG(ERROR) << "[DeviceManager]: Refusing to start because " + "initialization did not complete"; + stop_devices_(false); + return false; + } + + std::vector>> + devices; + { + std::shared_lock lock(devices_mutex_); + devices.reserve(devices_.size()); + for (const auto& [id, record] : devices_) { + devices.emplace_back(id, record.device); } } + + bool all_started = true; + for (const auto& [id, device] : devices) { + if (!device) { + CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; + update_device_status_( + id, ManagedDeviceState::Error, + "cannot start null device: " + id); + all_started = false; + continue; + } + bool started = false; + std::string error_message; + try { + (void)safety_manager_->advanceDeviceGeneration(id); + started = device->start(); + if (!started) { + error_message = "device start returned false: " + id; + } + } catch (const std::exception& error) { + error_message = + "device start threw for " + id + ": " + error.what(); + CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id + << " threw: " << error.what(); + } catch (...) { + error_message = + "device start threw an unknown exception: " + id; + CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id + << " threw an unknown exception"; + } + if (started) { + const auto health = sample_device_health_(device); + update_device_health_(id, health); + update_device_status_(id, ManagedDeviceState::Running); + CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success"; + } else { + update_device_status_( + id, ManagedDeviceState::Error, error_message); + CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed"; + all_started = false; + } + } + if (all_started) { + const auto coverage = safety_manager_->validateStartupCoverage( + safety::SafetyClock::now() + kSafetyStartupValidationTimeout); + if (!coverage.ready) { + all_started = false; + for (const auto& issue : coverage.issues) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Safety startup coverage failed" + << ", target=" << issue.target_id + << ", reason=" << safety::toString(issue.reason) + << ", detail=" << issue.detail; + } + } + } + if (!all_started) { + CMVR_LOG(ERROR) << "[DeviceManager]: Device or safety startup failed; " + "stopping all devices"; + // Rollback is a physical cleanup operation. Preserve the start + // results in the status table so the failure is diagnosable; an + // explicit stop() records Stopped/Error transitions. + stop_devices_(false); + } else { + safety_manager_->markStartupComplete(); + } + return all_started; } -void DeviceManager::restart() { +bool DeviceManager::restart() { stop(); - start(); + return start(); } void DeviceManager::stop() { - for (auto& [id, record] : devices_) { - if (!record.device) { + std::lock_guard lifecycle_lock(lifecycle_mutex_); + stop_devices_(); +} + +void DeviceManager::stop_devices_(const bool update_status) { + std::vector>> + devices; + { + std::shared_lock lock(devices_mutex_); + devices.reserve(devices_.size()); + for (const auto& [id, record] : devices_) { + devices.emplace_back(id, record.device); + } + } + + for (const auto& [id, device] : devices) { + if (!device) { CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; + if (update_status) { + update_device_status_( + id, ManagedDeviceState::Error, + "cannot stop null device: " + id); + } continue; } - if (record.device->stop()) { + bool stopped = false; + std::string error_message; + try { + stopped = device->stop(); + if (!stopped) { + error_message = "device stop returned false: " + id; + } + } catch (const std::exception& error) { + error_message = + "device stop threw for " + id + ": " + error.what(); + CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id + << " threw: " << error.what(); + } catch (...) { + error_message = + "device stop threw an unknown exception: " + id; + CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id + << " threw an unknown exception"; + } + if (stopped) { + if (update_status) { + update_device_status_(id, ManagedDeviceState::Stopped); + } CMVR_LOG(INFO) << "[DeviceManager]: Stop device " << id << " Success"; + safety_manager_->updateDeviceRuntimeState( + id, ManagedDeviceState::Stopped, + sample_device_health_(device)); } else { + if (update_status) { + update_device_status_( + id, ManagedDeviceState::Error, error_message); + } CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id << " Failed"; + safety_manager_->updateDeviceRuntimeState( + id, ManagedDeviceState::Error, + {DeviceHealthState::Fault, error_message}); } } } @@ -192,6 +441,7 @@ void DeviceManager::stop() { template std::shared_ptr DeviceManager::getDevice(const std::string& device_id) { + std::shared_lock lock(devices_mutex_); auto it = devices_.find(device_id); if (it == devices_.end()) { CMVR_LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " not found."; @@ -217,8 +467,27 @@ std::shared_ptr DeviceManager::getDeviceBase(const std::string& return it->second.device; } +std::vector DeviceManager::inventorySnapshot() const +{ + std::vector result; + { + std::shared_lock lock(devices_mutex_); + result.reserve(devices_.size()); + for (const auto& [id, record] : devices_) { + result.push_back({id, record.kind, record.device}); + } + } + + std::sort(result.begin(), result.end(), + [](const auto& lhs, const auto& rhs) { + return lhs.id < rhs.id; + }); + return result; +} + void DeviceManager::getDeviceList(std::list>& device_list){ device_list.clear(); + std::shared_lock lock(devices_mutex_); for (const auto& [device_id, record] : devices_) { device_list.emplace_back(device_id, record.type_name); } @@ -244,42 +513,139 @@ void DeviceManager::registerDevice(const std::string& device_id, CMVR_LOG(ERROR) << "[DeviceManager]: Cannot register device with empty id"; return; } - if (devices_.count(device_id)) { - CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id; - return; - } DeviceRecord record; record.id = device_id; record.kind = device->kind(); record.type_name = device->typeName(); record.device = device; - devices_.emplace(record.id, std::move(record)); + { + std::unique_lock lock(devices_mutex_); + if (devices_.count(device_id)) { + CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id; + return; + } + + ManagedDeviceSnapshot status; + status.id = record.id; + status.kind = record.kind; + status.type_name = record.type_name; + status.enabled = true; + status.state = ManagedDeviceState::Registered; + status.status_updated_at_unix_ms = unixTimeMs(); + devices_.emplace(record.id, std::move(record)); + device_statuses_[device_id] = std::move(status); + } + if (!register_device_safety_(device)) { + update_device_status_( + device_id, ManagedDeviceState::Error, + "failed to register device safety capability: " + device_id); + CMVR_LOG(ERROR) << "[DeviceManager]: Failed to register device safety " + "capability, id=" << device_id; + } else { + update_device_health_( + device_id, sample_device_health_(device)); + } CMVR_LOG(INFO) << "[DeviceManager]: Register device success" << ", id=" << device_id << ", type=" << device->typeName() << ", kind=" << toString(device->kind()); } +void DeviceManager::initialize_device_statuses_() +{ + std::unique_lock lock(devices_mutex_); + for (const auto& entry : cfg_.devices()) { + const auto kind = deviceTypeToKind(entry.type()); + ManagedDeviceSnapshot status; + status.id = entry.id(); + status.kind = kind; + status.type_name = toString(kind); + status.enabled = entry.enable(); + status.state = entry.enable() + ? ManagedDeviceState::Initializing + : ManagedDeviceState::Disabled; + status.status_updated_at_unix_ms = unixTimeMs(); + + if (entry.id().empty()) { + status.state = ManagedDeviceState::Error; + status.abnormal = true; + status.error_message = + "configured device id must not be empty"; + } + + const auto [it, inserted] = + device_statuses_.emplace(entry.id(), std::move(status)); + if (!inserted) { + auto& duplicate_status = it->second; + duplicate_status.enabled = + duplicate_status.enabled || entry.enable(); + duplicate_status.state = ManagedDeviceState::Error; + duplicate_status.abnormal = true; + duplicate_status.error_message = truncateDeviceError( + "duplicate configured device id: " + entry.id()); + duplicate_status.status_updated_at_unix_ms = unixTimeMs(); + } + } +} + +void DeviceManager::mark_initializing_statuses_error_( + const std::string& error_message) +{ + std::unique_lock lock(devices_mutex_); + for (auto& [id, status] : device_statuses_) { + if (status.state != ManagedDeviceState::Initializing) { + continue; + } + status.state = ManagedDeviceState::Error; + status.abnormal = true; + status.error_message = truncateDeviceError( + error_message + ": " + id); + status.status_updated_at_unix_ms = unixTimeMs(); + } +} + +void DeviceManager::update_device_status_( + const std::string& device_id, + const ManagedDeviceState state, + const std::string& error_message) +{ + DeviceHealthSnapshot health; + { + std::unique_lock lock(devices_mutex_); + auto& status = device_statuses_[device_id]; + if (status.id.empty()) { + status.id = device_id; + } + const auto device_it = devices_.find(device_id); + if (device_it != devices_.end()) { + status.kind = device_it->second.kind; + status.type_name = device_it->second.type_name; + } + status.enabled = true; + status.state = state; + status.abnormal = state == ManagedDeviceState::Error; + status.error_message = + state == ManagedDeviceState::Error + ? truncateDeviceError( + error_message.empty() + ? "device lifecycle operation failed: " + device_id + : error_message) + : std::string{}; + status.status_updated_at_unix_ms = unixTimeMs(); + health = status.health; + } + safety_manager_->updateDeviceRuntimeState(device_id, state, health); +} + DeviceManagerSnapshot DeviceManager::snapshot() const { - struct SnapshotSource { - ManagedDeviceSnapshot status; - std::shared_ptr device; - }; - - std::vector sources; + std::vector sources; { std::shared_lock lock(devices_mutex_); - sources.reserve(devices_.size()); - for (const auto& [id, record] : devices_) { - SnapshotSource source; - source.status.id = id; - source.status.kind = record.kind; - source.status.type_name = record.type_name; - source.status.enabled = true; - source.status.state = ManagedDeviceState::Ready; - source.device = record.device; - sources.push_back(std::move(source)); + sources.reserve(device_statuses_.size()); + for (const auto& [id, stored_status] : device_statuses_) { + (void)id; + sources.push_back(stored_status); } } @@ -290,23 +656,22 @@ DeviceManagerSnapshot DeviceManager::snapshot() const result.devices.reserve(sources.size()); for (auto& source : sources) { - if (source.device) { - try { - source.status.health = source.device->healthSnapshot(); - } catch (const std::exception& error) { - source.status.health.state = DeviceHealthState::Fault; - source.status.health.error_message = error.what(); - } catch (...) { - source.status.health.state = DeviceHealthState::Fault; - source.status.health.error_message = - "device health snapshot threw an unknown exception"; - } + source.health.error_message = + truncateDeviceError(source.health.error_message); + const bool lifecycle_error = + source.state == ManagedDeviceState::Error; + const bool health_error = + source.health.state == DeviceHealthState::Degraded || + source.health.state == DeviceHealthState::Fault; + source.abnormal = lifecycle_error || health_error; + if (source.error_message.empty()) { + source.error_message = source.health.error_message; } - source.status.abnormal = - source.status.health.state == DeviceHealthState::Degraded || - source.status.health.state == DeviceHealthState::Fault; - source.status.error_message = source.status.health.error_message; - result.devices.push_back(std::move(source.status)); + source.error_message = truncateDeviceError(source.error_message); + if (source.status_updated_at_unix_ms == 0) { + source.status_updated_at_unix_ms = unixTimeMs(); + } + result.devices.push_back(std::move(source)); } std::sort(result.devices.begin(), result.devices.end(), @@ -317,6 +682,78 @@ DeviceManagerSnapshot DeviceManager::snapshot() const return result; } +DeviceHealthSnapshot DeviceManager::sample_device_health_( + const std::shared_ptr& device) const +{ + if (!device) { + return { + DeviceHealthState::Fault, + "device health target is null"}; + } + try { + auto health = device->healthSnapshot(); + health.error_message = truncateDeviceError(health.error_message); + return health; + } catch (const std::exception& error) { + return {DeviceHealthState::Fault, truncateDeviceError(error.what())}; + } catch (...) { + return { + DeviceHealthState::Fault, + "device health snapshot threw an unknown exception"}; + } +} + +void DeviceManager::update_device_health_( + const std::string& device_id, + DeviceHealthSnapshot health) +{ + ManagedDeviceState lifecycle = ManagedDeviceState::Unknown; + { + std::unique_lock lock(devices_mutex_); + auto& status = device_statuses_[device_id]; + status.health = std::move(health); + lifecycle = status.state; + const bool health_error = + status.health.state == DeviceHealthState::Degraded || + status.health.state == DeviceHealthState::Fault; + status.abnormal = + status.state == ManagedDeviceState::Error || health_error; + if (status.state != ManagedDeviceState::Error) { + status.error_message = status.health.error_message; + } + status.status_updated_at_unix_ms = unixTimeMs(); + health = status.health; + } + safety_manager_->updateDeviceRuntimeState( + device_id, lifecycle, std::move(health)); +} + +bool DeviceManager::register_device_safety_( + const std::shared_ptr& device, + const config::DeviceConfigEntry* config_entry) +{ + auto maximum_age = std::chrono::milliseconds::zero(); + auto stop_timeout = std::chrono::milliseconds::zero(); + if (config_entry) { + if (config_entry->maximum_safety_snapshot_age_ms() != 0) { + maximum_age = std::chrono::milliseconds( + config_entry->maximum_safety_snapshot_age_ms()); + } + if (config_entry->safety_stop_timeout_ms() != 0) { + stop_timeout = std::chrono::milliseconds( + config_entry->safety_stop_timeout_ms()); + } + } + auto registration = makeDeviceSafetyRegistration( + device, maximum_age, stop_timeout); + if (registration.descriptor.device_id.empty()) { + return false; + } + const bool registered = + safety_manager_->registerDevice(std::move(registration)); + return registered; +} + std::string DeviceManager::version() const { return cfg_.version().empty() ? "1.0" : cfg_.version(); } @@ -354,10 +791,11 @@ void DeviceManager::log_device_plan_() const CMVR_LOG(INFO) << "[DeviceManager]: Device plan end"; } -void DeviceManager::pre_scan_robot_arm_dependencies_() const +bool DeviceManager::pre_scan_robot_arm_dependencies_() const { MotorJointSelections selections; std::unordered_map motor_roots; + MotorManager::clearActiveJoints(); for (const auto& entry : cfg_.devices()) { if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) { @@ -365,22 +803,22 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty"; - return; + return false; } if (entry.config_file().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id(); - return; + return false; } config::MotorRootConfig root_cfg; if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file(); - return; + return false; } if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) { CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id() << "' does not match config id '" << root_cfg.motor().id() << "'"; - return; + return false; } motor_roots.emplace(entry.id(), std::move(root_cfg)); } @@ -391,17 +829,17 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty"; - return; + return false; } if (entry.config_file().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id(); - return; + return false; } config::ArmRootConfig root_cfg; if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file(); - return; + return false; } const config::RobotArmConfig* arm_cfg = nullptr; @@ -414,29 +852,30 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const if (!arm_cfg) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id() << "' not found in config: " << entry.config_file(); - return; + return false; } - if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) { + if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor || + arm_cfg->backend_case() == config::RobotArmConfig::kUme) { continue; } if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id(); - return; + return false; } const auto& motor_config = arm_cfg->motor(); if (motor_config.motor_system_id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id(); - return; + return false; } if (motor_config.motor_group_ids_size() == 0) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id(); - return; + return false; } if (motor_config.joint_names_size() == 0) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id(); - return; + return false; } const auto motor_root_it = motor_roots.find(motor_config.motor_system_id()); @@ -444,7 +883,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() << "' depends on disabled or missing MotorManager: " << motor_config.motor_system_id(); - return; + return false; } std::unordered_set allowed_groups; @@ -452,7 +891,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const for (const auto& group_id : motor_config.motor_group_ids()) { if (group_id.empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id(); - return; + return false; } allowed_groups.insert(group_id); } @@ -461,7 +900,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const for (const auto& joint_name : motor_config.joint_names()) { if (joint_name.empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id(); - return; + return false; } std::string matched_group; @@ -480,7 +919,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() << "' joint '" << joint_name << "' not found in configured motor_group_ids"; - return; + return false; } group_selection[matched_group].insert(joint_name); } @@ -495,46 +934,132 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } } - MotorManager::clearActiveJoints(); for (auto& [motor_system_id, group_selection] : selections) { MotorManager::setActiveJoints(motor_system_id, std::move(group_selection)); } + return true; } -void DeviceManager::init_devices_() { +bool DeviceManager::init_devices_() { + bool all_initialized = true; for (const auto& entry : cfg_.devices()) { if (!entry.enable()) { continue; } + { + std::shared_lock lock(devices_mutex_); + const auto status_it = device_statuses_.find(entry.id()); + if (status_it != device_statuses_.end() && + status_it->second.state == ManagedDeviceState::Error) { + all_initialized = false; + continue; + } + } + CMVR_LOG(INFO) << "[DeviceManager]: Initialize device begin" << ", id=" << entry.id() << ", type=" << deviceTypeToString(entry.type()) << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); - DeviceRecord record = dev_factory_->create(entry); + DeviceRecord record; + try { + record = dev_factory_->create(entry); + } catch (const std::exception& error) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "device creation threw for " + entry.id() + ": " + + error.what()); + CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw for " + << entry.id() << ": " << error.what(); + all_initialized = false; + continue; + } catch (...) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "device creation threw an unknown exception: " + + entry.id()); + CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw an " + "unknown exception for " << entry.id(); + all_initialized = false; + continue; + } if (!record.device || record.id.empty()) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "failed to create configured device: " + entry.id()); CMVR_LOG(ERROR) << "[DeviceManager]: Failed to create device for entry id=" << entry.id(); + all_initialized = false; continue; } CMVR_LOG(INFO) << "[DeviceManager]: Create device object success" << ", id=" << record.id << ", type=" << record.type_name << ", kind=" << toString(record.kind); - if (devices_.count(record.id)) { + bool duplicate_device = false; + { + std::shared_lock lock(devices_mutex_); + duplicate_device = devices_.count(record.id) != 0; + } + if (duplicate_device) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "duplicate configured device id: " + record.id); CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate " << record.type_name << " Device ID " << record.id; + all_initialized = false; continue; } CMVR_LOG(INFO) << "[DeviceManager]: Init device object begin" << ", id=" << record.id << ", type=" << record.type_name << ", kind=" << toString(record.kind); - if (!record.device->init()) { + bool device_initialized = false; + std::string init_error_message; + try { + device_initialized = record.device->init(); + if (!device_initialized) { + init_error_message = + "device init returned false: " + record.id; + } + } catch (const std::exception& error) { + init_error_message = + "device init threw for " + record.id + ": " + + error.what(); + CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw" + << ", id=" << record.id + << ", error=" << error.what(); + } catch (...) { + init_error_message = + "device init threw an unknown exception: " + record.id; + CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw an " + "unknown exception, id=" << record.id; + } + if (!device_initialized) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + init_error_message); CMVR_LOG(ERROR) << "[DeviceManager]: Init device object failed" << ", id=" << record.id << ", type=" << record.type_name << ", kind=" << toString(record.kind) << ", config_file=" << entry.config_file(); + try { + if (!record.device->stop()) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Cleanup after failed init " + "returned false, id=" << record.id; + } + } catch (const std::exception& error) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Cleanup after failed init threw" + << ", id=" << record.id + << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Cleanup after failed init threw an " + "unknown exception, id=" << record.id; + } + all_initialized = false; continue; } CMVR_LOG(INFO) << "[DeviceManager]: Init device object success" @@ -542,8 +1067,55 @@ void DeviceManager::init_devices_() { << ", type=" << record.type_name << ", kind=" << toString(record.kind) << ", config_file=" << entry.config_file(); - devices_.emplace(record.id, std::move(record)); + const auto registered_device = record.device; + std::string registered_id; + { + std::unique_lock lock(devices_mutex_); + const auto id = record.id; + const auto kind = record.kind; + const auto type_name = record.type_name; + const auto [device_it, inserted] = + devices_.emplace(id, std::move(record)); + if (!inserted) { + auto& status = device_statuses_[entry.id()]; + status.state = ManagedDeviceState::Error; + status.abnormal = true; + status.error_message = truncateDeviceError( + "duplicate configured device id: " + id); + status.status_updated_at_unix_ms = unixTimeMs(); + all_initialized = false; + continue; + } + + auto& status = device_statuses_[id]; + status.id = id; + status.kind = kind; + status.type_name = type_name; + status.enabled = true; + status.state = ManagedDeviceState::Ready; + status.abnormal = false; + status.error_message.clear(); + status.status_updated_at_unix_ms = unixTimeMs(); + registered_id = id; + } + + if (!register_device_safety_(registered_device, &entry)) { + update_device_status_( + registered_id, ManagedDeviceState::Error, + "failed to register device safety capability: " + + registered_id); + CMVR_LOG(ERROR) + << "[DeviceManager]: Failed to register device safety " + "capability" + << ", id=" << registered_id + << ", kind=" << toString(registered_device->kind()); + all_initialized = false; + } else { + update_device_health_( + registered_id, sample_device_health_(registered_device)); + } } + return all_initialized; } void DeviceManager::configure_mujoco_viewer_pip_() diff --git a/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp b/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp index 05d5bf8c..25f69f36 100644 --- a/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp +++ b/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp @@ -5,8 +5,12 @@ #include "devices/microphone/abstract_microphone.h" #include +#include +#include #include +#include #include +#include #include #include #include @@ -23,6 +27,7 @@ namespace { using cmvr::device::AbstractDevice; using cmvr::device::DeviceHealthSnapshot; using cmvr::device::DeviceHealthState; +using cmvr::device::DeviceInventoryEntry; using cmvr::device::DeviceKind; using cmvr::device::DeviceManager; using cmvr::device::DeviceManagerSnapshot; @@ -123,6 +128,51 @@ public: std::atomic health_calls{0}; }; +class BlockingHealthDevice final : public AbstractDevice { +public: + explicit BlockingHealthDevice(std::string id) + : AbstractDevice(std::move(id)) + { + } + + DeviceKind kind() const noexcept override { return DeviceKind::Arm; } + std::string typeName() const override { return "BlockingHealthDevice"; } + + DeviceHealthSnapshot healthSnapshot() override + { + std::unique_lock lock(mutex_); + ++health_calls; + health_entered_ = true; + condition_.notify_all(); + condition_.wait(lock, [this] { return release_health_; }); + return {DeviceHealthState::Healthy, {}}; + } + + bool waitForHealthCall(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [this] { return health_entered_; }); + } + + void releaseHealthCall() + { + { + std::lock_guard lock(mutex_); + release_health_ = true; + } + condition_.notify_all(); + } + + std::atomic health_calls{0}; + +private: + std::mutex mutex_; + std::condition_variable condition_; + bool health_entered_{false}; + bool release_health_{false}; +}; + const ManagedDeviceSnapshot* findDevice(const DeviceManagerSnapshot& snapshot, const std::string& id) { @@ -144,6 +194,16 @@ bool isSorted(const DeviceManagerSnapshot& snapshot) return true; } +bool isSorted(const std::vector& inventory) +{ + for (std::size_t i = 1; i < inventory.size(); ++i) { + if (inventory[i].id < inventory[i - 1].id) { + return false; + } + } + return true; +} + bool testCategoryHealthAdapters() { MemoryCamera camera; @@ -253,6 +313,13 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(duplicate_status->error_message == "duplicate configured device id: duplicate_device"); + // Configuration failures deliberately make this manager ineligible for + // start(). Use a fresh, valid manager for dynamic registration and + // lifecycle transitions so the test does not weaken fail-closed startup. + DeviceManager::destroyInstance(); + cmvr::config::DeviceManagerConfig dynamic_config; + auto& dynamic_manager = DeviceManager::getInstance(dynamic_config); + auto healthy = std::make_shared("z_healthy"); auto degraded = std::make_shared("a_degraded"); degraded->health = { @@ -264,18 +331,18 @@ bool testConfiguredAndDynamicSnapshots() auto health_throw = std::make_shared("b_health_throw"); health_throw->throw_on_health = true; - manager.registerDevice(healthy); - manager.registerDevice(degraded); - manager.registerDevice(start_fail); - manager.registerDevice(stop_fail); - manager.registerDevice(health_throw); + dynamic_manager.registerDevice(healthy); + dynamic_manager.registerDevice(degraded); + dynamic_manager.registerDevice(start_fail); + dynamic_manager.registerDevice(stop_fail); + dynamic_manager.registerDevice(health_throw); // Duplicate registration must retain the original object and status. - manager.registerDevice( + dynamic_manager.registerDevice( std::make_shared("z_healthy", DeviceKind::Speaker)); - CHECK_TRUE(manager.getDeviceBase("z_healthy") == healthy); + CHECK_TRUE(dynamic_manager.getDeviceBase("z_healthy") == healthy); - const auto registered = manager.snapshot(); + const auto registered = dynamic_manager.snapshot(); CHECK_TRUE(isSorted(registered)); const auto* healthy_registered = findDevice(registered, "z_healthy"); @@ -304,8 +371,8 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(thrown_health->health.error_message.size() <= 512); CHECK_TRUE(thrown_health->error_message.size() <= 512); - manager.start(); - const auto running = manager.snapshot(); + CHECK_TRUE(!dynamic_manager.start()); + const auto running = dynamic_manager.snapshot(); CHECK_TRUE(findDevice(running, "z_healthy")->state == ManagedDeviceState::Running); CHECK_TRUE(findDevice(running, "m_start_fail")->state == @@ -319,14 +386,16 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(healthy_registered->state == ManagedDeviceState::Registered); - manager.stop(); - const auto stopped = manager.snapshot(); + dynamic_manager.stop(); + const auto stopped = dynamic_manager.snapshot(); CHECK_TRUE(findDevice(stopped, "z_healthy")->state == ManagedDeviceState::Stopped); CHECK_TRUE(findDevice(stopped, "n_stop_fail")->state == ManagedDeviceState::Error); CHECK_TRUE(findDevice(stopped, "n_stop_fail")->abnormal); - CHECK_TRUE(healthy->stop_calls.load() == 1); + // Failed start rolls back every device once; explicit stop performs the + // second best-effort stop. + CHECK_TRUE(healthy->stop_calls.load() == 2); return true; } @@ -365,6 +434,71 @@ bool testConcurrentSnapshotAndRegistration() return true; } +bool testManagerSnapshotsDoNotWaitForDeviceHealth() +{ + DeviceManager::destroyInstance(); + cmvr::config::DeviceManagerConfig config; + auto& manager = DeviceManager::getInstance(config); + auto blocking_device = + std::make_shared("blocked_health_arm"); + auto other_device = + std::make_shared("a_camera", DeviceKind::Camera); + manager.registerDevice(other_device); + + auto registration_future = std::async( + std::launch::async, [&manager, blocking_device] { + manager.registerDevice(blocking_device); + }); + if (!blocking_device->waitForHealthCall(std::chrono::seconds(2))) { + blocking_device->releaseHealthCall(); + registration_future.wait(); + return false; + } + + auto snapshot_future = std::async(std::launch::async, [&manager] { + return manager.snapshot(); + }); + if (snapshot_future.wait_for(std::chrono::milliseconds(250)) != + std::future_status::ready) { + blocking_device->releaseHealthCall(); + snapshot_future.wait(); + registration_future.wait(); + return false; + } + + auto inventory_future = std::async(std::launch::async, [&manager] { + return manager.inventorySnapshot(); + }); + if (inventory_future.wait_for(std::chrono::milliseconds(250)) != + std::future_status::ready) { + blocking_device->releaseHealthCall(); + inventory_future.wait(); + registration_future.wait(); + return false; + } + + const auto snapshot = snapshot_future.get(); + const auto inventory = inventory_future.get(); + const auto* blocked_status = + findDevice(snapshot, "blocked_health_arm"); + const bool snapshots_valid = + blocked_status != nullptr && + blocked_status->state == ManagedDeviceState::Registered && + blocked_status->health.state == DeviceHealthState::Unknown && + inventory.size() == 2 && isSorted(inventory) && + inventory[0].id == "a_camera" && + inventory[0].kind == DeviceKind::Camera && + inventory[0].device == other_device && + inventory[1].id == "blocked_health_arm" && + inventory[1].kind == DeviceKind::Arm && + inventory[1].device == blocking_device && + blocking_device->health_calls.load() == 1; + + blocking_device->releaseHealthCall(); + registration_future.get(); + return snapshots_valid && blocking_device->health_calls.load() == 1; +} + } // namespace int main() @@ -373,7 +507,8 @@ int main() const bool success = testCategoryHealthAdapters() && testConfiguredAndDynamicSnapshots() && - testConcurrentSnapshotAndRegistration(); + testConcurrentSnapshotAndRegistration() && + testManagerSnapshotsDoNotWaitForDeviceHealth(); DeviceManager::destroyInstance(); return success ? 0 : 1; } diff --git a/cmvr-es/manager/media_source_hub/CMakeLists.txt b/cmvr-es/manager/media_source_hub/CMakeLists.txt deleted file mode 100644 index c6220d16..00000000 --- a/cmvr-es/manager/media_source_hub/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) - cmake_minimum_required(VERSION 3.22) - project(cmvr_media_source_hub LANGUAGES CXX) - enable_testing() -endif() - -add_library(media_source_hub STATIC - src/media_source_hub.cpp -) - -target_compile_features(media_source_hub PUBLIC cxx_std_17) -target_include_directories(media_source_hub - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../.. -) - -add_library(cmvr_es::media_source_hub ALIAS media_source_hub) - -if(NOT CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) - add_library(media_source_hub_device_adapter STATIC - src/device_media_source_adapter.cpp - ) - target_compile_features(media_source_hub_device_adapter PUBLIC cxx_std_17) - target_include_directories(media_source_hub_device_adapter - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../.. - ) - target_link_libraries(media_source_hub_device_adapter - PUBLIC - cmvr_es::media_source_hub - cmvr_es::common - cmvr_es::proto - cmvr_es::logging - ) - add_library(cmvr_es::media_source_hub_device_adapter ALIAS media_source_hub_device_adapter) -endif() - -option(CMVR_MEDIA_SOURCE_HUB_BUILD_TESTS - "Build the standalone MediaSourceHub self-test" - ${PROJECT_IS_TOP_LEVEL}) - -if(CMVR_MEDIA_SOURCE_HUB_BUILD_TESTS) - find_package(Threads REQUIRED) - add_executable(media_source_hub_test - tests/media_source_hub_test.cpp - ) - target_compile_features(media_source_hub_test PRIVATE cxx_std_17) - target_link_libraries(media_source_hub_test - PRIVATE - cmvr_es::media_source_hub - Threads::Threads - ) - # This self-test only links the static Hub and pthreads. In the root build, - # the project-wide third-party RUNPATH can otherwise make the loader pick up - # a vendor libstdc++.so (for example from the AUBO SDK), even though the test - # has no dependency on that SDK. - set_target_properties(media_source_hub_test PROPERTIES - SKIP_BUILD_RPATH TRUE - ) - add_test(NAME media_source_hub_test COMMAND media_source_hub_test) -endif() diff --git a/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h b/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h deleted file mode 100644 index f1b87205..00000000 --- a/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H -#define CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H - -#pragma once - -#include -#include -#include - -#include "devices/camera/abstract_camera.h" -#include "devices/microphone/abstract_microphone.h" -#include "manager/media_source_hub/include/media_source_hub.h" - -namespace cmvr::media { - -// Process-wide protocol-neutral media hub shared by gRPC and QUIC services. -MediaSourceHub& globalMediaSourceHub(); - -// Registration is idempotent for an already registered track. The adapter owns a -// short-lived pump thread and one startStreaming()/stopStreaming() lease only while -// at least one Hub subscription is active. It ensures start() succeeds but deliberately -// does not call stop(), because the base device lifecycle can also be owned by control RPCs. -bool ensureCameraMediaSource( - MediaSourceHub& hub, - const std::shared_ptr& camera, - size_t ring_capacity = 64); - -bool ensureMicrophoneMediaSource( - MediaSourceHub& hub, - const std::shared_ptr& microphone, - size_t ring_capacity = 256); - -} // namespace cmvr::media - -#endif // CMVR_ES_DEVICE_MEDIA_SOURCE_ADAPTER_H diff --git a/cmvr-es/manager/media_source_hub/include/media_source_hub.h b/cmvr-es/manager/media_source_hub/include/media_source_hub.h deleted file mode 100644 index 39daa7bd..00000000 --- a/cmvr-es/manager/media_source_hub/include/media_source_hub.h +++ /dev/null @@ -1,132 +0,0 @@ -#ifndef CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H -#define CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/base/ring_buffer.h" -#include "common/media/media_frame.h" - -namespace cmvr::media { - -// MediaSourceHub owns no protocol-specific state. A device or capture adapter registers -// start/stop callbacks and receives a sink callback when the first consumer subscribes. -class MediaSourceHub final { -public: - using FrameRing = BroadcastFrameRing; - using FrameReadResult = FrameRing::ReadResult; - using StartPosition = FrameRing::StartPosition; - using FrameSink = std::function; - // Cancellation checks run while MediaSourceHub protects source lifecycle - // state. Predicates must therefore be fast, non-blocking and must not call - // back into the same hub. - using CancelPredicate = std::function; - - struct SourceCallbacks { - // start() may run asynchronously. It must observe cancelled during any - // potentially blocking startup work and return false promptly once set. - // MediaSourceHub retains the callback state until a non-cooperative start - // eventually returns, so late completion cannot access destroyed state. - std::function start; - // stop() is the synchronous publication barrier for the last lease and - // must unblock and join the source producer before returning. - std::function stop; - std::function request_key_frame; - }; - -private: - struct SourceState; - -public: - class Subscription final { - public: - Subscription() = default; - ~Subscription(); - - Subscription(const Subscription&) = delete; - Subscription& operator=(const Subscription&) = delete; - Subscription(Subscription&& other) noexcept; - Subscription& operator=(Subscription&& other) noexcept; - - // A Subscription owns one reader cursor and is single-consumer. Moving, - // resetting, or reading the same object concurrently is unsupported; use - // one independent subscription per consumer thread. - bool valid() const; - explicit operator bool() const { return valid(); } - - // Returns the most recently observed immutable descriptor. A callback source may - // replace the initially registered UNKNOWN codec/config descriptor with the first - // real frame descriptor without invalidating existing subscriptions. - TrackDescriptorPtr descriptor() const; - - std::optional tryRead(); - std::optional waitRead(std::chrono::milliseconds timeout); - uint64_t discardPendingIfExceeds(size_t maximum_pending_frames); - uint64_t droppedCount() const noexcept; - void reset(); - - private: - friend class MediaSourceHub; - Subscription(std::shared_ptr source, FrameRing::Cursor cursor); - - std::shared_ptr source_; - FrameRing::Cursor cursor_; - bool active_{false}; - }; - - MediaSourceHub(); - ~MediaSourceHub(); - - MediaSourceHub(const MediaSourceHub&) = delete; - MediaSourceHub& operator=(const MediaSourceHub&) = delete; - - bool registerSource( - TrackDescriptorPtr initial_descriptor, - SourceCallbacks callbacks, - size_t ring_capacity = 64); - - // Active sources cannot be unregistered. Destroy/reset their subscriptions first. - bool unregisterSource(const std::string& track_id); - - bool hasSource(const std::string& track_id) const; - std::vector listTracks() const; - size_t subscriberCount(const std::string& track_id) const; - - // Protocol adapters can request an IDR after a discontinuity without knowing the - // concrete camera implementation. Returns false when unsupported or not running. - bool requestKeyFrame(const std::string& track_id) const; - - Subscription subscribe( - const std::string& track_id, - StartPosition start_position = StartPosition::NEXT_PUBLISHED, - CancelPredicate cancelled = {}); - - // Stops all registered sources and invalidates outstanding subscriptions. The - // subscriptions remain destructible and their waitRead calls are awakened. - // A cooperative in-progress start is cancelled; a callback that violates the - // cancellation contract is quarantined with retained state rather than blocking - // shutdown or risking a use-after-free. - void shutdown(); - -private: - struct Impl; - std::shared_ptr impl_; -}; - -// Compatibility alias for older protocol tests and integrations. New code should -// use MediaSourceHub directly. -using MediaSourceManager = MediaSourceHub; - -} // namespace cmvr::media - -#endif // CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H diff --git a/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp b/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp deleted file mode 100644 index d974a1ac..00000000 --- a/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp +++ /dev/null @@ -1,687 +0,0 @@ -#include "manager/media_source_hub/include/device_media_source_adapter.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/base/logging/logger.h" - -namespace cmvr::media { -namespace { - -std::string normalizedCodec(std::string codec) { - codec.erase( - std::remove_if(codec.begin(), codec.end(), [](const unsigned char c) { - return !std::isalnum(c); - }), - codec.end()); - std::transform(codec.begin(), codec.end(), codec.begin(), [](const unsigned char c) { - return static_cast(std::tolower(c)); - }); - return codec; -} - -Codec videoCodec(const std::string& value) { - const std::string codec = normalizedCodec(value); - if (codec == "h264" || codec == "avc" || codec == "avc1" || - codec == "libx264" || codec == "h264qsv") { - return Codec::H264; - } - if (codec == "h265" || codec == "hevc" || codec == "hvc1" || - codec == "libx265" || codec == "h265qsv" || codec == "hevcqsv") { - return Codec::H265; - } - return Codec::UNKNOWN; -} - -PayloadFormat videoPayloadFormat( - const Codec codec, - const std::vector& payload) noexcept { - if (codec != Codec::H264 && codec != Codec::H265) { - return PayloadFormat::UNKNOWN; - } - const bool three_byte_start_code = payload.size() >= 3 && - payload[0] == 0U && payload[1] == 0U && payload[2] == 1U; - const bool four_byte_start_code = payload.size() >= 4 && - payload[0] == 0U && payload[1] == 0U && payload[2] == 0U && payload[3] == 1U; - return three_byte_start_code || four_byte_start_code - ? PayloadFormat::ANNEX_B - : PayloadFormat::UNKNOWN; -} - -Codec audioCodec(const std::string& value) { - const std::string codec = normalizedCodec(value); - if (codec == "opus") return Codec::OPUS; - if (codec == "aac") return Codec::AAC; - if (codec == "pcms16le") return Codec::PCM_S16LE; - return Codec::UNKNOWN; -} - -Codec audioCodec(const device::AudioStreamFrameData& source) { - const Codec codec = audioCodec(source.codec); - if (codec != Codec::UNKNOWN || !normalizedCodec(source.codec).empty()) { - return codec; - } - switch (source.format) { - case device::AudioStreamFormat::PCM: - return Codec::PCM_S16LE; - case device::AudioStreamFormat::AAC: - return Codec::AAC; - case device::AudioStreamFormat::OPUS: - return Codec::OPUS; - default: - return Codec::UNKNOWN; - } -} - -PayloadFormat audioPayloadFormat( - const Codec codec, - const std::vector& payload) noexcept { - switch (codec) { - case Codec::OPUS: - return PayloadFormat::OPUS_PACKET; - case Codec::PCM_S16LE: - return PayloadFormat::RAW; - case Codec::AAC: { - // FFmpeg encoders commonly expose raw AAC access units plus AudioSpecificConfig; - // only advertise ADTS when the sync word and layer bits are actually present. - const bool has_adts_header = payload.size() >= 2 && payload[0] == 0xFFU && - (payload[1] & 0xF6U) == 0xF0U; - return has_adts_header ? PayloadFormat::AAC_ADTS : PayloadFormat::RAW; - } - default: - return PayloadFormat::UNKNOWN; - } -} - -Rational sanitizedTimeBase( - const int32_t numerator, - const int32_t denominator, - const int32_t fallback_denominator) noexcept { - return Rational{ - numerator > 0 ? numerator : 1, - denominator > 0 ? denominator : std::max(1, fallback_denominator)}; -} - -uint64_t descriptorGeneration( - const uint64_t stream_epoch, - const uint32_t codec_generation) { - const uint64_t generation = (stream_epoch << 32U) | codec_generation; - return generation == 0 ? 1 : generation; -} - -template -struct PumpState : public std::enable_shared_from_this> { - explicit PumpState(std::shared_ptr device_ptr) - : device(std::move(device_ptr)) {} - - virtual ~PumpState() { - stop(); - } - - bool begin( - const MediaSourceHub::FrameSink& frame_sink, - const MediaSourceHub::CancelPredicate& cancelled) { - if (!frame_sink || !device) { - return false; - } - const auto cancellation_requested = [&cancelled] { - if (!cancelled) return false; - try { - return cancelled(); - } catch (...) { - return true; - } - }; - if (cancellation_requested()) { - return false; - } - - std::unique_lock lock(mutex); - if (running.load(std::memory_order_acquire)) { - return true; - } - if (worker.joinable()) { - // A previous worker must always be collected before a new capture lease starts. - std::thread stale_worker = std::move(worker); - lock.unlock(); - collectThread(std::move(stale_worker)); - lock.lock(); - } - - sink = frame_sink; - bool streaming_attempted = false; - try { - if (!device->start()) { - sink = {}; - return false; - } - if (cancellation_requested()) { - sink = {}; - return false; - } - streaming_attempted = true; - if (!device->startStreaming()) { - sink = {}; - lock.unlock(); - stopDeviceStreaming(); - return false; - } - streaming_started = true; - if (cancellation_requested()) { - streaming_started = false; - sink = {}; - lock.unlock(); - stopDeviceStreaming(); - return false; - } - running.store(true, std::memory_order_release); - try { - // The worker owns the pump while run() is active. This also - // makes the defensive self-stop/detach path lifetime-safe. - const auto self = this->shared_from_this(); - worker = std::thread([self] { self->run(); }); - } catch (...) { - running.store(false, std::memory_order_release); - streaming_started = false; - sink = {}; - lock.unlock(); - stopDeviceStreaming(); - return false; - } - } catch (const std::exception& error) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to start media source: " - << error.what(); - sink = {}; - lock.unlock(); - if (streaming_attempted) stopDeviceStreaming(); - return false; - } catch (...) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to start media source"; - sink = {}; - lock.unlock(); - if (streaming_attempted) stopDeviceStreaming(); - return false; - } - return true; - } - - void stop() noexcept { - std::thread thread; - bool stop_streaming = false; - { - std::lock_guard lock(mutex); - running.store(false, std::memory_order_release); - stop_streaming = streaming_started; - streaming_started = false; - sink = {}; - thread = std::move(worker); - } - if (stop_streaming) { - stopDeviceStreaming(); - } - if (thread.joinable()) { - collectThread(std::move(thread)); - } - } - - static void collectThread(std::thread thread) noexcept { - if (!thread.joinable()) { - return; - } - try { - if (thread.get_id() == std::this_thread::get_id()) { - thread.detach(); - } else { - thread.join(); - } - } catch (const std::exception& error) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to collect media pump: " - << error.what(); - if (thread.joinable()) { - try { - thread.detach(); - } catch (...) { - // std::thread's destructor would terminate if this extremely rare - // platform error occurred; there is no recoverable ownership path. - } - } - } - } - - virtual void run() = 0; - - void stopDeviceStreaming() noexcept { - try { - if (device) { - device->stopStreaming(); - } - } catch (const std::exception& error) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to stop media source: " - << error.what(); - } catch (...) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to stop media source"; - } - } - - std::shared_ptr device; - std::atomic running{false}; - std::mutex mutex; - std::thread worker; - MediaSourceHub::FrameSink sink; - bool streaming_started{false}; -}; - -struct CameraPump final : PumpState { - CameraPump(std::shared_ptr camera, std::string id) - : PumpState(std::move(camera)), track_id(std::move(id)) {} - ~CameraPump() override { stop(); } - - void run() override { - size_t cursor = 0; - uint64_t last_epoch = 0; - uint64_t last_sequence = 0; - uint64_t cached_config_generation = 0; - std::vector cached_codec_config; - TrackDescriptorPtr last_descriptor; - bool have_previous = false; - bool have_cached_config_generation = false; - bool waiting_for_key_frame = true; - bool pending_discontinuity = true; - bool requested_key_frame = false; - std::chrono::steady_clock::time_point last_key_frame_request; - - const auto request_key_frame = [&] { - const auto now = std::chrono::steady_clock::now(); - if (requested_key_frame && - now - last_key_frame_request < std::chrono::milliseconds(250)) { - return; - } - requested_key_frame = true; - last_key_frame_request = now; - try { - device->requestKeyFrame(); - } catch (...) { - // Unsupported/failed key-frame requests fall back to the encoder's GOP. - } - }; - request_key_frame(); - - while (running.load(std::memory_order_acquire)) { - device::StreamFrameData source; - try { - if (!device->waitEncodedFrame(source, cursor, std::chrono::milliseconds(50))) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - continue; - } - } catch (const std::exception& error) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Camera frame read failed for " - << track_id << ": " << error.what(); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - continue; - } catch (...) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Camera frame read failed for " - << track_id; - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - continue; - } - if (source.rgbFrame.empty()) { - continue; - } - - const uint64_t descriptor_generation = descriptorGeneration( - source.stream_epoch, source.codec_config_generation); - if (!have_cached_config_generation || - cached_config_generation != descriptor_generation) { - cached_codec_config.clear(); - cached_config_generation = descriptor_generation; - have_cached_config_generation = true; - } - if (!source.codec_config.empty()) { - cached_codec_config = source.codec_config; - } - - TrackDescriptor::Config track; - track.id = track_id; - track.source_id = device->id(); - track.kind = MediaKind::VIDEO; - track.codec = videoCodec(source.codec); - track.payload_format = videoPayloadFormat(track.codec, source.rgbFrame); - track.time_base = sanitizedTimeBase( - source.time_base_num, - source.time_base_den, - source.fps); - track.width = static_cast(std::max(0, source.width)); - track.height = static_cast(std::max(0, source.height)); - track.nominal_rate = static_cast(std::max(0, source.fps)); - track.fx = source.intrinsics.fx; - track.fy = source.intrinsics.fy; - track.cx = source.intrinsics.cx; - track.cy = source.intrinsics.cy; - track.distortion.assign( - std::begin(source.intrinsics.coeffs), - std::end(source.intrinsics.coeffs)); - track.generation = descriptor_generation; - track.codec_config = cached_codec_config; - - const bool epoch_changed = have_previous && source.stream_epoch != last_epoch; - const bool sequence_wrapped = have_previous && - last_sequence == std::numeric_limits::max() && source.sequence != 0; - const bool sequence_gap = have_previous && !epoch_changed && - (sequence_wrapped || - (last_sequence != std::numeric_limits::max() && - source.sequence != last_sequence + 1)); - - TrackDescriptorPtr descriptor; - try { - descriptor = makeTrackDescriptor(std::move(track)); - } catch (const std::exception& error) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid camera descriptor for " - << track_id << ": " << error.what(); - pending_discontinuity = true; - last_epoch = source.stream_epoch; - last_sequence = source.sequence; - have_previous = true; - continue; - } - const bool descriptor_changed = last_descriptor && - !equivalentTrackDescriptor(*last_descriptor, *descriptor); - const bool discontinuity = !have_previous || source.discontinuity || epoch_changed || - sequence_gap || descriptor_changed; - const bool inter_frame_codec = descriptor->codec == Codec::H264 || - descriptor->codec == Codec::H265; - - if (discontinuity) { - pending_discontinuity = true; - if (inter_frame_codec) { - waiting_for_key_frame = true; - request_key_frame(); - } else { - waiting_for_key_frame = false; - } - } - - last_epoch = source.stream_epoch; - last_sequence = source.sequence; - last_descriptor = descriptor; - have_previous = true; - - if (waiting_for_key_frame && !source.bKey) { - request_key_frame(); - continue; - } - waiting_for_key_frame = false; - - MediaFrame::Config frame; - frame.descriptor = std::move(descriptor); - frame.payload = std::move(source.rgbFrame); - frame.sequence = source.sequence; - frame.source_timestamp = source.source_timestamp; - frame.source_frame_number = source.source_frame_number; - frame.pts = source.pts; - frame.dts = source.dts; - frame.duration = source.duration; - frame.capture_time_ns = source.capture_monotonic_ns > 0 - ? static_cast(source.capture_monotonic_ns) - : 0; - frame.capture_utc_ns = source.capture_utc_ns; - frame.key_frame = source.bKey; - frame.discontinuity = pending_discontinuity; - - MediaSourceHub::FrameSink current_sink; - { - std::lock_guard lock(mutex); - current_sink = sink; - } - if (current_sink) { - try { - current_sink(makeMediaFrame(std::move(frame))); - pending_discontinuity = false; - } catch (const std::exception& error) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid camera frame for " - << track_id << ": " << error.what(); - pending_discontinuity = true; - } - } - } - } - - std::string track_id; -}; - -struct MicrophonePump final : PumpState { - MicrophonePump(std::shared_ptr microphone, std::string id) - : PumpState(std::move(microphone)), track_id(std::move(id)) {} - ~MicrophonePump() override { stop(); } - - void run() override { - size_t cursor = 0; - uint64_t last_epoch = 0; - uint64_t last_sequence = 0; - uint64_t cached_config_generation = 0; - std::vector cached_codec_config; - TrackDescriptorPtr last_descriptor; - bool have_previous = false; - bool have_cached_config_generation = false; - bool pending_discontinuity = true; - while (running.load(std::memory_order_acquire)) { - device::AudioStreamFrameData source; - try { - if (!device->waitEncodedFrame(source, cursor, std::chrono::milliseconds(50))) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - continue; - } - } catch (const std::exception& error) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Microphone frame read failed for " - << track_id << ": " << error.what(); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - continue; - } catch (...) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Microphone frame read failed for " - << track_id; - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - continue; - } - if (source.data.empty()) { - continue; - } - - const uint64_t descriptor_generation = descriptorGeneration( - source.stream_epoch, source.codec_config_generation); - if (!have_cached_config_generation || - cached_config_generation != descriptor_generation) { - cached_codec_config.clear(); - cached_config_generation = descriptor_generation; - have_cached_config_generation = true; - } - if (!source.codec_config.empty()) { - cached_codec_config = source.codec_config; - } - - TrackDescriptor::Config track; - track.id = track_id; - track.source_id = device->id(); - track.kind = MediaKind::AUDIO; - track.codec = audioCodec(source); - track.payload_format = audioPayloadFormat(track.codec, source.data); - track.time_base = sanitizedTimeBase( - source.time_base_num, - source.time_base_den, - source.sample_rate); - track.sample_rate = static_cast(std::max(0, source.sample_rate)); - track.channels = static_cast(std::max(0, source.channels)); - // Packet sample counts belong to MediaFrame::duration, not immutable track metadata. - track.nominal_rate = 0; - track.generation = descriptor_generation; - track.codec_config = cached_codec_config; - - const bool epoch_changed = have_previous && source.stream_epoch != last_epoch; - const bool sequence_wrapped = have_previous && - last_sequence == std::numeric_limits::max() && source.sequence != 0; - const bool sequence_gap = have_previous && !epoch_changed && - (sequence_wrapped || - (last_sequence != std::numeric_limits::max() && - source.sequence != last_sequence + 1)); - - TrackDescriptorPtr descriptor; - try { - descriptor = makeTrackDescriptor(std::move(track)); - } catch (const std::exception& error) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid microphone descriptor for " - << track_id << ": " << error.what(); - pending_discontinuity = true; - last_epoch = source.stream_epoch; - last_sequence = source.sequence; - have_previous = true; - continue; - } - const bool descriptor_changed = last_descriptor && - !equivalentTrackDescriptor(*last_descriptor, *descriptor); - if (!have_previous || source.discontinuity || epoch_changed || sequence_gap || - descriptor_changed) { - pending_discontinuity = true; - } - - last_epoch = source.stream_epoch; - last_sequence = source.sequence; - last_descriptor = descriptor; - have_previous = true; - - MediaFrame::Config frame; - frame.descriptor = std::move(descriptor); - frame.payload = std::move(source.data); - frame.sequence = source.sequence; - frame.pts = source.pts; - frame.dts = source.dts; - frame.duration = source.duration > 0 ? source.duration : source.nb_samples; - frame.capture_time_ns = source.capture_monotonic_ns > 0 - ? static_cast(source.capture_monotonic_ns) - : 0; - frame.capture_utc_ns = source.capture_utc_ns; - frame.key_frame = true; - frame.discontinuity = pending_discontinuity; - - MediaSourceHub::FrameSink current_sink; - { - std::lock_guard lock(mutex); - current_sink = sink; - } - if (current_sink) { - try { - current_sink(makeMediaFrame(std::move(frame))); - pending_discontinuity = false; - } catch (const std::exception& error) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Invalid microphone frame for " - << track_id << ": " << error.what(); - pending_discontinuity = true; - } - } - } - } - - std::string track_id; -}; - -TrackDescriptorPtr initialTrack( - std::string track_id, - std::string source_id, - const MediaKind kind) { - TrackDescriptor::Config config; - config.id = std::move(track_id); - config.source_id = std::move(source_id); - config.kind = kind; - // A placeholder descriptor is never emitted as a media sample, but it - // still carries a mathematically valid neutral time base. - config.time_base = Rational{1, 1}; - config.generation = 1; - return makeTrackDescriptor(std::move(config)); -} - -} // namespace - -MediaSourceHub& globalMediaSourceHub() { - static MediaSourceHub hub; - return hub; -} - -static std::string cameraColorTrackId(const std::string& device_id) { - return device_id + "/video/color"; -} - -static std::string microphoneTrackId(const std::string& device_id) { - return device_id + "/audio/main"; -} - -bool ensureCameraMediaSource( - MediaSourceHub& hub, - const std::shared_ptr& camera, - const size_t ring_capacity) { - if (!camera || camera->id().empty()) { - return false; - } - const std::string track_id = cameraColorTrackId(camera->id()); - if (hub.hasSource(track_id)) { - return true; - } - - const auto pump = std::make_shared(camera, track_id); - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [pump]( - const MediaSourceHub::FrameSink& sink, - const MediaSourceHub::CancelPredicate& cancelled) { - return pump->begin(sink, cancelled); - }; - callbacks.stop = [pump] { pump->stop(); }; - callbacks.request_key_frame = [camera] { return camera->requestKeyFrame(); }; - const bool registered = hub.registerSource( - initialTrack(track_id, camera->id(), MediaKind::VIDEO), - std::move(callbacks), - ring_capacity); - if (!registered && !hub.hasSource(track_id)) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to register camera track: " << track_id; - return false; - } - return true; -} - -bool ensureMicrophoneMediaSource( - MediaSourceHub& hub, - const std::shared_ptr& microphone, - const size_t ring_capacity) { - if (!microphone || microphone->id().empty()) { - return false; - } - const std::string track_id = microphoneTrackId(microphone->id()); - if (hub.hasSource(track_id)) { - return true; - } - - const auto pump = std::make_shared(microphone, track_id); - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [pump]( - const MediaSourceHub::FrameSink& sink, - const MediaSourceHub::CancelPredicate& cancelled) { - return pump->begin(sink, cancelled); - }; - callbacks.stop = [pump] { pump->stop(); }; - const bool registered = hub.registerSource( - initialTrack(track_id, microphone->id(), MediaKind::AUDIO), - std::move(callbacks), - ring_capacity); - if (!registered && !hub.hasSource(track_id)) { - CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to register microphone track: " << track_id; - return false; - } - return true; -} - -} // namespace cmvr::media diff --git a/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp b/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp deleted file mode 100644 index cace2727..00000000 --- a/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp +++ /dev/null @@ -1,596 +0,0 @@ -#include "manager/media_source_hub/include/media_source_hub.h" - -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::media { - -struct MediaSourceHub::SourceState final : public std::enable_shared_from_this { - enum class Lifecycle { - STOPPED, - STARTING, - RUNNING, - STOPPING - }; - - struct StartAttempt { - size_t waiters{0}; - bool completed{false}; - bool succeeded{false}; - std::atomic cancel_requested{false}; - }; - - SourceState( - TrackDescriptorPtr initial_descriptor, - SourceCallbacks source_callbacks, - const size_t ring_capacity) - : track_id(initial_descriptor->id), - descriptor(std::move(initial_descriptor)), - callbacks(std::move(source_callbacks)), - ring(ring_capacity) {} - - FrameSink makeSink() { - const std::weak_ptr weak_source = shared_from_this(); - return [weak_source](MediaFramePtr frame) { - if (const auto source = weak_source.lock()) { - source->acceptFrame(std::move(frame)); - } - }; - } - - void acceptFrame(MediaFramePtr frame) { - if (!frame || !frame->descriptor || frame->descriptor->id != track_id) { - return; - } - - { - std::lock_guard lock(lifecycle_mutex); - if (!registered || - (lifecycle != Lifecycle::STARTING && lifecycle != Lifecycle::RUNNING)) { - return; - } - } - - const TrackDescriptorPtr current = std::atomic_load(&descriptor); - if (!current || !equivalentTrackDescriptor(*current, *frame->descriptor)) { - // C++17 atomic shared_ptr free functions provide an atomic descriptor snapshot to - // all subscriptions while frames remain immutable. - std::atomic_store(&descriptor, frame->descriptor); - } - ring.publish(std::move(frame)); - } - - static bool isCancelled(const CancelPredicate& cancelled) noexcept { - if (!cancelled) return false; - try { - return cancelled(); - } catch (...) { - return true; - } - } - - bool invokeStart( - const FrameSink& sink, - const CancelPredicate& cancelled) noexcept { - std::lock_guard callback_lock(callback_mutex); - try { - return callbacks.start && callbacks.start(sink, cancelled); - } catch (...) { - return false; - } - } - - void invokeStop() noexcept { - std::lock_guard callback_lock(callback_mutex); - try { - if (callbacks.stop) callbacks.stop(); - } catch (...) { - } - } - - void completeStart( - const std::shared_ptr& attempt, - const bool started) { - bool stop_abandoned_start = false; - { - std::lock_guard lock(lifecycle_mutex); - if (start_attempt != attempt || lifecycle != Lifecycle::STARTING) { - return; - } - attempt->completed = true; - attempt->succeeded = started; - if (started && registered && attempt->waiters != 0U) { - lifecycle = Lifecycle::RUNNING; - } else if (started) { - lifecycle = Lifecycle::STOPPING; - ring.close(); - stop_abandoned_start = true; - } else { - lifecycle = Lifecycle::STOPPED; - } - lifecycle_condition.notify_all(); - } - - if (stop_abandoned_start) { - invokeStop(); - std::lock_guard lock(lifecycle_mutex); - if (lifecycle == Lifecycle::STOPPING) { - lifecycle = Lifecycle::STOPPED; - } - lifecycle_condition.notify_all(); - } - } - - bool acquire( - const StartPosition start_position, - FrameRing::Cursor& cursor, - const CancelPredicate& cancelled) { - std::unique_lock lock(lifecycle_mutex); - while (lifecycle == Lifecycle::STOPPING) { - if (!registered || isCancelled(cancelled)) return false; - lifecycle_condition.wait_for(lock, std::chrono::milliseconds(10)); - } - if (!registered || isCancelled(cancelled)) return false; - - if (lifecycle == Lifecycle::RUNNING) { - ++subscriber_count; - lock.unlock(); - cursor = ring.makeCursor(start_position); - return true; - } - - std::shared_ptr attempt; - if (lifecycle == Lifecycle::STOPPED) { - lifecycle = Lifecycle::STARTING; - ring.reset(); - const FrameSink sink = makeSink(); - attempt = std::make_shared(); - attempt->waiters = 1U; - start_attempt = attempt; - const auto self = shared_from_this(); - try { - std::thread([self, attempt, sink]() { - const CancelPredicate cancelled = [attempt] { - return attempt->cancel_requested.load( - std::memory_order_acquire); - }; - const bool started = self->invokeStart(sink, cancelled); - self->completeStart(attempt, started); - }).detach(); - } catch (...) { - start_attempt.reset(); - lifecycle = Lifecycle::STOPPED; - lifecycle_condition.notify_all(); - return false; - } - } else if (lifecycle == Lifecycle::STARTING) { - attempt = start_attempt; - if (!attempt || attempt->cancel_requested.load(std::memory_order_acquire)) { - return false; - } - ++attempt->waiters; - } else { - return false; - } - - while (registered && !attempt->completed) { - if (isCancelled(cancelled)) { - if (attempt->waiters != 0U) --attempt->waiters; - if (attempt->waiters == 0U) { - attempt->cancel_requested.store(true, std::memory_order_release); - } - lifecycle_condition.notify_all(); - return false; - } - lifecycle_condition.wait_for(lock, std::chrono::milliseconds(10)); - } - - const bool caller_cancelled = isCancelled(cancelled); - const bool acquired = !caller_cancelled && registered && attempt->completed && - attempt->succeeded && - lifecycle == Lifecycle::RUNNING; - if (attempt->waiters != 0U) --attempt->waiters; - if (!acquired) { - if (attempt->waiters == 0U) { - attempt->cancel_requested.store(true, std::memory_order_release); - } - const bool stop_unclaimed_source = - start_attempt == attempt && attempt->waiters == 0U && - subscriber_count == 0U && - lifecycle == Lifecycle::RUNNING; - if (stop_unclaimed_source) { - lifecycle = Lifecycle::STOPPING; - ring.close(); - lock.unlock(); - invokeStop(); - lock.lock(); - if (lifecycle == Lifecycle::STOPPING) { - lifecycle = Lifecycle::STOPPED; - } - lifecycle_condition.notify_all(); - } - return false; - } - ++subscriber_count; - lock.unlock(); - cursor = ring.makeCursor(start_position); - return true; - } - - void release() { - std::unique_lock lock(lifecycle_mutex); - if (subscriber_count == 0) { - return; - } - --subscriber_count; - if (subscriber_count != 0 || lifecycle != Lifecycle::RUNNING) { - return; - } - - lifecycle = Lifecycle::STOPPING; - ring.close(); - lock.unlock(); - invokeStop(); - lock.lock(); - lifecycle = Lifecycle::STOPPED; - lifecycle_condition.notify_all(); - } - - bool deactivateIfUnused() { - std::unique_lock lock(lifecycle_mutex); - if (subscriber_count != 0) { - return false; - } - registered = false; - ring.close(); - if (lifecycle == Lifecycle::STARTING) { - if (start_attempt) { - start_attempt->cancel_requested.store(true, std::memory_order_release); - } - lifecycle_condition.notify_all(); - return true; - } - if (lifecycle == Lifecycle::STOPPING || lifecycle == Lifecycle::STOPPED) { - lifecycle_condition.notify_all(); - return true; - } - - lifecycle = Lifecycle::STOPPING; - lock.unlock(); - invokeStop(); - lock.lock(); - if (lifecycle == Lifecycle::STOPPING) { - lifecycle = Lifecycle::STOPPED; - } - lifecycle_condition.notify_all(); - return true; - } - - void shutdown() { - std::unique_lock lock(lifecycle_mutex); - registered = false; - ring.close(); - if (lifecycle == Lifecycle::STARTING) { - if (start_attempt) { - start_attempt->cancel_requested.store(true, std::memory_order_release); - } - lifecycle_condition.notify_all(); - return; - } - if (lifecycle == Lifecycle::STOPPING) { - lifecycle_condition.notify_all(); - return; - } - if (lifecycle == Lifecycle::STOPPED) { - lifecycle_condition.notify_all(); - return; - } - - lifecycle = Lifecycle::STOPPING; - lock.unlock(); - invokeStop(); - lock.lock(); - if (lifecycle == Lifecycle::STOPPING) { - lifecycle = Lifecycle::STOPPED; - } - lifecycle_condition.notify_all(); - } - - bool validForSubscription() const { - std::lock_guard lock(lifecycle_mutex); - return registered && lifecycle == Lifecycle::RUNNING; - } - - size_t subscriberCount() const { - std::lock_guard lock(lifecycle_mutex); - return subscriber_count; - } - - bool requestKeyFrame() const { - // Serialize with stop first, then re-check lifecycle. A stop that has - // already begun rejects the request; a stop that begins afterwards - // waits for this callback before invoking the device stop barrier. - std::lock_guard callback_lock(callback_mutex); - { - std::lock_guard lock(lifecycle_mutex); - if (!registered || lifecycle != Lifecycle::RUNNING || !callbacks.request_key_frame) { - return false; - } - } - try { - return callbacks.request_key_frame(); - } catch (...) { - return false; - } - } - - TrackDescriptorPtr currentDescriptor() const { - return std::atomic_load(&descriptor); - } - - const std::string track_id; - mutable TrackDescriptorPtr descriptor; - const SourceCallbacks callbacks; - FrameRing ring; - - mutable std::mutex callback_mutex; - mutable std::mutex lifecycle_mutex; - std::condition_variable lifecycle_condition; - Lifecycle lifecycle{Lifecycle::STOPPED}; - size_t subscriber_count{0}; - bool registered{true}; - std::shared_ptr start_attempt; -}; - -struct MediaSourceHub::Impl final { - mutable std::mutex mutex; - std::unordered_map> sources; -}; - -MediaSourceHub::Subscription::Subscription( - std::shared_ptr source, - FrameRing::Cursor cursor) - : source_(std::move(source)), - cursor_(std::move(cursor)), - active_(static_cast(source_)) {} - -MediaSourceHub::Subscription::~Subscription() { - reset(); -} - -MediaSourceHub::Subscription::Subscription(Subscription&& other) noexcept - : source_(std::move(other.source_)), - cursor_(other.cursor_), - active_(other.active_) { - other.active_ = false; -} - -MediaSourceHub::Subscription& MediaSourceHub::Subscription::operator=(Subscription&& other) noexcept { - if (this == &other) { - return *this; - } - reset(); - source_ = std::move(other.source_); - cursor_ = other.cursor_; - active_ = other.active_; - other.active_ = false; - return *this; -} - -bool MediaSourceHub::Subscription::valid() const { - return active_ && source_ && source_->validForSubscription(); -} - -TrackDescriptorPtr MediaSourceHub::Subscription::descriptor() const { - return source_ ? source_->currentDescriptor() : nullptr; -} - -std::optional MediaSourceHub::Subscription::tryRead() { - if (!active_ || !source_) { - return std::nullopt; - } - return source_->ring.tryRead(cursor_); -} - -std::optional MediaSourceHub::Subscription::waitRead( - const std::chrono::milliseconds timeout) { - if (!active_ || !source_) { - return std::nullopt; - } - return source_->ring.waitRead(cursor_, timeout); -} - -uint64_t MediaSourceHub::Subscription::discardPendingIfExceeds( - const size_t maximum_pending_frames) { - if (!active_ || !source_) { - return 0; - } - return source_->ring.discardPendingIfExceeds(cursor_, maximum_pending_frames); -} - -uint64_t MediaSourceHub::Subscription::droppedCount() const noexcept { - return cursor_.dropped_count; -} - -void MediaSourceHub::Subscription::reset() { - if (active_ && source_) { - source_->release(); - } - active_ = false; - source_.reset(); -} - -MediaSourceHub::MediaSourceHub() - : impl_(std::make_shared()) {} - -MediaSourceHub::~MediaSourceHub() { - shutdown(); -} - -bool MediaSourceHub::registerSource( - TrackDescriptorPtr initial_descriptor, - SourceCallbacks callbacks, - const size_t ring_capacity) { - if (!impl_ || !initial_descriptor || initial_descriptor->id.empty() || - !callbacks.start || ring_capacity == 0) { - return false; - } - - std::shared_ptr source; - try { - source = std::make_shared( - std::move(initial_descriptor), std::move(callbacks), ring_capacity); - } catch (...) { - return false; - } - - std::lock_guard lock(impl_->mutex); - return impl_->sources.emplace(source->track_id, std::move(source)).second; -} - -bool MediaSourceHub::unregisterSource(const std::string& track_id) { - if (!impl_ || track_id.empty()) { - return false; - } - - std::shared_ptr source; - { - std::lock_guard lock(impl_->mutex); - const auto it = impl_->sources.find(track_id); - if (it == impl_->sources.end()) { - return false; - } - source = it->second; - } - - if (!source->deactivateIfUnused()) { - return false; - } - - std::lock_guard lock(impl_->mutex); - const auto it = impl_->sources.find(track_id); - if (it != impl_->sources.end() && it->second == source) { - impl_->sources.erase(it); - return true; - } - return false; -} - -bool MediaSourceHub::hasSource(const std::string& track_id) const { - if (!impl_) { - return false; - } - std::lock_guard lock(impl_->mutex); - return impl_->sources.find(track_id) != impl_->sources.end(); -} - -std::vector MediaSourceHub::listTracks() const { - std::vector> sources; - if (!impl_) { - return {}; - } - { - std::lock_guard lock(impl_->mutex); - sources.reserve(impl_->sources.size()); - for (const auto& [track_id, source] : impl_->sources) { - (void)track_id; - sources.push_back(source); - } - } - - std::vector descriptors; - descriptors.reserve(sources.size()); - for (const auto& source : sources) { - descriptors.push_back(source->currentDescriptor()); - } - std::sort(descriptors.begin(), descriptors.end(), [](const auto& lhs, const auto& rhs) { - if (!lhs) return static_cast(rhs); - if (!rhs) return false; - return lhs->id < rhs->id; - }); - return descriptors; -} - -size_t MediaSourceHub::subscriberCount(const std::string& track_id) const { - if (!impl_) { - return 0; - } - std::shared_ptr source; - { - std::lock_guard lock(impl_->mutex); - const auto it = impl_->sources.find(track_id); - if (it == impl_->sources.end()) { - return 0; - } - source = it->second; - } - return source->subscriberCount(); -} - -bool MediaSourceHub::requestKeyFrame(const std::string& track_id) const { - if (!impl_) { - return false; - } - std::shared_ptr source; - { - std::lock_guard lock(impl_->mutex); - const auto it = impl_->sources.find(track_id); - if (it == impl_->sources.end()) { - return false; - } - source = it->second; - } - return source->requestKeyFrame(); -} - -MediaSourceHub::Subscription MediaSourceHub::subscribe( - const std::string& track_id, - const StartPosition start_position, - CancelPredicate cancelled) { - if (!impl_) { - return {}; - } - - std::shared_ptr source; - { - std::lock_guard lock(impl_->mutex); - const auto it = impl_->sources.find(track_id); - if (it == impl_->sources.end()) { - return {}; - } - source = it->second; - } - - FrameRing::Cursor cursor; - if (!source->acquire(start_position, cursor, cancelled)) { - return {}; - } - return Subscription(std::move(source), std::move(cursor)); -} - -void MediaSourceHub::shutdown() { - if (!impl_) { - return; - } - - std::vector> sources; - { - std::lock_guard lock(impl_->mutex); - sources.reserve(impl_->sources.size()); - for (auto& [track_id, source] : impl_->sources) { - (void)track_id; - sources.push_back(std::move(source)); - } - impl_->sources.clear(); - } - for (const auto& source : sources) { - source->shutdown(); - } -} - -} // namespace cmvr::media diff --git a/cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp b/cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp deleted file mode 100644 index b96cea9b..00000000 --- a/cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp +++ /dev/null @@ -1,683 +0,0 @@ -#include "manager/media_source_hub/include/media_source_hub.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace { - -using namespace std::chrono_literals; -using cmvr::media::Codec; -using cmvr::media::MediaFrame; -using cmvr::media::MediaFramePtr; -using cmvr::media::MediaKind; -using cmvr::media::MediaSourceHub; -using cmvr::media::PayloadFormat; -using cmvr::media::Rational; -using cmvr::media::TrackDescriptor; -using cmvr::media::TrackDescriptorPtr; - -static_assert(!std::is_copy_assignable::value, "MediaFrame must be immutable"); -static_assert(!std::is_copy_assignable::value, "TrackDescriptor must be immutable"); -static_assert(std::is_same::value, - "MediaFramePtr must share const frames"); - -int failures = 0; - -#define CHECK_TRUE(expression) \ - do { \ - if (!(expression)) { \ - std::cerr << __FILE__ << ':' << __LINE__ << " check failed: " #expression << '\n'; \ - ++failures; \ - } \ - } while (false) - -TrackDescriptorPtr makeVideoDescriptor( - const Codec codec, - const uint64_t generation, - std::vector codec_config = {}) { - TrackDescriptor::Config config; - config.id = "camera.front.video"; - config.source_id = "camera.front"; - config.kind = MediaKind::VIDEO; - config.codec = codec; - config.payload_format = codec == Codec::UNKNOWN ? PayloadFormat::UNKNOWN : PayloadFormat::ANNEX_B; - config.time_base = Rational{1, 90000}; - config.width = 640; - config.height = 360; - config.nominal_rate = 30; - config.generation = generation; - config.codec_config = std::move(codec_config); - return cmvr::media::makeTrackDescriptor(std::move(config)); -} - -MediaFramePtr makeFrame( - TrackDescriptorPtr descriptor, - const uint64_t sequence, - const uint8_t marker) { - MediaFrame::Config config; - config.descriptor = std::move(descriptor); - config.payload = {marker, static_cast(marker + 1)}; - config.sequence = sequence; - config.pts = static_cast(sequence * 3000); - config.dts = config.pts; - config.duration = 3000; - config.capture_time_ns = sequence * 1000000; - config.capture_utc_ns = 1700000000000000000LL + static_cast(sequence); - config.key_frame = sequence == 0; - return cmvr::media::makeMediaFrame(std::move(config)); -} - -void testMediaMetadataValidation() { - TrackDescriptor::Config invalid; - invalid.id = "invalid.video"; - invalid.source_id = "invalid"; - invalid.kind = MediaKind::VIDEO; - invalid.time_base = Rational{0, 1}; - bool rejected = false; - try { - (void)cmvr::media::makeTrackDescriptor(std::move(invalid)); - } catch (const std::invalid_argument&) { - rejected = true; - } - CHECK_TRUE(rejected); - - const auto frame = makeFrame(makeVideoDescriptor(Codec::H264, 1), 7, 1); - CHECK_TRUE(frame->capture_time_ns == 7000000); - CHECK_TRUE(frame->capture_utc_ns == 1700000000000000007LL); - CHECK_TRUE(frame->duration == 3000); -} - -void testLegacySpmcCompatibility() { - bool ring_zero_capacity_rejected = false; - try { - RingBuffer invalid_ring(0); - } catch (const std::invalid_argument&) { - ring_zero_capacity_rejected = true; - } - CHECK_TRUE(ring_zero_capacity_rejected); - - bool spmc_zero_capacity_rejected = false; - try { - SPMCRingBuffer invalid_ring(0); - } catch (const std::invalid_argument&) { - spmc_zero_capacity_rejected = true; - } - CHECK_TRUE(spmc_zero_capacity_rejected); - - SPMCRingBuffer ring(2); - ring.push(10); - ring.push(20); - CHECK_TRUE(ring.size() == 2); - CHECK_TRUE(ring.getHead() == 2); - CHECK_TRUE(ring.getTail() == 0); - CHECK_TRUE(ring.getLast().has_value() && *ring.getLast() == 20); - - size_t reader = 0; - CHECK_TRUE(ring.pop(reader).has_value()); - ring.push(30); - ring.push(40); - CHECK_TRUE(!ring.pop(reader).has_value()); - CHECK_TRUE(reader == ring.getTail()); - CHECK_TRUE(ring.pop(reader).has_value()); - - ring.clear(); - CHECK_TRUE(ring.empty()); - CHECK_TRUE(ring.getHead() == 4); - ring.push(50); - CHECK_TRUE(!ring.pop(reader).has_value()); - const auto after_clear = ring.pop(reader); - CHECK_TRUE(after_clear.has_value() && *after_clear == 50); -} - -void testBroadcastFrameRing() { - using Ring = BroadcastFrameRing; - Ring ring(2); - const auto descriptor = makeVideoDescriptor(Codec::H264, 1, {1, 2, 3}); - auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE); - - CHECK_TRUE(ring.publish(makeFrame(descriptor, 0, 10)).value() == 0); - CHECK_TRUE(ring.publish(makeFrame(descriptor, 1, 20)).value() == 1); - CHECK_TRUE(ring.publish(makeFrame(descriptor, 2, 30)).value() == 2); - - const auto first = ring.tryRead(cursor); - CHECK_TRUE(first.has_value()); - CHECK_TRUE(first->sequence == 1); - CHECK_TRUE(first->value->sequence == 1); - CHECK_TRUE(first->dropped_count == 1); - CHECK_TRUE(first->dropped_since_last_read == 1); - CHECK_TRUE(ring.stats().dropped_count == 1); - - const auto second = ring.tryRead(cursor); - CHECK_TRUE(second.has_value() && second->sequence == 2); - CHECK_TRUE(second->dropped_since_last_read == 0); - - auto waiting_cursor = ring.makeCursor(Ring::StartPosition::NEXT_PUBLISHED); - auto waiting_read = std::async(std::launch::async, [&ring, &waiting_cursor] { - return ring.waitRead(waiting_cursor, 1s); - }); - std::this_thread::sleep_for(10ms); - ring.publish(makeFrame(descriptor, 3, 40)); - CHECK_TRUE(waiting_read.wait_for(500ms) == std::future_status::ready); - CHECK_TRUE(waiting_read.get().has_value()); - - const uint64_t next_generation = ring.reset(); - CHECK_TRUE(next_generation == 2); - ring.publish(makeFrame(descriptor, 4, 50)); - const auto after_reset = ring.tryRead(cursor); - CHECK_TRUE(after_reset.has_value()); - CHECK_TRUE(after_reset->generation == 2); - CHECK_TRUE(after_reset->sequence == 0); - CHECK_TRUE(after_reset->generation_changed); - - auto close_cursor = ring.makeCursor(Ring::StartPosition::NEXT_PUBLISHED); - auto close_wait = std::async(std::launch::async, [&ring, &close_cursor] { - return ring.waitRead(close_cursor, 2s); - }); - ring.close(); - CHECK_TRUE(close_wait.wait_for(500ms) == std::future_status::ready); - CHECK_TRUE(!close_wait.get().has_value()); - CHECK_TRUE(!ring.publish(makeFrame(descriptor, 5, 60)).has_value()); -} - -void testBroadcastDiscardPending() { - using Ring = BroadcastFrameRing; - const auto descriptor = makeVideoDescriptor(Codec::H264, 1, {1, 2, 3}); - - { - Ring ring(8); - auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE); - ring.publish(makeFrame(descriptor, 0, 10)); - ring.publish(makeFrame(descriptor, 1, 20)); - - CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 2) == 0); - const auto first = ring.tryRead(cursor); - CHECK_TRUE(first.has_value()); - CHECK_TRUE(first->sequence == 0); - CHECK_TRUE(first->dropped_since_last_read == 0); - } - - { - Ring ring(8); - auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE); - ring.publish(makeFrame(descriptor, 0, 10)); - ring.publish(makeFrame(descriptor, 1, 20)); - ring.publish(makeFrame(descriptor, 2, 30)); - - CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 2) == 3); - CHECK_TRUE(!ring.tryRead(cursor).has_value()); - - ring.publish(makeFrame(descriptor, 3, 40)); - const auto after_discard = ring.tryRead(cursor); - CHECK_TRUE(after_discard.has_value()); - CHECK_TRUE(after_discard->sequence == 3); - CHECK_TRUE(after_discard->dropped_count == 3); - CHECK_TRUE(after_discard->dropped_since_last_read == 3); - } - - { - // Two frames are overwritten before the explicit three-frame discard. - // Both kinds of loss must be reported by the next successful read. - Ring ring(3); - auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE); - for (uint64_t sequence = 0; sequence < 5; ++sequence) { - ring.publish(makeFrame( - descriptor, - sequence, - static_cast(sequence))); - } - - CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 2) == 3); - CHECK_TRUE(cursor.dropped_count == 5); - ring.publish(makeFrame(descriptor, 5, 50)); - const auto after_overwrite_and_discard = ring.tryRead(cursor); - CHECK_TRUE(after_overwrite_and_discard.has_value()); - CHECK_TRUE(after_overwrite_and_discard->sequence == 5); - CHECK_TRUE(after_overwrite_and_discard->dropped_count == 5); - CHECK_TRUE(after_overwrite_and_discard->dropped_since_last_read == 5); - } - - { - // An old-generation OLDEST_AVAILABLE cursor adopts the reset generation - // before deciding whether that generation's pending frames are excessive. - Ring ring(4); - auto cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE); - ring.publish(makeFrame(descriptor, 0, 10)); - ring.reset(); - ring.publish(makeFrame(descriptor, 1, 20)); - ring.publish(makeFrame(descriptor, 2, 30)); - - CHECK_TRUE(ring.discardPendingIfExceeds(cursor, 1) == 2); - ring.publish(makeFrame(descriptor, 3, 40)); - const auto after_reset = ring.tryRead(cursor); - CHECK_TRUE(after_reset.has_value()); - CHECK_TRUE(after_reset->generation == 2); - CHECK_TRUE(after_reset->sequence == 2); - CHECK_TRUE(after_reset->generation_changed); - CHECK_TRUE(after_reset->dropped_since_last_read == 2); - } -} - -void testBroadcastDiscardConcurrentPublish() { - using Ring = BroadcastFrameRing; - constexpr uint64_t frame_count = 4000; - Ring ring(64); - auto cursor = ring.makeCursor(Ring::StartPosition::NEXT_PUBLISHED); - std::atomic start{false}; - std::atomic publisher_done{false}; - - std::thread publisher([&] { - while (!start.load(std::memory_order_acquire)) { - std::this_thread::yield(); - } - for (uint64_t sequence = 0; sequence < frame_count; ++sequence) { - ring.publish(std::make_shared(static_cast(sequence))); - if ((sequence & 7U) == 0U) { - std::this_thread::yield(); - } - } - publisher_done.store(true, std::memory_order_release); - }); - - uint64_t read_count = 0; - uint64_t actively_discarded = 0; - start.store(true, std::memory_order_release); - while (true) { - actively_discarded += ring.discardPendingIfExceeds(cursor, 8); - if (ring.tryRead(cursor)) { - ++read_count; - continue; - } - if (publisher_done.load(std::memory_order_acquire)) { - actively_discarded += ring.discardPendingIfExceeds(cursor, 8); - if (ring.tryRead(cursor)) { - ++read_count; - continue; - } - break; - } - std::this_thread::yield(); - } - publisher.join(); - - CHECK_TRUE(read_count + cursor.dropped_count == frame_count); - CHECK_TRUE(actively_discarded <= cursor.dropped_count); -} - -void testBroadcastConcurrency() { - using Ring = BroadcastFrameRing; - constexpr uint64_t frame_count = 500; - Ring ring(frame_count); - const auto descriptor = makeVideoDescriptor(Codec::H264, 1, {1, 2, 3}); - auto first_cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE); - auto second_cursor = ring.makeCursor(Ring::StartPosition::OLDEST_AVAILABLE); - - auto consume = [&ring](Ring::Cursor& cursor) { - uint64_t expected = 0; - while (expected < frame_count) { - const auto result = ring.waitRead(cursor, 1s); - if (!result || result->sequence != expected || result->value->sequence != expected) { - return false; - } - ++expected; - } - return cursor.dropped_count == 0; - }; - - auto first_consumer = std::async(std::launch::async, consume, std::ref(first_cursor)); - auto second_consumer = std::async(std::launch::async, consume, std::ref(second_cursor)); - std::thread producer([&ring, &descriptor] { - for (uint64_t sequence = 0; sequence < frame_count; ++sequence) { - ring.publish(makeFrame(descriptor, sequence, static_cast(sequence))); - } - }); - - producer.join(); - CHECK_TRUE(first_consumer.get()); - CHECK_TRUE(second_consumer.get()); -} - -void testHubLifecycleAndDescriptorRefresh() { - MediaSourceHub hub; - const auto initial_descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); - std::atomic start_count{0}; - std::atomic stop_count{0}; - std::atomic key_frame_requests{0}; - std::mutex sink_mutex; - MediaSourceHub::FrameSink sink; - - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceHub::FrameSink& callback_sink, - const MediaSourceHub::CancelPredicate&) { - { - std::lock_guard lock(sink_mutex); - sink = callback_sink; - } - ++start_count; - return true; - }; - callbacks.stop = [&] { - ++stop_count; - std::lock_guard lock(sink_mutex); - sink = {}; - }; - callbacks.request_key_frame = [&] { - ++key_frame_requests; - return true; - }; - - CHECK_TRUE(hub.registerSource(initial_descriptor, callbacks, 4)); - CHECK_TRUE(!hub.registerSource(initial_descriptor, callbacks, 4)); - CHECK_TRUE(hub.hasSource(initial_descriptor->id)); - CHECK_TRUE(hub.listTracks().size() == 1); - - auto first = hub.subscribe(initial_descriptor->id); - auto second = hub.subscribe(initial_descriptor->id); - CHECK_TRUE(first.valid() && second.valid()); - CHECK_TRUE(hub.requestKeyFrame(initial_descriptor->id)); - CHECK_TRUE(key_frame_requests == 1); - CHECK_TRUE(start_count == 1); - CHECK_TRUE(hub.subscriberCount(initial_descriptor->id) == 2); - CHECK_TRUE(first.descriptor()->codec == Codec::UNKNOWN); - CHECK_TRUE(!hub.unregisterSource(initial_descriptor->id)); - - // Content changes at the same generation must atomically replace the initial descriptor. - const auto actual_descriptor = makeVideoDescriptor(Codec::H264, 1, {0, 0, 0, 1, 0x67}); - MediaSourceHub::FrameSink producer; - { - std::lock_guard lock(sink_mutex); - producer = sink; - } - CHECK_TRUE(static_cast(producer)); - const auto shared_frame = makeFrame(actual_descriptor, 0, 70); - producer(shared_frame); - - const auto first_read = first.waitRead(500ms); - const auto second_read = second.waitRead(500ms); - CHECK_TRUE(first_read.has_value() && first_read->value == shared_frame); - CHECK_TRUE(second_read.has_value() && second_read->value == shared_frame); - CHECK_TRUE(first.descriptor() == actual_descriptor); - CHECK_TRUE(second.descriptor()->codec_config == actual_descriptor->codec_config); - - first.reset(); - CHECK_TRUE(stop_count == 0); - CHECK_TRUE(hub.subscriberCount(initial_descriptor->id) == 1); - second.reset(); - CHECK_TRUE(stop_count == 1); - CHECK_TRUE(!hub.requestKeyFrame(initial_descriptor->id)); - CHECK_TRUE(hub.subscriberCount(initial_descriptor->id) == 0); - - { - auto restarted = hub.subscribe(initial_descriptor->id); - CHECK_TRUE(restarted.valid()); - CHECK_TRUE(start_count == 2); - } - CHECK_TRUE(stop_count == 2); - CHECK_TRUE(hub.unregisterSource(initial_descriptor->id)); - CHECK_TRUE(!hub.hasSource(initial_descriptor->id)); -} - -void testSubscriptionDiscardPending() { - MediaSourceHub hub; - const auto descriptor = makeVideoDescriptor(Codec::H264, 1); - std::mutex sink_mutex; - MediaSourceHub::FrameSink sink; - - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceHub::FrameSink& callback_sink, - const MediaSourceHub::CancelPredicate&) { - std::lock_guard lock(sink_mutex); - sink = callback_sink; - return true; - }; - callbacks.stop = [&] { - std::lock_guard lock(sink_mutex); - sink = {}; - }; - - CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 8)); - auto subscription = hub.subscribe(descriptor->id); - CHECK_TRUE(subscription.valid()); - - MediaSourceHub::FrameSink producer; - { - std::lock_guard lock(sink_mutex); - producer = sink; - } - CHECK_TRUE(static_cast(producer)); - producer(makeFrame(descriptor, 0, 10)); - producer(makeFrame(descriptor, 1, 20)); - producer(makeFrame(descriptor, 2, 30)); - - CHECK_TRUE(subscription.discardPendingIfExceeds(2) == 3); - CHECK_TRUE(!subscription.tryRead().has_value()); - producer(makeFrame(descriptor, 3, 40)); - const auto next = subscription.tryRead(); - CHECK_TRUE(next.has_value()); - CHECK_TRUE(next->value->sequence == 3); - CHECK_TRUE(next->dropped_since_last_read == 3); - CHECK_TRUE(subscription.droppedCount() == 3); -} - -void testHubFailedStartAndShutdown() { - MediaSourceHub hub; - const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); - std::atomic start_attempts{0}; - std::atomic retry_stop_count{0}; - MediaSourceHub::SourceCallbacks failed_callbacks; - failed_callbacks.start = [&](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { - return ++start_attempts >= 2; - }; - failed_callbacks.stop = [&] { ++retry_stop_count; }; - CHECK_TRUE(hub.registerSource(descriptor, std::move(failed_callbacks), 2)); - auto failed = hub.subscribe(descriptor->id); - CHECK_TRUE(!failed.valid()); - auto retry = hub.subscribe(descriptor->id); - CHECK_TRUE(retry.valid()); - retry.reset(); - CHECK_TRUE(start_attempts == 2); - CHECK_TRUE(retry_stop_count == 1); - CHECK_TRUE(hub.unregisterSource(descriptor->id)); - - std::atomic stop_count{0}; - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { - return true; - }; - callbacks.stop = [&] { ++stop_count; }; - CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2)); - auto live = hub.subscribe(descriptor->id); - CHECK_TRUE(live.valid()); - hub.shutdown(); - CHECK_TRUE(stop_count == 1); - CHECK_TRUE(!live.valid()); - CHECK_TRUE(!live.waitRead(50ms).has_value()); -} - -void testKeyFrameRequestIsOrderedBeforeStop() { - MediaSourceHub hub; - const auto descriptor = makeVideoDescriptor(Codec::H264, 1); - std::atomic key_frame_entered{false}; - std::atomic release_key_frame{false}; - std::atomic stop_count{0}; - - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { - return true; - }; - callbacks.stop = [&] { ++stop_count; }; - callbacks.request_key_frame = [&] { - key_frame_entered.store(true, std::memory_order_release); - while (!release_key_frame.load(std::memory_order_acquire)) { - std::this_thread::sleep_for(1ms); - } - return true; - }; - CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2)); - auto subscription = hub.subscribe(descriptor->id); - CHECK_TRUE(subscription.valid()); - - auto key_frame = std::async(std::launch::async, [&] { - return hub.requestKeyFrame(descriptor->id); - }); - const auto enter_deadline = std::chrono::steady_clock::now() + 500ms; - while (!key_frame_entered.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < enter_deadline) { - std::this_thread::sleep_for(1ms); - } - CHECK_TRUE(key_frame_entered.load(std::memory_order_acquire)); - - auto stop = std::async(std::launch::async, [&] { subscription.reset(); }); - CHECK_TRUE(stop.wait_for(20ms) == std::future_status::timeout); - CHECK_TRUE(stop_count.load(std::memory_order_acquire) == 0); - - release_key_frame.store(true, std::memory_order_release); - CHECK_TRUE(key_frame.get()); - CHECK_TRUE(stop.wait_for(500ms) == std::future_status::ready); - stop.get(); - CHECK_TRUE(stop_count.load(std::memory_order_acquire) == 1); - CHECK_TRUE(!hub.requestKeyFrame(descriptor->id)); -} - -void testHubCancelsBlockedStartWithoutBlockingShutdown() { - MediaSourceHub hub; - const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); - std::atomic start_entered{false}; - std::atomic start_exited{false}; - - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate& cancelled) { - start_entered.store(true, std::memory_order_release); - while (!cancelled()) { - std::this_thread::sleep_for(2ms); - } - start_exited.store(true, std::memory_order_release); - return false; - }; - CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2)); - - auto subscription_future = std::async(std::launch::async, [&] { - return hub.subscribe(descriptor->id); - }); - const auto start_deadline = std::chrono::steady_clock::now() + 500ms; - while (!start_entered.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < start_deadline) { - std::this_thread::sleep_for(2ms); - } - - auto shutdown_future = std::async(std::launch::async, [&] { hub.shutdown(); }); - const bool shutdown_completed = shutdown_future.wait_for(500ms) == - std::future_status::ready; - if (shutdown_completed) shutdown_future.get(); - const bool subscribe_completed = subscription_future.wait_for(500ms) == - std::future_status::ready; - bool invalid_subscription = false; - if (subscribe_completed) { - invalid_subscription = !subscription_future.get().valid(); - } - const auto exit_deadline = std::chrono::steady_clock::now() + 500ms; - while (!start_exited.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < exit_deadline) { - std::this_thread::sleep_for(2ms); - } - - CHECK_TRUE(start_entered.load(std::memory_order_acquire)); - CHECK_TRUE(shutdown_completed); - CHECK_TRUE(subscribe_completed); - CHECK_TRUE(invalid_subscription); - CHECK_TRUE(start_exited.load(std::memory_order_acquire)); -} - -void testHubQuarantinesNonCooperativeStart() { - MediaSourceHub hub; - const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); - std::atomic start_entered{false}; - std::atomic release_start{false}; - std::atomic start_exited{false}; - std::atomic stop_count{0}; - - MediaSourceHub::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceHub::FrameSink&, - const MediaSourceHub::CancelPredicate&) { - start_entered.store(true, std::memory_order_release); - while (!release_start.load(std::memory_order_acquire)) { - std::this_thread::sleep_for(2ms); - } - start_exited.store(true, std::memory_order_release); - return true; - }; - callbacks.stop = [&] { ++stop_count; }; - CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2)); - - auto subscription_future = std::async(std::launch::async, [&] { - return hub.subscribe(descriptor->id); - }); - const auto start_deadline = std::chrono::steady_clock::now() + 500ms; - while (!start_entered.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < start_deadline) { - std::this_thread::sleep_for(2ms); - } - - const auto shutdown_started = std::chrono::steady_clock::now(); - hub.shutdown(); - const bool shutdown_was_bounded = - std::chrono::steady_clock::now() - shutdown_started < 500ms; - const bool subscribe_completed = subscription_future.wait_for(500ms) == - std::future_status::ready; - bool invalid_subscription = false; - if (subscribe_completed) { - invalid_subscription = !subscription_future.get().valid(); - } - - // Release the deliberately non-cooperative test callback before its stack - // captures go out of scope. Late successful startup must be stopped once. - release_start.store(true, std::memory_order_release); - const auto exit_deadline = std::chrono::steady_clock::now() + 500ms; - while ((!start_exited.load(std::memory_order_acquire) || stop_count.load() != 1) && - std::chrono::steady_clock::now() < exit_deadline) { - std::this_thread::sleep_for(2ms); - } - - CHECK_TRUE(start_entered.load(std::memory_order_acquire)); - CHECK_TRUE(shutdown_was_bounded); - CHECK_TRUE(subscribe_completed); - CHECK_TRUE(invalid_subscription); - CHECK_TRUE(start_exited.load(std::memory_order_acquire)); - CHECK_TRUE(stop_count.load() == 1); -} - -} // namespace - -int main() { - testMediaMetadataValidation(); - testLegacySpmcCompatibility(); - testBroadcastFrameRing(); - testBroadcastDiscardPending(); - testBroadcastDiscardConcurrentPublish(); - testBroadcastConcurrency(); - testHubLifecycleAndDescriptorRefresh(); - testSubscriptionDiscardPending(); - testHubFailedStartAndShutdown(); - testKeyFrameRequestIsOrderedBeforeStop(); - testHubCancelsBlockedStartWithoutBlockingShutdown(); - testHubQuarantinesNonCooperativeStart(); - - if (failures != 0) { - std::cerr << failures << " media_source_hub checks failed\n"; - return 1; - } - std::cout << "media_source_hub self-test passed\n"; - return 0; -} diff --git a/cmvr-es/manager/task_manager/CMakeLists.txt b/cmvr-es/manager/task_manager/CMakeLists.txt index 40b1eb48..bbc08858 100644 --- a/cmvr-es/manager/task_manager/CMakeLists.txt +++ b/cmvr-es/manager/task_manager/CMakeLists.txt @@ -11,6 +11,7 @@ target_link_libraries(task_manager PRIVATE cmvr_es::common cmvr_es::device_manager + cmvr_es::stop_all_admission_gate ) add_library(cmvr_es::task_manager ALIAS task_manager) diff --git a/cmvr-es/manager/task_manager/include/task_manager.h b/cmvr-es/manager/task_manager/include/task_manager.h index b1d0609f..aa80e503 100644 --- a/cmvr-es/manager/task_manager/include/task_manager.h +++ b/cmvr-es/manager/task_manager/include/task_manager.h @@ -8,6 +8,7 @@ #include #include #include +#include #include "task/task.h" #include "task/touch_screen_task/include/touch_screen_task.h" @@ -23,21 +24,31 @@ namespace cmvr::task { static TaskManager& getInstance(const config::TaskManagerConfig& cfg); static TaskManager& getInstance(); static void destroyInstance(); + static std::vector> + activitySnapshotIfInitialized(); + static bool stopAllActivitiesIfInitialized( + std::vector* failures = nullptr); ~TaskManager(); - void startRunTask(double control_period_s = 0.001); + bool startRunTask(double control_period_s = 0.001); void stopRunTask(); bool running() const { return running_.load(); } + bool initialized() const noexcept { return initialized_; } std::shared_ptr getTask(const std::string& task_id) const; std::shared_ptr getTouchScreenTask(const std::string& task_id = "touch_screen") const; + // Stops command-driven operational activity without stopping the + // scheduler or destroying task/device lifecycle state. + bool stopAllActivities(std::vector* failures = nullptr); + std::vector> activitySnapshot() const; + private: explicit TaskManager(const config::TaskManagerConfig& cfg); void logTaskPlan() const; - void initTasks(); + bool initTasks(); void runTaskLoop(double control_period_s); static TaskRunMode toTaskRunMode(config::TaskConfigEntry::TaskRunMode run_mode); @@ -49,8 +60,10 @@ namespace cmvr::task { std::unordered_map task_period_s_; std::unordered_map next_step_time_; mutable std::mutex tasks_mutex_; + std::mutex lifecycle_mutex_; std::atomic running_{false}; std::thread run_thread_; + bool initialized_{false}; }; } // namespace cmvr::task diff --git a/cmvr-es/manager/task_manager/src/task_manager.cpp b/cmvr-es/manager/task_manager/src/task_manager.cpp index a8af771c..12e0a53b 100644 --- a/cmvr-es/manager/task_manager/src/task_manager.cpp +++ b/cmvr-es/manager/task_manager/src/task_manager.cpp @@ -1,5 +1,6 @@ #include "manager/task_manager/include/task_manager.h" +#include #include #include #include @@ -8,6 +9,7 @@ #include "common/base/logging/logger.h" #include "common/config/config_files.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include "task/task_factory.h" using namespace cmvr; @@ -40,6 +42,8 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type) return "TASK_TYPE_SELF_COLLISION"; case config::TaskConfigEntry::TASK_TYPE_QUIC_EDGE: return "TASK_TYPE_QUIC_EDGE"; + case config::TaskConfigEntry::TASK_TYPE_UME_TELEOP: + return "TASK_TYPE_UME_TELEOP"; case config::TaskConfigEntry::TASK_TYPE_UNKNOWN: default: return "TASK_TYPE_UNKNOWN"; @@ -59,6 +63,21 @@ const char* taskConfigRunModeToString(const config::TaskConfigEntry::TaskRunMode } } +void stopTaskNoThrow(const std::shared_ptr& task) +{ + if (!task) { + return; + } + try { + task->stop(); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] task stop threw: " + << error.what(); + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] task stop threw an unknown exception"; + } +} + } // namespace std::shared_ptr TaskManager::instance_ = nullptr; @@ -70,7 +89,11 @@ TaskManager::TaskManager(const config::TaskManagerConfig& cfg) logSection("Task Plan"); logTaskPlan(); logSection("Initialize Tasks"); - initTasks(); + initialized_ = initTasks(); + if (!initialized_) { + CMVR_LOG(ERROR) << "[TaskManager] Initialization failed for at least " + "one enabled task"; + } } TaskManager::~TaskManager() @@ -98,11 +121,46 @@ TaskManager& TaskManager::getInstance() void TaskManager::destroyInstance() { - std::lock_guard lock(init_mutex_); - if (instance_) { - instance_->stopRunTask(); + std::shared_ptr instance; + { + std::lock_guard lock(init_mutex_); + instance = instance_; } - instance_.reset(); + if (instance) { + // Task shutdown may wait for an in-flight SystemService handler. That + // handler can query the process-wide task snapshot, so never retain + // init_mutex_ while stopping tasks or joining service workers. + instance->stopRunTask(); + } + { + std::lock_guard lock(init_mutex_); + if (instance_ == instance) { + instance_.reset(); + } + } +} + +bool TaskManager::stopAllActivitiesIfInitialized( + std::vector* failures) +{ + std::shared_ptr manager; + { + std::lock_guard lock(init_mutex_); + manager = instance_; + } + return !manager || manager->stopAllActivities(failures); +} + +std::vector> +TaskManager::activitySnapshotIfInitialized() +{ + std::shared_ptr manager; + { + std::lock_guard lock(init_mutex_); + manager = instance_; + } + return manager ? manager->activitySnapshot() + : std::vector>{}; } std::shared_ptr TaskManager::getTouchScreenTask(const std::string& task_id) const @@ -126,16 +184,84 @@ std::shared_ptr TaskManager::getTask(const std::string& task_id) const return it->second; } -void TaskManager::startRunTask(const double control_period_s) +bool TaskManager::stopAllActivities(std::vector* failures) { - if (!std::isfinite(control_period_s) || control_period_s <= 0.0) { - CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s"; - return; + const auto tasks = activitySnapshot(); + + bool all_stopped = true; + for (const auto& task : tasks) { + bool stopped = false; + try { + stopped = task->stopActivity(); + } catch (const std::exception& error) { + if (failures) { + failures->push_back( + task->id() + ": stop threw: " + error.what()); + } + } catch (...) { + if (failures) { + failures->push_back( + task->id() + ": stop threw an unknown exception"); + } + } + if (!stopped) { + all_stopped = false; + if (failures && (failures->empty() || + failures->back().compare(0, task->id().size(), task->id()) != 0)) { + failures->push_back( + task->id() + + ": operational stop was not confirmed"); + } + } + } + return all_stopped; +} + +std::vector> TaskManager::activitySnapshot() const +{ + std::vector> tasks; + std::lock_guard lock(tasks_mutex_); + tasks.reserve(tasks_.size()); + for (const auto& [id, task] : tasks_) { + (void)id; + if (task) { + tasks.push_back(task); + } + } + return tasks; +} + +bool TaskManager::startRunTask(const double control_period_s) +{ + auto& admission_gate = service::globalStopAllAdmissionGate(); + std::uint64_t admission_generation = 0U; + { + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting()) { + CMVR_LOG(WARNING) << "[TaskManager] task startup is paused by " + "System StopAll"; + return false; + } + admission_generation = admission.generation(); } - bool expected = false; - if (!running_.compare_exchange_strong(expected, true)) { - return; + std::lock_guard lifecycle_lock(lifecycle_mutex_); + const auto admission_current = [&] { + auto admission = admission_gate.lockAdmission(); + return admission.accepting() && + admission.generation() == admission_generation; + }; + if (!initialized_) { + CMVR_LOG(ERROR) << "[TaskManager] refusing to start because " + "initialization did not complete"; + return false; + } + if (!std::isfinite(control_period_s) || control_period_s <= 0.0) { + CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s"; + return false; + } + if (running_.load()) { + return admission_current(); } std::vector> tasks; @@ -151,39 +277,98 @@ void TaskManager::startRunTask(const double control_period_s) std::vector> started_tasks; for (const auto& task : tasks) { - if (!task->start()) { - CMVR_LOG(ERROR) << "[TaskManager] task start failed: " << task->id(); - for (const auto& started_task : started_tasks) { - try { - started_task->stop(); - } catch (...) { - } + if (!admission_current()) { + CMVR_LOG(WARNING) << "[TaskManager] task startup was interrupted " + "by System StopAll"; + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); } running_.store(false); - return; + return false; + } + + bool started = false; + try { + started = task->start(); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] task start threw: " + << task->id() << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] task start threw an unknown " + "exception: " << task->id(); + } + if (!started) { + CMVR_LOG(ERROR) << "[TaskManager] task start failed: " << task->id(); + stopTaskNoThrow(task); + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); + } + running_.store(false); + return false; } started_tasks.push_back(task); + if (!admission_current()) { + CMVR_LOG(WARNING) << "[TaskManager] task startup crossed a System " + "StopAll boundary: " << task->id(); + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); + } + running_.store(false); + return false; + } } + bool admission_changed = false; try { - run_thread_ = std::thread(&TaskManager::runTaskLoop, this, control_period_s); + // Publish the scheduler under a short admission guard. No task/device + // call or rollback is made while the global StopAll mutex is held. + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting() || + admission.generation() != admission_generation) { + admission_changed = true; + } else { + running_.store(true); + run_thread_ = std::thread( + &TaskManager::runTaskLoop, this, control_period_s); + } } catch (const std::exception& e) { CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread: " << e.what(); running_.store(false); - for (const auto& task : started_tasks) { - try { - task->stop(); - } catch (...) { - } + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); } - return; + return false; + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread with an " + "unknown exception"; + running_.store(false); + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); + } + return false; } + if (admission_changed) { + CMVR_LOG(WARNING) << "[TaskManager] scheduler startup was " + "interrupted by System StopAll"; + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); + } + running_.store(false); + return false; + } + return true; } void TaskManager::stopRunTask() { - bool expected = true; - if (!running_.compare_exchange_strong(expected, false)) { + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (!running_.exchange(false)) { return; } @@ -201,22 +386,24 @@ void TaskManager::stopRunTask() } } } + std::sort(tasks.begin(), tasks.end(), [](const auto& lhs, const auto& rhs) { + return lhs->shutdownPhase() < rhs->shutdownPhase(); + }); for (const auto& task : tasks) { - try { - task->stop(); - } catch (...) { - } + stopTaskNoThrow(task); } } -void TaskManager::initTasks() +bool TaskManager::initTasks() { + bool all_initialized = true; for (const auto& entry : cfg_.tasks()) { if (!entry.enable()) { continue; } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[TaskManager] Task ID is empty"; + all_initialized = false; continue; } @@ -226,9 +413,30 @@ void TaskManager::initTasks() << ", run_mode=" << taskConfigRunModeToString(entry.run_mode()) << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); - auto task = TaskFactory::create(entry); + std::shared_ptr task; + try { + task = TaskFactory::create(entry); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] Task creation threw: " + << entry.id() << ", error=" << error.what(); + all_initialized = false; + continue; + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] Task creation threw an unknown " + "exception: " << entry.id(); + all_initialized = false; + continue; + } if (!task || task->id() != entry.id()) { CMVR_LOG(ERROR) << "[TaskManager] Task ID mismatch: " << entry.id(); + all_initialized = false; + continue; + } + if (entry.run_mode() == + config::TaskConfigEntry::TASK_RUN_MODE_UNKNOWN) { + CMVR_LOG(ERROR) << "[TaskManager] Task run_mode is unknown: " + << entry.id(); + all_initialized = false; continue; } const TaskRunMode configured_run_mode = toTaskRunMode(entry.run_mode()); @@ -236,23 +444,39 @@ void TaskManager::initTasks() CMVR_LOG(ERROR) << "[TaskManager] Task run_mode mismatch: id=" << entry.id() << ", configured=" << taskRunModeToString(configured_run_mode) << ", actual=" << taskRunModeToString(task->runMode()); + all_initialized = false; continue; } double control_period_s = entry.control_period_s(); if (configured_run_mode == TaskRunMode::PERIODIC_STEP && (!std::isfinite(control_period_s) || control_period_s <= 0.0)) { CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s for task: " << entry.id(); + all_initialized = false; continue; } - if (!task->init()) { + bool task_initialized = false; + try { + task_initialized = task->init(); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] Task init threw: " + << entry.id() << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] Task init threw an unknown " + "exception: " << entry.id(); + } + if (!task_initialized) { CMVR_LOG(ERROR) << "[TaskManager] Task init failed: " << entry.id() << ", status=" << task->detailStatusString(); + stopTaskNoThrow(task); + all_initialized = false; continue; } { std::lock_guard lock(tasks_mutex_); if (tasks_.count(entry.id())) { CMVR_LOG(ERROR) << "[TaskManager] Duplicate task ID: " << entry.id(); + stopTaskNoThrow(task); + all_initialized = false; continue; } if (configured_run_mode == TaskRunMode::PERIODIC_STEP) { @@ -261,6 +485,7 @@ void TaskManager::initTasks() tasks_.emplace(entry.id(), std::move(task)); } } + return all_initialized; } void TaskManager::logTaskPlan() const diff --git a/cmvr-es/runtime/CMakeLists.txt b/cmvr-es/runtime/CMakeLists.txt index f8a9a628..b9bb492c 100644 --- a/cmvr-es/runtime/CMakeLists.txt +++ b/cmvr-es/runtime/CMakeLists.txt @@ -13,6 +13,7 @@ target_link_libraries(cmvr_runtime PUBLIC cmvr_es::task_manager cmvr_es::service cmvr_es::quic_edge_task + cmvr_es::ume_teleop_task cmvr_es::mujoco_viewer ) diff --git a/cmvr-es/runtime/src/cmvr_runtime.cpp b/cmvr-es/runtime/src/cmvr_runtime.cpp index 6f0378ae..b39d51d7 100644 --- a/cmvr-es/runtime/src/cmvr_runtime.cpp +++ b/cmvr-es/runtime/src/cmvr_runtime.cpp @@ -12,6 +12,7 @@ #include "common/io/proto_file_io.h" #include "task/grpc_server_task/include/grpc_server_task.h" #include "task/quic_edge_task/include/quic_edge_task.h" +#include "task/ume_teleop_task/include/ume_teleop_task.h" namespace cmvr { namespace { @@ -100,13 +101,27 @@ bool Runtime::init_(const std::string& config_path, return false; } CMVR_LOG(INFO) << "[Startup] Initialize DeviceManager"; - device::DeviceManager::getInstance(device_manager_root.device_manager()); + auto& device_manager = + device::DeviceManager::getInstance( + device_manager_root.device_manager()); + if (!device_manager.initialized()) { + CMVR_LOG(ERROR) << "[Startup] DeviceManager initialization failed"; + device_manager.stop(); + device::DeviceManager::destroyInstance(); + return false; + } + const auto rollback_device_manager = [&device_manager]() { + device_manager.stop(); + device::DeviceManager::destroyInstance(); + }; task::registerGrpcServerTaskFactory(); task::registerQuicEdgeTaskFactory(); + task::registerUmeTeleopTaskFactory(); if (app_config.task_manager_config_file().empty()) { CMVR_LOG(ERROR) << "TaskManager config file is empty"; + rollback_device_manager(); return false; } logSection("TaskManager"); @@ -116,11 +131,19 @@ bool Runtime::init_(const std::string& config_path, if (!ConfigHelper::loadConfigFile(app_config.task_manager_config_file(), task_manager_root)) { CMVR_LOG(ERROR) << "Failed to load TaskManager config: " << app_config.task_manager_config_file(); + rollback_device_manager(); return false; } CMVR_LOG(INFO) << "[Startup] Initialize TaskManager"; - task::TaskManager::getInstance(task_manager_root.task_manager()); + auto& task_manager = + task::TaskManager::getInstance(task_manager_root.task_manager()); + if (!task_manager.initialized()) { + CMVR_LOG(ERROR) << "[Startup] TaskManager initialization failed"; + task::TaskManager::destroyInstance(); + rollback_device_manager(); + return false; + } initialized_ = true; return true; } @@ -134,9 +157,26 @@ bool Runtime::startTasks(const double control_period_s) return true; } + // Device init() constructs and validates resources; start() owns worker + // threads. Start devices before any task can publish commands or sample + // them. UME start remains passive and never enables actuators. + logSection("Start Devices"); + CMVR_LOG(INFO) << "[Startup] Start devices"; + if (!device::DeviceManager::getInstance().start()) { + CMVR_LOG(ERROR) << "[Startup] One or more enabled devices failed " + "to start; tasks will not be started"; + return false; + } + logSection("Start Tasks"); CMVR_LOG(INFO) << "[Startup] Start tasks"; - task::TaskManager::getInstance().startRunTask(control_period_s); + if (!task::TaskManager::getInstance().startRunTask(control_period_s)) { + CMVR_LOG(ERROR) << "[Startup] One or more enabled tasks failed " + "to start; stopping devices"; + device::DeviceManager::getInstance().stop(); + tasks_started_ = false; + return false; + } tasks_started_ = true; return true; } diff --git a/cmvr-es/service/grpc/include/grpc_agv_service.h b/cmvr-es/service/grpc/include/grpc_agv_service.h deleted file mode 100644 index d6310d2d..00000000 --- a/cmvr-es/service/grpc/include/grpc_agv_service.h +++ /dev/null @@ -1,87 +0,0 @@ -#ifndef CMVR_ES_GRPC_AGV_SERVICE_H -#define CMVR_ES_GRPC_AGV_SERVICE_H - -#include "cmvr/api/agv_service.grpc.pb.h" -#include "devices/agv/abstract_agv.h" -#include "manager/device_manager/include/device_manager.h" - -namespace cmvr::service { - -class gRPCAgvServiceImpl final : public api::AgvService::Service { -public: - gRPCAgvServiceImpl(); - ~gRPCAgvServiceImpl() override = default; - - grpc::Status getRuntimeState(grpc::ServerContext* context, - const api::AgvRuntimeStateCommand_Request* request, - api::AgvRuntimeStateCommand_Feedback* response) override; - grpc::Status getNavigationStatus(grpc::ServerContext* context, - const api::AgvNavigationStatusCommand_Request* request, - api::AgvNavigationStatusCommand_Feedback* response) override; - grpc::Status emergencyStop(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) override; - grpc::Status clearFault(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) override; - grpc::Status navigateToPose(grpc::ServerContext* context, - const api::AgvNavigateToPoseCommand_Request* request, - api::AgvNavigateToPoseCommand_Feedback* response) override; - grpc::Status navigateToStation(grpc::ServerContext* context, - const api::AgvNavigateToStationCommand_Request* request, - api::AgvNavigateToStationCommand_Feedback* response) override; - grpc::Status followPath(grpc::ServerContext* context, - const api::AgvFollowPathCommand_Request* request, - api::AgvFollowPathCommand_Feedback* response) override; - grpc::Status pauseNavigation(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) override; - grpc::Status resumeNavigation(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) override; - grpc::Status cancelNavigation(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) override; - grpc::Status setVelocity(grpc::ServerContext* context, - const api::AgvSetVelocityCommand_Request* request, - api::AgvSetVelocityCommand_Feedback* response) override; - grpc::Status stopVelocityControl(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) override; - grpc::Status listMaps(grpc::ServerContext* context, - const api::AgvListMapsCommand_Request* request, - api::AgvListMapsCommand_Feedback* response) override; - grpc::Status listStations(grpc::ServerContext* context, - const api::AgvListStationsCommand_Request* request, - api::AgvListStationsCommand_Feedback* response) override; - grpc::Status switchMap(grpc::ServerContext* context, - const api::AgvMapCommand_Request* request, - api::AgvMapCommand_Feedback* response) override; - grpc::Status uploadMap(grpc::ServerContext* context, - const api::AgvMapCommand_Request* request, - api::AgvMapCommand_Feedback* response) override; - 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; - - - grpc::Status relocalize(grpc::ServerContext* context, - const api::AgvRelocalizeCommand_Request* request, - api::AgvRelocalizeCommand_Feedback* response) override; - -private: - device::DeviceManager& dmgr_; -}; - -} // namespace cmvr::service - -#endif // CMVR_ES_GRPC_AGV_SERVICE_H diff --git a/cmvr-es/service/grpc/include/grpc_arm_service.h b/cmvr-es/service/grpc/include/grpc_arm_service.h deleted file mode 100644 index 3133cc0c..00000000 --- a/cmvr-es/service/grpc/include/grpc_arm_service.h +++ /dev/null @@ -1,64 +0,0 @@ -#pragma once - -#include "cmvr/api/arm_service.grpc.pb.h" -#include "devices/arm/robot_arm.h" -#include "manager/device_manager/include/device_manager.h" - -namespace cmvr::service { - -class gRPCArmServiceImpl final : public api::ArmService::Service { -public: - gRPCArmServiceImpl(); - ~gRPCArmServiceImpl() override = default; - - grpc::Status torqueOff(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) override; - grpc::Status torqueOn(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) override; - grpc::Status moveJ(grpc::ServerContext* context, - const api::MoveJ_Request* request, - api::MoveJ_Response* response) override; - grpc::Status moveL(grpc::ServerContext* context, - const api::MoveL_Request* request, - api::MoveL_Response* response) override; - grpc::Status speedJ(grpc::ServerContext* context, - const api::SpeedJ_Request* request, - api::SpeedJ_Response* response) override; - grpc::Status speedL(grpc::ServerContext* context, - const api::SpeedL_Request* request, - api::SpeedL_Response* response) override; - grpc::Status servoJ(grpc::ServerContext* context, - const api::ServoJ_Request* request, - api::ServoJ_Response* response) override; - grpc::Status stopMotion(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) override; - grpc::Status getJointState(grpc::ServerContext* context, - const api::JointRequest* request, - api::JointResponse* response) override; - grpc::Status getRobotState(grpc::ServerContext* context, - const api::GetRobotState_Request* request, - api::GetRobotState_Response* response) override; - grpc::Status getPose(grpc::ServerContext* context, - const api::GetPose_Request* request, - api::GetPose_Response* response) override; - grpc::Status calibrateZeroQ(grpc::ServerContext* context, - const api::CalibrateZeroQ_Request* request, - api::CalibrateZeroQ_Response* response) override; - grpc::Status getPoseMatrix(grpc::ServerContext* context, - const api::GetPoseMatrix_Request* request, - api::GetPoseMatrix_Response* response) override; - grpc::Status computeForwardKinematics(grpc::ServerContext* context, - const api::ComputeForwardKinematics_Request* request, - api::ComputeForwardKinematics_Response* response) override; - grpc::Status clearFault(grpc::ServerContext *context, - const cmvr::api::CommandHeader_Request *request, - cmvr::api::CommandHeader_Feedback *response) override; - -private: - device::DeviceManager& dmgr_; -}; - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_camera_service.h b/cmvr-es/service/grpc/include/grpc_camera_service.h deleted file mode 100644 index 62e72277..00000000 --- a/cmvr-es/service/grpc/include/grpc_camera_service.h +++ /dev/null @@ -1,46 +0,0 @@ -// -// Created by xtkuang on 2025/6/1. -// - -#ifndef GRPC_CAMERA_SERVICE_H -#define GRPC_CAMERA_SERVICE_H - -#include "cmvr/api/camera_service.grpc.pb.h" -#include "common/base/grpc_utils.h" -#include "manager/device_manager/include/device_manager.h" -#include "devices/camera/abstract_camera.h" -#include "service/grpc/include/grpc_camera_stream_policy.h" - -namespace cmvr::service { - - class gRPCCameraServiceImpl final: public api::CameraService::Service { - public: - explicit gRPCCameraServiceImpl( - CameraStreamLowLatencyConfig stream_config = {}); - ~gRPCCameraServiceImpl() override = default; - grpc::Status GetStatus(grpc::ServerContext* context, const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) override; - grpc::Status StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) override; - grpc::Status StopCamera(grpc::ServerContext* context, const api::StopCameraCommand_Request* request, api::StopCameraCommand_Feedback* response) override; - grpc::Status GetRGBImage(grpc::ServerContext* context, const api::GetRGBImageCommand_Request* request, api::GetRGBImageCommand_Feedback* response) override; - grpc::Status GetDepthImage(grpc::ServerContext* context, const api::GetDepthImageCommand_Request* request, api::GetDepthImageCommand_Feedback* response) override; - grpc::Status GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) override; - grpc::Status StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) override; - grpc::Status StopRecording(grpc::ServerContext* context, const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) override; - grpc::Status ControlPtz(grpc::ServerContext* context, const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response) override; - grpc::Status GetDepthImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; - grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; - grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; - private: - device::DeviceManager& dmgr_; - CameraStreamLowLatencyConfig stream_config_; - - //双向流读写线程 - std::shared_ptr read_thread_ = nullptr; - std::shared_ptr write_thread_ = nullptr; - std::atomic running_{false}; - }; -} - - - -#endif //GRPC_CAMERA_SERVICE_H diff --git a/cmvr-es/service/grpc/include/grpc_camera_stream_policy.h b/cmvr-es/service/grpc/include/grpc_camera_stream_policy.h deleted file mode 100644 index cf5a1f16..00000000 --- a/cmvr-es/service/grpc/include/grpc_camera_stream_policy.h +++ /dev/null @@ -1,60 +0,0 @@ -#ifndef CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H -#define CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H - -#pragma once - -#include -#include -#include -#include - -namespace cmvr::service { - -inline constexpr size_t kDefaultCameraStreamMaxPendingFrames = 2; -inline constexpr uint32_t kDefaultCameraStreamMaxFrameAgeMs = 250; - -struct CameraStreamLowLatencyConfig { - size_t max_pending_frames{kDefaultCameraStreamMaxPendingFrames}; - std::chrono::milliseconds max_frame_age{ - kDefaultCameraStreamMaxFrameAgeMs}; -}; - -inline CameraStreamLowLatencyConfig makeCameraStreamLowLatencyConfig( - const uint32_t max_pending_frames, - const uint32_t max_frame_age_ms) noexcept { - CameraStreamLowLatencyConfig config; - config.max_pending_frames = max_pending_frames == 0 - ? kDefaultCameraStreamMaxPendingFrames - : static_cast(max_pending_frames); - config.max_frame_age = std::chrono::milliseconds( - max_frame_age_ms == 0 - ? kDefaultCameraStreamMaxFrameAgeMs - : max_frame_age_ms); - return config; -} - -inline std::optional cameraFrameAgeNs( - const uint64_t capture_time_ns, - const uint64_t now_ns) noexcept { - if (capture_time_ns == 0 || now_ns < capture_time_ns) { - return std::nullopt; - } - return now_ns - capture_time_ns; -} - -inline bool cameraFrameExceedsAgeLimit( - const uint64_t capture_time_ns, - const uint64_t now_ns, - const std::chrono::milliseconds max_frame_age) noexcept { - const auto age_ns = cameraFrameAgeNs(capture_time_ns, now_ns); - if (!age_ns || max_frame_age.count() <= 0) { - return false; - } - const auto max_age_ns = std::chrono::duration_cast( - max_frame_age).count(); - return *age_ns > static_cast(max_age_ns); -} - -} // namespace cmvr::service - -#endif // CMVR_ES_GRPC_CAMERA_STREAM_POLICY_H diff --git a/cmvr-es/service/grpc/include/grpc_dexhand_service.h b/cmvr-es/service/grpc/include/grpc_dexhand_service.h deleted file mode 100644 index 865a9870..00000000 --- a/cmvr-es/service/grpc/include/grpc_dexhand_service.h +++ /dev/null @@ -1,31 +0,0 @@ -// -// Created by linbo on 2025/7/3. -// - -#ifndef GRPC_DEXHAND_SERVICE_H -#define GRPC_DEXHAND_SERVICE_H - -#include "cmvr/api/dexhand_service.grpc.pb.h" -#include "common/base/grpc_utils.h" -#include "manager/device_manager/include/device_manager.h" -#include "devices/dexhand/abstract_dexhand.h" -namespace cmvr::service { - class gRPCDexHandServiceImpl final: public api::DexHandService::Service { - public: - gRPCDexHandServiceImpl(); - ~gRPCDexHandServiceImpl() override = default; - grpc::Status GetStatus(grpc::ServerContext* context, const api::GetDexHandStateCommand_Request* request,api::GetDexHandStateCommand_Feedback* response) override; - grpc::Status SetDexHandPos(grpc::ServerContext* context, const cmvr::api::SetDexHandPositionsCommand_Request* request, cmvr::api::SetDexHandPositionsCommand_Feedback* response) override; - grpc::Status SetDexHandAngle(grpc::ServerContext* context, const cmvr::api::SetDexHandAnglesCommand_Request* request, cmvr::api::SetDexHandAnglesCommand_Feedback* response) override; - grpc::Status SetDexHandForce(grpc::ServerContext* context, const cmvr::api::SetDexHandForceCommand_Request* request, cmvr::api::SetDexHandForceCommand_Feedback* response) override; - grpc::Status SetDexHandSpeed(grpc::ServerContext* context, const cmvr::api::SetDexHandSpeedCommand_Request* request, cmvr::api::SetDexHandSpeedCommand_Feedback* response) override; - grpc::Status SetDexHandPresetAct(grpc::ServerContext* context, const cmvr::api::SetDexHandPresetActCommand_Request* request, cmvr::api::SetDexHandPresetActCommand_Feedback* response) override; - grpc::Status GetSensorData(grpc::ServerContext* context, const cmvr::api::GetSensorDataCommand_Request* request, cmvr::api::GetSensorDataCommand_Feedback* response) override; - grpc::Status GetSensorDataStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; - private: - device::DeviceManager& dmgr_; - }; - -} - -#endif //GRPC_DEXHAND_SERVICE_H diff --git a/cmvr-es/service/grpc/include/grpc_head_service.h b/cmvr-es/service/grpc/include/grpc_head_service.h deleted file mode 100644 index 4b8b20c3..00000000 --- a/cmvr-es/service/grpc/include/grpc_head_service.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef BIO_HEAD_SERVICE_H -#define BIO_HEAD_SERVICE_H - -#include "cmvr/api/biohead_service.grpc.pb.h" -#include "manager/device_manager/include/device_manager.h" -#include "devices/biohead/abstract_biohead.h" - -namespace cmvr::service -{ - class gRPCMBioHeadServiceImpl : public api::BioHeadService::Service { - public: - gRPCMBioHeadServiceImpl(); - ~gRPCMBioHeadServiceImpl() override = default; - - grpc::Status SetExpression(grpc::ServerContext* context, - const api::SetFacialExpression_Request* request, - api::SetFacialExpression_Feedback* response) override; - - grpc::Status StreamExpression(grpc::ServerContext* context, - grpc::ServerReaderWriter* stream) override; - - - grpc::Status GetSystemStatus(grpc::ServerContext* context, - const api::GetStatus_Request* request, - api::GetStatus_Feedback* response) override; - - grpc::Status EmergencyStop(grpc::ServerContext* context, - const api::EmergencyStop_Request* request, - api::EmergencyStop_Feedback* response) override; - grpc::Status SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response) override; - - grpc::Status SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response) override; - - grpc::Status Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response) override; - - grpc::Status Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response) override; - - grpc::Status ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response) override; - - grpc::Status ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response) override; - - grpc::Status ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response) override; - - grpc::Status ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response) override; - private: - device::DeviceManager& dmgr_; - }; -} // namespace cmvr::service - -#endif // BIO_HEAD_SERVICE_H diff --git a/cmvr-es/service/grpc/include/grpc_hlc_service.h b/cmvr-es/service/grpc/include/grpc_hlc_service.h deleted file mode 100644 index 9fc593cc..00000000 --- a/cmvr-es/service/grpc/include/grpc_hlc_service.h +++ /dev/null @@ -1,21 +0,0 @@ -// -// Created by lgv on 2025/8/25. -// -#pragma once - -#include "cmvr/api/hlc_service.grpc.pb.h" - -namespace cmvr { - namespace service { - class gRPCHlcServiceImpl final : public api::HlcService::Service { - public: - gRPCHlcServiceImpl(); - ~gRPCHlcServiceImpl() = default; - grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override; - }; - - - - } - -} diff --git a/cmvr-es/service/grpc/include/grpc_microphone_service.h b/cmvr-es/service/grpc/include/grpc_microphone_service.h deleted file mode 100644 index 39eb731f..00000000 --- a/cmvr-es/service/grpc/include/grpc_microphone_service.h +++ /dev/null @@ -1,36 +0,0 @@ -// -// Created by linbo on 2025/6/13. -// Created by xtkuang on 2025/6/13. -// - -#ifndef GRPC_MICROPHONE_SERVICE_H -#define GRPC_MICROPHONE_SERVICE_H -#include "cmvr/api/microphone_service.grpc.pb.h" -#include "manager/device_manager/include/device_manager.h" -#include "devices/microphone/abstract_microphone.h" -#include "common/base/grpc_utils.h" - -namespace cmvr::service -{ - - class gRPCMicroPhoneServiceImpl: public api::MicPhoneService::Service { - public: - gRPCMicroPhoneServiceImpl(); - ~gRPCMicroPhoneServiceImpl() override = default; - grpc::Status ListDevices( - grpc::ServerContext* context, - const api::ListMicrophoneDevicesCommand_Request* request, - api::ListMicrophoneDevicesCommand_Feedback* response) override; - grpc::Status GetStatus(grpc::ServerContext* context, const api::GetMicStateCommand_Request* request,api::GetMicStateCommand_Feedback* response) override; - grpc::Status StartRecord(grpc::ServerContext* context, const api::StartMicRecordingCommand_Request* request,api::StartMicRecordingCommand_Feedback* response) override; - grpc::Status StopRecord(grpc::ServerContext* context, const api::StopMicRecordingCommand_Request* request,api::StopMicRecordingCommand_Feedback* response) override; - grpc::Status PauseRecord(grpc::ServerContext* context, const api::PauseMicRecordingCommand_Request* request,api::PauseMicRecordingCommand_Feedback* response) override; - grpc::Status ResumeRecord(grpc::ServerContext* context, const api::ResumeMicRecordingCommand_Request* request,api::ResumeMicRecordingCommand_Feedback* response) override; - grpc::Status StreamAudio(grpc::ServerContext* context, const api::StreamMicAudioCommand_Request* request, grpc::ServerWriter* writer) override; - grpc::Status SetVolume(grpc::ServerContext* context, const api::SetMicPhoneVolumeCommand_Request* request,api::SetMicPhoneVolumeCommand_Feedback* response) override; - grpc::Status GetVolume(grpc::ServerContext* context, const api::GetMicPhoneVolumeCommand_Request* request,api::GetMicPhoneVolumeCommand_Feedback* response) override; - private: - device::DeviceManager& dmgr_; - }; -} -#endif //GRPC_MICROPHONE_SERVICE_H diff --git a/cmvr-es/service/grpc/include/grpc_speaker_service.h b/cmvr-es/service/grpc/include/grpc_speaker_service.h deleted file mode 100644 index 2bf0e4c1..00000000 --- a/cmvr-es/service/grpc/include/grpc_speaker_service.h +++ /dev/null @@ -1,32 +0,0 @@ -// -// Created by xtkuang on 2025/6/10. -// - -#ifndef GRPC_SPEAKER_SERVICE_H -#define GRPC_SPEAKER_SERVICE_H - -#include "cmvr/api/speaker_service.grpc.pb.h" -#include "manager/device_manager/include/device_manager.h" -#include "devices/speaker/abstract_speaker.h" -#include "common/base/grpc_utils.h" - -namespace cmvr::service { - class gRPCSpeakerServiceImpl: public api::SpeakerService::Service { - public: - gRPCSpeakerServiceImpl(); - ~gRPCSpeakerServiceImpl() override = default; - grpc::Status GetStatus(grpc::ServerContext* context, const api::GetSpeakerStateCommand_Request* request,api::GetSpeakerStateCommand_Feedback* response) override; - grpc::Status PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request,api::PlayAudioCommand_Feedback* response) override; - grpc::Status StreamAudio(grpc::ServerContext* context, grpc::ServerReader* reader, api::StreamSpeakerAudioCommand_Feedback* response) override; - grpc::Status StopPlayback(grpc::ServerContext* context, const api::StopSpeakerCommand_Request* request,api::StopSpeakerCommand_Feedback* response) override; - grpc::Status PausePlayback(grpc::ServerContext* context, const api::PauseSpeakerCommand_Request* request,api::PauseSpeakerCommand_Feedback* response) override; - grpc::Status ResumePlayback(grpc::ServerContext* context, const api::ResumeSpeakerCommand_Request* request,api::ResumeSpeakerCommand_Feedback* response) override; - grpc::Status SetVolume(grpc::ServerContext* context, const api::SetSpeakerVolumeCommand_Request* request,api::SetSpeakerVolumeCommand_Feedback* response) override; - grpc::Status GetVolume(grpc::ServerContext* context, const api::GetSpeakerVolumeCommand_Request* request,api::GetSpeakerVolumeCommand_Feedback* response) override; - private: - device::DeviceManager& dmgr_; - }; -} - - -#endif //GRPC_SPEAKER_SERVICE_H diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h deleted file mode 100644 index 41fb2646..00000000 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ /dev/null @@ -1,28 +0,0 @@ -// -// Created by xtkuang on 2025/6/6. -// - -#ifndef GRPC_SYSTEM_SERVICE_H -#define GRPC_SYSTEM_SERVICE_H - -#include "cmvr/api/system_service.grpc.pb.h" -#include "common/base/grpc_utils.h" -#include "manager/device_manager/include/device_manager.h" - -namespace cmvr::service -{ - class gRPCSystemServiceImpl: public api::SystemService::Service { - public: - gRPCSystemServiceImpl(); - ~gRPCSystemServiceImpl() override = default; - grpc::Status GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) override; - grpc::Status GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) override; - grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override; - grpc::Status ExecuteJsonCommand(grpc::ServerContext* context, const cmvr::api::JsonDeviceCommand_Request* request, cmvr::api::JsonDeviceCommand_Feedback* response) override; - grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override; - private: - device::DeviceManager& dmgr_; - }; -} - -#endif //GRPC_SYSTEM_SERVICE_H diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp deleted file mode 100644 index 1f8d160f..00000000 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ /dev/null @@ -1,752 +0,0 @@ -#include "service/grpc/include/grpc_agv_service.h" - -#include -#include -#include -#include - -#include - -using google::protobuf::util::TimeUtil; - -namespace cmvr::service { - -namespace { - -void fillFeedback(api::CommandHeader_Feedback* feedback, - const bool success, - const std::string& message = {}) -{ - feedback->set_success(success); - feedback->set_error_message(message); - *feedback->mutable_timestamp() = TimeUtil::GetCurrentTime(); -} - -grpc::Status resultToStatus(const device::AgvResult& result) -{ - if (result.ok()) { - return grpc::Status::OK; - } - return grpc::Status(grpc::StatusCode::INTERNAL, result.message); -} - -template -grpc::Status setResponseResult(Response* response, const device::AgvResult& result) -{ - fillFeedback(response->mutable_header(), result.ok(), result.ok() ? "" : result.message); - return resultToStatus(result); -} - -grpc::Status setResponseResult(api::CommandHeader_Feedback* response, const device::AgvResult& result) -{ - fillFeedback(response, result.ok(), result.ok() ? "" : result.message); - return resultToStatus(result); -} - -template -grpc::Status setDeviceNotFound(Response* response, const std::string& device_id) -{ - const std::string message = "AGV device not found: " + device_id; - fillFeedback(response->mutable_header(), false, message); - return grpc::Status(grpc::StatusCode::NOT_FOUND, message); -} - -grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std::string& device_id) -{ - const std::string message = "AGV device not found: " + device_id; - fillFeedback(response, false, message); - return grpc::Status(grpc::StatusCode::NOT_FOUND, message); -} - -device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src) -{ - device::AgvAdapterParams dst; - for (const auto& [key, value] : src.values()) { - dst.values.emplace(key, value); - } - return dst; -} - -device::AgvMotionOptions toMotionOptions(const msgs::AgvMotionOptions& src) -{ - device::AgvMotionOptions dst; - dst.max_speed = src.max_speed(); - dst.max_angular_speed = src.max_angular_speed(); - dst.max_acceleration = src.max_acceleration(); - dst.max_angular_acceleration = src.max_angular_acceleration(); - dst.reach_distance = src.reach_distance(); - dst.reach_angle = src.reach_angle(); - dst.speed_ratio = src.speed_ratio() > 0.0 ? src.speed_ratio() : 1.0; - dst.asynchronous = src.asynchronous(); - return dst; -} - -device::AgvVelocity toVelocity(const msgs::AgvVelocity& src) -{ - return {src.vx(), src.vy(), src.wz()}; -} - -device::AgvPathSegment toPathSegment(const msgs::AgvPathSegment& src) -{ - device::AgvPathSegment dst; - dst.source_station = src.source_station(); - dst.target_station = src.target_station(); - return dst; -} - -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); - dst->set_y(src.y); - dst->set_theta(src.theta); -} - -void fillVelocity(msgs::AgvVelocity* dst, const device::AgvVelocity& src) -{ - dst->set_vx(src.vx); - dst->set_vy(src.vy); - dst->set_wz(src.wz); -} - -void fillBattery(msgs::AgvBatteryState* dst, const device::AgvBatteryState& src) -{ - dst->set_percentage(src.percentage); - dst->set_voltage(src.voltage); - dst->set_current(src.current); - dst->set_temperature(src.temperature); - dst->set_charging(src.charging); -} - -void fillRuntimeState(msgs::AgvRuntimeState* dst, const device::AgvRuntimeState& src) -{ - dst->set_timestamp(src.timestamp); - dst->set_mode(static_cast(src.mode)); - dst->set_connected(src.connected); - dst->set_localized(src.localized); - dst->set_moving(src.moving); - dst->set_fault(src.fault); - dst->set_emergency_stopped(src.emergency_stopped); - fillPose2d(dst->mutable_pose(), src.pose); - fillVelocity(dst->mutable_velocity(), src.velocity); - fillBattery(dst->mutable_battery(), src.battery); - dst->set_current_map(src.current_map); - dst->set_current_station(src.current_station); - dst->set_last_error(src.last_error); -} - -void fillNavigationStatus(msgs::AgvNavigationStatus* dst, const device::AgvNavigationStatus& src) -{ - dst->set_state(static_cast(src.state)); - dst->set_type(static_cast(src.type)); - dst->set_progress(src.progress); - dst->set_message(src.message); -} - -void fillStation(msgs::AgvStation* dst, const device::AgvStation& src) -{ - dst->set_id(src.id); - dst->set_type(src.type); - fillPose2d(dst->mutable_pose(), src.pose); - 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() - : dmgr_(device::DeviceManager::getInstance()) -{ -} - -grpc::Status gRPCAgvServiceImpl::getRuntimeState(grpc::ServerContext*, - const api::AgvRuntimeStateCommand_Request* request, - api::AgvRuntimeStateCommand_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); - } - fillRuntimeState(response->mutable_state(), agv->runtimeState()); - fillFeedback(response->mutable_header(), true); - return grpc::Status::OK; - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::getNavigationStatus(grpc::ServerContext*, - const api::AgvNavigationStatusCommand_Request* request, - api::AgvNavigationStatusCommand_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); - } - fillNavigationStatus(response->mutable_status(), agv->navigationStatus()); - fillFeedback(response->mutable_header(), true); - return grpc::Status::OK; - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::emergencyStop(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->emergencyStop()); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::clearFault(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->clearFault()); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*, - const api::AgvNavigateToPoseCommand_Request* request, - api::AgvNavigateToPoseCommand_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); - } - return setResponseResult(response, agv->navigateToPose( - toPose2d(request->pose()), - toMotionOptions(request->options()), - toAdapterParams(request->adapter_params()))); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*, - const api::AgvNavigateToStationCommand_Request* request, - api::AgvNavigateToStationCommand_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); - } - return setResponseResult(response, agv->navigateToStation( - request->station_id(), - toMotionOptions(request->options()), - toAdapterParams(request->adapter_params()))); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext*, - const api::AgvFollowPathCommand_Request* request, - api::AgvFollowPathCommand_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); - } - std::vector path; - path.reserve(static_cast(request->path_size())); - for (const auto& segment : request->path()) { - path.push_back(toPathSegment(segment)); - } - return setResponseResult(response, agv->followPath(path)); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::pauseNavigation(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->pauseNavigation()); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::resumeNavigation(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->resumeNavigation()); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::cancelNavigation(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->cancelNavigation()); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*, - const api::AgvSetVelocityCommand_Request* request, - api::AgvSetVelocityCommand_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); - } - return setResponseResult(response, agv->setVelocity(toVelocity(request->velocity()))); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::stopVelocityControl(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->stopVelocityControl()); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::listMaps(grpc::ServerContext*, - const api::AgvListMapsCommand_Request* request, - api::AgvListMapsCommand_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); - } - std::vector maps; - const auto result = agv->listMaps(maps); - if (result.ok()) { - for (const auto& map : maps) { - response->add_maps(map); - } - } - 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::listStations(grpc::ServerContext*, - const api::AgvListStationsCommand_Request* request, - api::AgvListStationsCommand_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); - } - std::vector stations; - const auto result = agv->listStations(stations); - if (result.ok()) { - for (const auto& station : stations) { - fillStation(response->add_stations(), station); - } - } - 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::switchMap(grpc::ServerContext*, - const api::AgvMapCommand_Request* request, - api::AgvMapCommand_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); - } - return setResponseResult(response, agv->switchMap(request->map_name())); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*, - const api::AgvMapCommand_Request* request, - api::AgvMapCommand_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); - } - return setResponseResult(response, agv->uploadMap(request->map_name(), request->content())); - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCAgvServiceImpl::downloadMap(grpc::ServerContext*, - const api::AgvMapCommand_Request* request, - api::AgvMapCommand_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); - } - std::string content; - const auto result = agv->downloadMap(request->map_name(), content); - if (result.ok()) { - response->set_content(content); - } - 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::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/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp b/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp deleted file mode 100644 index 6559a1e2..00000000 --- a/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp +++ /dev/null @@ -1,109 +0,0 @@ -#include -#include "common/base/logging/logger.h" -#include -#include - -#include "cmvr/api/arm_service.grpc.pb.h" - -using google::protobuf::util::TimeUtil; - -namespace { - -std::unique_ptr makeStub() -{ - auto channel = grpc::CreateChannel("0.0.0.0:50055", grpc::InsecureChannelCredentials()); - return cmvr::api::ArmService::NewStub(channel); -} - -void fillHeader(cmvr::api::CommandHeader_Request* header) -{ - header->set_device_id("right_arm"); - *header->mutable_timestamp() = TimeUtil::GetCurrentTime(); -} - -} // namespace - -TEST(GrpcArmClientTest, TorqueOn) -{ - auto stub = makeStub(); - cmvr::api::CommandHeader_Request request; - fillHeader(&request); - - cmvr::api::CommandHeader_Feedback response; - grpc::ClientContext context; - const grpc::Status status = stub->torqueOn(&context, request, &response); - - if (!status.ok()) { - CMVR_LOG(ERROR) << "RPC failed: " << status.error_message(); - } -} - -TEST(GrpcArmClientTest, MoveJ) -{ - auto stub = makeStub(); - cmvr::api::MoveJ_Request request; - fillHeader(request.mutable_header()); - - const std::vector q{ - 0.00203898, 1.34062, 0.0, 0.322261, 0.0, -0.000210733, -0.0942364 - }; - for (double value : q) { - request.mutable_target()->add_position(value); - } - request.mutable_options()->set_velocity(0.8); - request.mutable_options()->set_acceleration(0.8); - - cmvr::api::MoveJ_Response response; - grpc::ClientContext context; - const grpc::Status status = stub->moveJ(&context, request, &response); - - if (!status.ok()) { - CMVR_LOG(ERROR) << "RPC failed: " << status.error_message(); - } -} - -TEST(GrpcArmClientTest, GetJointState) -{ - auto stub = makeStub(); - cmvr::api::JointRequest request; - fillHeader(request.mutable_header()); - - cmvr::api::JointResponse response; - grpc::ClientContext context; - const grpc::Status status = stub->getJointState(&context, request, &response); - - if (status.ok()) { - const auto& state = response.state(); - for (int i = 0; i < state.name_size() && i < state.position_size(); ++i) { - CMVR_LOG(INFO) << "Joint: " << state.name(i) - << ", Position: " << state.position(i); - } - } else { - CMVR_LOG(ERROR) << "RPC failed: " << status.error_message(); - } -} - -TEST(GrpcArmClientTest, GetRobotState) -{ - auto stub = makeStub(); - cmvr::api::GetRobotState_Request request; - fillHeader(request.mutable_header()); - - cmvr::api::GetRobotState_Response response; - grpc::ClientContext context; - const grpc::Status status = stub->getRobotState(&context, request, &response); - - if (status.ok()) { - const auto& state = response.state(); - CMVR_LOG(INFO) << "Robot state: connected=" << state.connected() - << ", powered_on=" << state.powered_on() - << ", moving=" << state.moving() - << ", fault=" << state.fault() - << ", robot_mode=" << state.robot_mode() - << ", safety_mode=" << state.safety_mode() - << ", control_mode=" << state.control_mode() - << ", joints=" << state.actual_joint_state().name_size(); - } else { - CMVR_LOG(ERROR) << "RPC failed: " << status.error_message(); - } -} diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp deleted file mode 100644 index 60dc3a26..00000000 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ /dev/null @@ -1,573 +0,0 @@ -#include "service/grpc/include/grpc_arm_service.h" - -#include - -#include "common/base/logging/logger.h" - -using google::protobuf::util::TimeUtil; - -namespace cmvr::service { - -namespace { - -void fillFeedback(api::CommandHeader_Feedback* feedback, - const bool success, - const std::string& message = {}) -{ - feedback->set_success(success); - feedback->set_error_message(message); - *feedback->mutable_timestamp() = TimeUtil::GetCurrentTime(); -} - -grpc::Status resultToStatus(const device::Result& result) -{ - if (result.ok()) { - return grpc::Status::OK; - } - return grpc::Status(grpc::StatusCode::INTERNAL, result.message); -} - -void logRpcSuccess(const char* rpc_name, const std::string& device_id) -{ - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (" << rpc_name - << "): success, id=" << device_id; -} - -device::FrameType toFrameType(const api::ArmFrameType frame) -{ - switch (frame) { - case api::ARM_FRAME_TOOL: - return device::FrameType::Tool; - case api::ARM_FRAME_WORLD: - return device::FrameType::World; - case api::ARM_FRAME_USER: - return device::FrameType::User; - case api::ARM_FRAME_BASE: - default: - return device::FrameType::Base; - } -} - -device::JointPositionCommand toJointPositionCommand(const api::JointPositionCommand& src) -{ - device::JointPositionCommand dst; - dst.position.assign(src.position().begin(), src.position().end()); - return dst; -} - -device::JointVelocityCommand toJointVelocityCommand(const api::JointVelocityCommand& src) -{ - device::JointVelocityCommand dst; - dst.velocity.assign(src.velocity().begin(), src.velocity().end()); - return dst; -} - -device::MotionOptions toMotionOptions(const api::MotionOptions& src) -{ - device::MotionOptions dst; - dst.velocity = src.velocity(); - dst.acceleration = src.acceleration(); - dst.blend_radius = src.blend_radius(); - dst.jerk = src.jerk() > 0.0 ? src.jerk() : 5.0; - dst.joint_velocity_limits.assign(src.joint_velocity_limits().begin(), - src.joint_velocity_limits().end()); - dst.asynchronous = src.asynchronous(); - return dst; -} - -device::CartesianPose toCartesianPose(const api::CartesianPose& src) -{ - return {src.x(), src.y(), src.z(), src.rx(), src.ry(), src.rz()}; -} - -api::CartesianPose toApiCartesianPose(const device::CartesianPose& src) -{ - api::CartesianPose dst; - dst.set_x(src.x); - dst.set_y(src.y); - dst.set_z(src.z); - dst.set_rx(src.rx); - dst.set_ry(src.ry); - dst.set_rz(src.rz); - return dst; -} - -api::CartesianVelocity toApiCartesianVelocity(const device::CartesianVelocity& src) -{ - api::CartesianVelocity dst; - dst.set_vx(src.vx); - dst.set_vy(src.vy); - dst.set_vz(src.vz); - dst.set_wx(src.wx); - dst.set_wy(src.wy); - dst.set_wz(src.wz); - return dst; -} - -api::CartesianWrench toApiCartesianWrench(const device::CartesianWrench& src) -{ - api::CartesianWrench dst; - dst.set_fx(src.fx); - dst.set_fy(src.fy); - dst.set_fz(src.fz); - dst.set_tx(src.tx); - dst.set_ty(src.ty); - dst.set_tz(src.tz); - return dst; -} - -api::ArmRobotMode toApiRobotMode(const device::RobotMode mode) -{ - switch (mode) { - case device::RobotMode::Disconnected: - return api::ARM_ROBOT_MODE_DISCONNECTED; - case device::RobotMode::PowerOff: - return api::ARM_ROBOT_MODE_POWER_OFF; - case device::RobotMode::Idle: - return api::ARM_ROBOT_MODE_IDLE; - case device::RobotMode::Running: - return api::ARM_ROBOT_MODE_RUNNING; - case device::RobotMode::Paused: - return api::ARM_ROBOT_MODE_PAUSED; - case device::RobotMode::Stopped: - return api::ARM_ROBOT_MODE_STOPPED; - case device::RobotMode::Fault: - return api::ARM_ROBOT_MODE_FAULT; - case device::RobotMode::Unknown: - default: - return api::ARM_ROBOT_MODE_UNKNOWN; - } -} - -api::ArmSafetyMode toApiSafetyMode(const device::SafetyMode mode) -{ - switch (mode) { - case device::SafetyMode::Normal: - return api::ARM_SAFETY_MODE_NORMAL; - case device::SafetyMode::Reduced: - return api::ARM_SAFETY_MODE_REDUCED; - case device::SafetyMode::ProtectiveStop: - return api::ARM_SAFETY_MODE_PROTECTIVE_STOP; - case device::SafetyMode::EmergencyStop: - return api::ARM_SAFETY_MODE_EMERGENCY_STOP; - case device::SafetyMode::SafeguardStop: - return api::ARM_SAFETY_MODE_SAFEGUARD_STOP; - case device::SafetyMode::SystemEmergencyStop: - return api::ARM_SAFETY_MODE_SYSTEM_EMERGENCY_STOP; - case device::SafetyMode::Fault: - return api::ARM_SAFETY_MODE_FAULT; - case device::SafetyMode::Unknown: - default: - return api::ARM_SAFETY_MODE_UNKNOWN; - } -} - -api::ArmControlMode toApiControlMode(const device::ControlMode mode) -{ - switch (mode) { - case device::ControlMode::Manual: - return api::ARM_CONTROL_MODE_MANUAL; - case device::ControlMode::Position: - return api::ARM_CONTROL_MODE_POSITION; - case device::ControlMode::Velocity: - return api::ARM_CONTROL_MODE_VELOCITY; - case device::ControlMode::Torque: - return api::ARM_CONTROL_MODE_TORQUE; - case device::ControlMode::Servo: - return api::ARM_CONTROL_MODE_SERVO; - case device::ControlMode::Freedrive: - return api::ARM_CONTROL_MODE_FREEDRIVE; - case device::ControlMode::None: - default: - return api::ARM_CONTROL_MODE_NONE; - } -} - -void fillJointState(const device::RobotModel& model, - const device::JointGroupState& state, - api::JointState* msg) -{ - for (const auto& name : model.joint_names) msg->add_name(name); - for (const double value : state.position) msg->add_position(value); - for (const double value : state.velocity) msg->add_velocity(value); - for (const double value : state.effort) msg->add_effort(value); -} - -void fillRobotState(const device::RobotModel& model, - const device::ArmState& state, - api::RobotState* msg) -{ - msg->set_timestamp(state.timestamp); - msg->set_robot_mode(toApiRobotMode(state.robot_mode)); - msg->set_safety_mode(toApiSafetyMode(state.safety_mode)); - msg->set_control_mode(toApiControlMode(state.control_mode)); - msg->set_connected(state.connected); - msg->set_powered_on(state.powered_on); - msg->set_brake_released(state.brake_released); - msg->set_moving(state.moving); - msg->set_program_running(state.program_running); - msg->set_protective_stopped(state.protective_stopped); - msg->set_emergency_stopped(state.emergency_stopped); - msg->set_fault(state.fault); - msg->set_speed_scaling(state.speed_scaling); - fillJointState(model, state.actual_joint_state, msg->mutable_actual_joint_state()); - fillJointState(model, state.target_joint_state, msg->mutable_target_joint_state()); - *msg->mutable_actual_tcp_pose() = toApiCartesianPose(state.actual_tcp_pose); - *msg->mutable_actual_tcp_velocity() = toApiCartesianVelocity(state.actual_tcp_velocity); - *msg->mutable_actual_tcp_wrench() = toApiCartesianWrench(state.actual_tcp_wrench); -} - -device::CartesianVelocity toCartesianVelocity(const api::CartesianVelocity& src) -{ - return {src.vx(), src.vy(), src.vz(), src.wx(), src.wy(), src.wz()}; -} - -template -grpc::Status setResponseResult(Response* response, const device::Result& result) -{ - fillFeedback(response->mutable_header(), result.ok(), result.ok() ? "" : result.message); - return resultToStatus(result); -} - -grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std::string& device_id) -{ - const std::string message = "RobotArm device not found: " + device_id; - fillFeedback(response, false, message); - return grpc::Status(grpc::StatusCode::NOT_FOUND, message); -} - -template -grpc::Status setDeviceNotFound(Response* response, const std::string& device_id) -{ - const std::string message = "RobotArm device not found: " + device_id; - fillFeedback(response->mutable_header(), false, message); - return grpc::Status(grpc::StatusCode::NOT_FOUND, message); -} - -} // namespace - -gRPCArmServiceImpl::gRPCArmServiceImpl() - : dmgr_(device::DeviceManager::getInstance()) -{ -} - -grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - try { - const std::string device_id = request->device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto result = arm->torqueOff(); - fillFeedback(response, result.ok(), result.ok() ? "" : result.message); - if (result.ok()) { - logRpcSuccess("torqueOff", device_id); - } - return resultToStatus(result); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - try { - const std::string device_id = request->device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto result = arm->torqueOn(); - fillFeedback(response, result.ok(), result.ok() ? "" : result.message); - if (result.ok()) { - logRpcSuccess("torqueOn", device_id); - } - return resultToStatus(result); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, - const api::MoveJ_Request* request, - api::MoveJ_Response* response) -{ - try { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto result = arm->moveJ(toJointPositionCommand(request->target()), - toMotionOptions(request->options())); - if (result.ok()) { - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id - << ", positions=" << request->target().position_size(); - } - 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 gRPCArmServiceImpl::moveL(grpc::ServerContext*, - const api::MoveL_Request* request, - api::MoveL_Response* response) -{ - try { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto result = arm->moveL(toCartesianPose(request->target()), - toMotionOptions(request->options()), - toFrameType(request->frame())); - if (result.ok()) { - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id - << ", frame=" << request->frame(); - } - 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 gRPCArmServiceImpl::speedJ(grpc::ServerContext*, - const api::SpeedJ_Request* request, - api::SpeedJ_Response* response) -{ - try { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()), - request->acceleration(), - request->duration()); - if (result.ok()) { - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedJ): success, id=" << device_id - << ", velocities=" << request->velocity().velocity_size() - << ", acceleration=" << request->acceleration() - << ", duration=" << request->duration(); - } - 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 gRPCArmServiceImpl::speedL(grpc::ServerContext*, - const api::SpeedL_Request* request, - api::SpeedL_Response* response) -{ - try { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto result = arm->speedL(toCartesianVelocity(request->velocity()), - request->acceleration(), - request->duration(), - toFrameType(request->frame())); - if (result.ok()) { - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedL): success, id=" << device_id - << ", acceleration=" << request->acceleration() - << ", duration=" << request->duration() - << ", frame=" << request->frame(); - } - 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 gRPCArmServiceImpl::servoJ(grpc::ServerContext*, - const api::ServoJ_Request* request, - api::ServoJ_Response* response) -{ - try { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto result = arm->servoJ(toJointPositionCommand(request->target())); - if (result.ok()) { - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (servoJ): success, id=" << device_id - << ", positions=" << request->target().position_size(); - } - 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 gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - try { - const std::string device_id = request->device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto result = arm->stopMotion(); - fillFeedback(response, result.ok(), result.ok() ? "" : result.message); - if (result.ok()) { - logRpcSuccess("stopMotion", device_id); - } - return resultToStatus(result); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*, - const api::JointRequest* request, - api::JointResponse* response) -{ - try { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto model = arm->getRobotModel(); - const auto state = arm->getJointState(); - auto* msg = response->mutable_state(); - fillJointState(model, state, msg); - fillFeedback(response->mutable_header(), true); - // CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getJointState): success, id=" << device_id - // << ", joints=" << msg->name_size() - // << ", positions=" << msg->position_size(); - return grpc::Status::OK; - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCArmServiceImpl::getRobotState( - grpc::ServerContext*, - const api::GetRobotState_Request* request, - api::GetRobotState_Response* response) -{ - try { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - - const auto model = arm->getRobotModel(); - const auto state = arm->getRobotState(); - fillRobotState(model, state, response->mutable_state()); - fillFeedback(response->mutable_header(), true); - logRpcSuccess("getRobotState", device_id); - return grpc::Status::OK; - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*, - const api::GetPose_Request* request, - api::GetPose_Response* response) -{ - try { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto pose = request->base_link().empty() || request->ee_link().empty() - ? arm->fk(true) - : arm->fk(request->base_link(), request->ee_link()); - *response->mutable_pose() = toApiCartesianPose(pose); - fillFeedback(response->mutable_header(), true); - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getPose): success, id=" << device_id - << ", pose=(" << pose.x << ", " << pose.y << ", " << pose.z - << ", " << pose.rx << ", " << pose.ry << ", " << pose.rz << ")"; - return grpc::Status::OK; - } catch (const std::exception& e) { - fillFeedback(response->mutable_header(), false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} - -grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*, - const api::CalibrateZeroQ_Request* request, - api::CalibrateZeroQ_Response* response) -{ - try { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto result = arm->calibrateZeroQ(request->joint_name()); - if (result.ok()) { - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (calibrateZeroQ): success, id=" << device_id - << ", joint=" << request->joint_name(); - } - 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 gRPCArmServiceImpl::getPoseMatrix(grpc::ServerContext*, - const api::GetPoseMatrix_Request*, - api::GetPoseMatrix_Response* response) -{ - fillFeedback(response->mutable_header(), false, "getPoseMatrix is not implemented"); - return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "getPoseMatrix is not implemented"); -} - -grpc::Status gRPCArmServiceImpl::computeForwardKinematics(grpc::ServerContext*, - const api::ComputeForwardKinematics_Request*, - api::ComputeForwardKinematics_Response* response) -{ - fillFeedback(response->mutable_header(), false, "computeForwardKinematics is not implemented"); - return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "computeForwardKinematics is not implemented"); -} - -grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, - const cmvr::api::CommandHeader_Request *request, - cmvr::api::CommandHeader_Feedback *response) -{ - try { - const std::string device_id = request->device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - const auto result = arm->clearFault(); - fillFeedback(response, result.ok(), result.ok() ? "" : result.message); - return resultToStatus(result); - } catch (const std::exception& e) { - fillFeedback(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} -} // namespace cmvr::service - diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp deleted file mode 100644 index 020d4d9e..00000000 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ /dev/null @@ -1,935 +0,0 @@ -#include "common/base/logging/logger.h" -#include "manager/media_source_hub/include/device_media_source_adapter.h" -// -// Created by xtkuang on 2025/6/1. -// - -#include "../include/grpc_camera_service.h" - -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace cmvr::service; -using namespace cmvr::device; - -namespace { -template -grpc::Status failResponse(ResponseT* response, const std::string& message) { - CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] " << message; - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(message); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; -} - -bool toPtzCommand(cmvr::api::ControlPtzCommand_Command command, PtzCommand& out) -{ - switch (command) { - case cmvr::api::ControlPtzCommand_Command_TILT_UP: - out = PtzCommand::TiltUp; - return true; - case cmvr::api::ControlPtzCommand_Command_TILT_DOWN: - out = PtzCommand::TiltDown; - return true; - case cmvr::api::ControlPtzCommand_Command_PAN_LEFT: - out = PtzCommand::PanLeft; - return true; - case cmvr::api::ControlPtzCommand_Command_PAN_RIGHT: - out = PtzCommand::PanRight; - return true; - case cmvr::api::ControlPtzCommand_Command_UP_LEFT: - out = PtzCommand::UpLeft; - return true; - case cmvr::api::ControlPtzCommand_Command_UP_RIGHT: - out = PtzCommand::UpRight; - return true; - case cmvr::api::ControlPtzCommand_Command_DOWN_LEFT: - out = PtzCommand::DownLeft; - return true; - case cmvr::api::ControlPtzCommand_Command_DOWN_RIGHT: - out = PtzCommand::DownRight; - return true; - case cmvr::api::ControlPtzCommand_Command_ZOOM_IN: - out = PtzCommand::ZoomIn; - return true; - case cmvr::api::ControlPtzCommand_Command_ZOOM_OUT: - out = PtzCommand::ZoomOut; - return true; - case cmvr::api::ControlPtzCommand_Command_PAN_AUTO: - out = PtzCommand::PanAuto; - return true; - default: - return false; - } -} - -// The legacy depth/RGBD RPCs acquire the camera's shared producer directly -// instead of going through MediaSourceHub. Keep that lease exception-safe: -// cancellation, a failed Write(), or any conversion error must release exactly -// the one startStreaming() reference acquired by this call. -class CameraStreamingLease final { -public: - explicit CameraStreamingLease(std::shared_ptr camera) - : camera_(std::move(camera)) { - active_ = camera_ && camera_->startStreaming(); - } - - ~CameraStreamingLease() { - if (!active_ || !camera_) { - return; - } - try { - camera_->stopStreaming(); - } catch (const std::exception& e) { - CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] failed to release camera stream lease: " - << e.what(); - } catch (...) { - CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] failed to release camera stream lease"; - } - } - - CameraStreamingLease(const CameraStreamingLease&) = delete; - CameraStreamingLease& operator=(const CameraStreamingLease&) = delete; - - explicit operator bool() const noexcept { return active_; } - -private: - std::shared_ptr camera_; - bool active_{false}; -}; -} - -gRPCCameraServiceImpl::gRPCCameraServiceImpl( - CameraStreamLowLatencyConfig stream_config) - : dmgr_(DeviceManager::getInstance()), - stream_config_(stream_config) {} - -grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context, - const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetStatus): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Camera device not found: " + dev_id); - } - CameraState state{}; - dev->getState(state); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - response->mutable_state()->set_is_initialized(state.is_initialized); - response->mutable_state()->set_is_opened(state.is_opened); - response->mutable_state()->set_is_streaming(state.is_streaming); - response->mutable_state()->set_is_recording(state.is_recording); - response->mutable_state()->set_is_error(state.is_error); - response->mutable_state()->set_error_message(state.error_message); - response->mutable_state()->set_fps(state.fps); - response->mutable_state()->set_width(state.width); - response->mutable_state()->set_height(state.height); - return grpc::Status::OK; - } - catch(const exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context, - const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartCamera): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Camera device not found: " + dev_id); - } - if (!dev->start()) { - return failResponse(response, "Failed to start camera: " + dev_id); - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - catch (const exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context, - const api::StopCameraCommand_Request* request, api::StopCameraCommand_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StopCamera): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Camera device not found: " + dev_id); - } - if (!dev->stop()) { - return failResponse(response, "Failed to stop camera: " + dev_id); - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, - const api::GetRGBImageCommand_Request* request, api::GetRGBImageCommand_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id; - cv::Mat image; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Camera device not found: " + dev_id); - } - Rs2Intrinsics intrinsics = {0}; - dev->getRGBImage(image,intrinsics); - if (image.empty()) { - return failResponse(response, "Camera returned an empty RGB image: " + dev_id); - } - - response->mutable_intrinsics()->set_fx(intrinsics.fx); - response->mutable_intrinsics()->set_fy(intrinsics.fy); - response->mutable_intrinsics()->set_cx(intrinsics.cx); - response->mutable_intrinsics()->set_cy(intrinsics.cy); - for (int i = 0; i < 5 ; i++) { - response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]); - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - - // Keep the single-frame contract explicit: FrameData::U8C3 is RGB - // byte order, while OpenCV/RealSense images are normally BGR. - if (image.type() == CV_8UC3) { - cv::Mat rgb_image; - cv::cvtColor(image, rgb_image, cv::COLOR_BGR2RGB); - image = std::move(rgb_image); - } - auto imageType = image.type(); - if (imageType == CV_8UC1) { - response->mutable_color_frame()->set_type(api::FrameData::U8C1); - } - else if (imageType == CV_8UC3) { - response->mutable_color_frame()->set_type(api::FrameData::U8C3); - } - else if (imageType == CV_16UC3) { - response->mutable_color_frame()->set_type(api::FrameData::U16C3); - } - else { - return failResponse(response, "unsupported image type"); - } - response->mutable_color_frame()->set_data( - reinterpret_cast(image.data), image.total() * image.elemSize()); - response->mutable_color_frame()->set_height(image.rows); - response->mutable_color_frame()->set_width(image.cols); - response->mutable_color_frame()->set_codec("none"); - return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, - const api::GetDepthImageCommand_Request* request, api::GetDepthImageCommand_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImage): id=" << dev_id; - cv::Mat image; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Camera device not found: " + dev_id); - } - Rs2Intrinsics intrinsics = {0}; - dev->getDepthImage(image,intrinsics); - if (image.empty()) { - return failResponse(response, "Camera returned an empty depth image: " + dev_id); - } - response->mutable_header()->set_success(true); - - response->mutable_intrinsics()->set_fx(intrinsics.fx); - response->mutable_intrinsics()->set_fy(intrinsics.fy); - response->mutable_intrinsics()->set_cx(intrinsics.cx); - response->mutable_intrinsics()->set_cy(intrinsics.cy); - for (int i = 0; i < 5 ; i++) { - response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]); - } - - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - - if (image.type() == CV_8UC1) { - response->mutable_depth_frame()->set_type(api::FrameData::U8C1); - } - else if (image.type() == CV_16UC1) { - response->mutable_depth_frame()->set_type(api::FrameData::U16C1); - } - else if (image.type() == CV_16FC1) { - response->mutable_depth_frame()->set_type(api::FrameData::F16C1); - } - else if (image.type() == CV_32FC1) { - response->mutable_depth_frame()->set_type(api::FrameData::F32C1); - } - else { - return failResponse(response, "unsupported image type"); - } - - response->mutable_depth_frame()->set_data( - reinterpret_cast(image.data), image.total() * image.elemSize()); - response->mutable_depth_frame()->set_height(image.rows); - response->mutable_depth_frame()->set_width(image.cols); - response->mutable_depth_frame()->set_codec("none"); - return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, - const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImages): id=" << dev_id; - cv::Mat color_image, depth_image; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Camera device not found: " + dev_id); - } - Rs2Intrinsics intrinsics = {0}; - dev->getRGBDImages(color_image,depth_image, intrinsics); - if (color_image.empty()) { - return failResponse(response, "Camera returned an empty RGB image: " + dev_id); - } - if (depth_image.empty()) { - return failResponse(response, "Camera returned an empty depth image: " + dev_id); - } - response->mutable_header()->set_success(true); - - response->mutable_intrinsics()->set_fx(intrinsics.fx); - response->mutable_intrinsics()->set_fy(intrinsics.fy); - response->mutable_intrinsics()->set_cx(intrinsics.cx); - response->mutable_intrinsics()->set_cy(intrinsics.cy); - for (int i = 0; i < 5 ; i++) { - response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]); - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - - // Keep the single-frame contract explicit: FrameData::U8C3 is RGB - // byte order, while OpenCV/RealSense images are normally BGR. - if (color_image.type() == CV_8UC3) { - cv::Mat rgb_image; - cv::cvtColor(color_image, rgb_image, cv::COLOR_BGR2RGB); - color_image = std::move(rgb_image); - response->mutable_color_frame()->set_type(api::FrameData::U8C3); - } - else if (color_image.type() == CV_16UC3) { - response->mutable_color_frame()->set_type(api::FrameData::U16C3); - } - else { - return failResponse(response, "unsupported image type"); - } - response->mutable_color_frame()->set_data( - reinterpret_cast(color_image.data), color_image.total() * color_image.elemSize()); - response->mutable_color_frame()->set_height(color_image.rows); - response->mutable_color_frame()->set_width(color_image.cols); - response->mutable_color_frame()->set_codec("none"); - - // depth image - if (depth_image.type() == CV_8UC1) { - response->mutable_depth_frame()->set_type(api::FrameData::U8C1); - } - else if (depth_image.type() == CV_16UC1) { - response->mutable_depth_frame()->set_type(api::FrameData::U16C1); - } - else if (depth_image.type() == CV_16FC1) { - response->mutable_depth_frame()->set_type(api::FrameData::F16C1); - } - else if (depth_image.type() == CV_32FC1) { - response->mutable_depth_frame()->set_type(api::FrameData::F32C1); - } - else { - return failResponse(response, "unsupported depth image type"); - } - response->mutable_depth_frame()->set_data( - reinterpret_cast(depth_image.data), depth_image.total() * depth_image.elemSize()); - response->mutable_depth_frame()->set_height(depth_image.rows); - response->mutable_depth_frame()->set_width(depth_image.cols); - response->mutable_depth_frame()->set_codec("none"); - return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context, - const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StartRecording): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Camera device not found: " + dev_id); - } - dev->startRecording(request->video_path()); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCCameraServiceImpl::StopRecording(grpc::ServerContext* context, - const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (StopRecording): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Camera device not found: " + dev_id); - } - dev->stopRecording(); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context, - const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (ControlPtz): id=" << dev_id - << ", command=" << request->command() - << ", action=" << request->action() - << ", speed=" << request->speed(); - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Camera device not found: " + dev_id); - } - - PtzCommand command{}; - if (!toPtzCommand(request->command(), command)) { - return failResponse(response, "Invalid PTZ command"); - } - - if (request->action() != api::ControlPtzCommand_Action_START && - request->action() != api::ControlPtzCommand_Action_STOP) { - return failResponse(response, "Invalid PTZ action"); - } - const bool stop = request->action() == api::ControlPtzCommand_Action_STOP; - if (!dev->controlPtz(command, stop, static_cast(request->speed()))) { - CameraState state{}; - dev->getState(state); - const std::string error_message = - state.error_message.empty() ? "Failed to control PTZ: " + dev_id : state.error_message; - return failResponse(response, error_message); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - catch (exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - - -grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* context - , grpc::ServerReaderWriter* stream){ - try { - //读取首次传递的数据，获取设备id - api::GetDepthImageStreamCommand_Request request; - if (!stream->Read(&request)) { - return grpc::Status::OK; - } - string dev_id = request.header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start,id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - api::GetDepthImageStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message("Camera device not found: " + dev_id); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } - CameraStreamingLease stream_lease(dev); - if (!stream_lease) { - api::GetDepthImageStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message("Failed to start camera stream: " + dev_id); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } - - int nFrameCount = 0; - size_t index = 0; - while (true) - { - if (context->IsCancelled()) - { - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; - break; - } - - api::GetDepthImageStreamCommand_Feedback response; - cmvr::device::StreamFrameData frame_data; - if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) && - !frame_data.depthFrame.empty()) { - response.mutable_header()->set_success(true); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - response.mutable_depth_frame()->set_type(api::FrameData::U16C1); - response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size()); - response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey); - response.mutable_depth_frame()->set_codec("none"); - response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width); - response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height); - - response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); - response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy); - response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx); - response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy); - for (int i = 0; i < 5 ; i++) { - response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]); - } - - response.set_seq_no(nFrameCount++); - - if (!stream->Write(response)) { - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; - break; - } - } - } - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): end,id=" << dev_id; - return grpc::Status::OK; - } - catch (const exception &e) { - api::GetDepthImageStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } -} -grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* context - , grpc::ServerReaderWriter* stream){ - try { - //读取首次传递的数据，获取设备id - api::GetRGBDImagesStreamCommand_Request request; - if (!stream->Read(&request)) { - return grpc::Status::OK; - } - string dev_id = request.header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start,id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - api::GetRGBDImagesStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message("Camera device not found: " + dev_id); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } - CameraStreamingLease stream_lease(dev); - if (!stream_lease) { - api::GetRGBDImagesStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message("Failed to start camera stream: " + dev_id); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } - - int nFrameCount = 0; - size_t index = 0; - while (true) - { - if (context->IsCancelled()) - { - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBDImagesStream) context is cancelled,id=" << dev_id; - break; - } - - api::GetRGBDImagesStreamCommand_Feedback response; - cmvr::device::StreamFrameData frame_data; - if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) && - !frame_data.rgbFrame.empty() && - !frame_data.depthFrame.empty()) { - response.mutable_header()->set_success(true); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size()); - response.mutable_color_frame()->set_is_key_frame(frame_data.bKey); - response.mutable_color_frame()->set_codec(frame_data.codec); - response.mutable_color_frame()->set_width(frame_data.width); - response.mutable_color_frame()->set_height(frame_data.height); - - response.mutable_depth_frame()->set_type(api::FrameData::U16C1); - response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size()); - response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey); - response.mutable_depth_frame()->set_codec("none"); - response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width); - response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height); - - response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); - response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy); - response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx); - response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy); - for (int i = 0; i < 5 ; i++) { - response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]); - } - - response.set_seq_no(nFrameCount++); - - if (!stream->Write(response)) { - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; - break; - } - } - } - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id; - return grpc::Status::OK; - } - catch (const exception &e) { - api::GetRGBDImagesStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } -} -grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream){ - try { - //读取首次传递的数据，获取设备id - api::GetRGBImageStreamCommand_Request request; - if (!stream->Read(&request)) { - return grpc::Status::OK; - } - string dev_id = request.header().device_id(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id - << ", max_pending_frames=" << stream_config_.max_pending_frames - << ", max_frame_age_ms=" << stream_config_.max_frame_age.count(); - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - api::GetRGBImageStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message("Camera device not found: " + dev_id); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } - auto& media_hub = cmvr::media::globalMediaSourceHub(); - const std::string track_id = cmvr::media::cameraColorTrackId(dev_id); - if (!cmvr::media::ensureCameraMediaSource(media_hub, dev)) { - api::GetRGBImageStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message("Failed to register camera media source: " + dev_id); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } - auto subscription = media_hub.subscribe( - track_id, - cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED, - [context] { return context->IsCancelled(); }); - if (!subscription) { - api::GetRGBImageStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message("Failed to subscribe camera media source: " + dev_id); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } - - std::atomic client_eof_requested{false}; - std::atomic request_stream_closed{false}; - std::atomic control_requests_read{1}; - std::thread request_reader([&] { - api::GetRGBImageStreamCommand_Request control_request; - while (stream->Read(&control_request)) { - ++control_requests_read; - if (control_request.eof()) { - client_eof_requested.store(true, std::memory_order_release); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] client requested RGB stream EOF" - << ", id=" << dev_id - << ", peer=" << context->peer() - << ", control_requests=" << control_requests_read.load(); - break; - } - } - request_stream_closed.store(true, std::memory_order_release); - }); - struct RequestReaderJoiner { - std::thread& thread; - ~RequestReaderJoiner() { - if (thread.joinable()) { - thread.join(); - } - } - } request_reader_joiner{request_reader}; - const auto join_request_reader = [&] { - if (request_reader.joinable()) { - request_reader.join(); - } - }; - - bool waiting_for_key_frame = true; - const char* exit_reason = "unknown"; - auto last_key_frame_request = std::chrono::steady_clock::now(); - auto last_latency_log = std::chrono::steady_clock::time_point{}; - uint64_t discarded_since_log = 0; - std::chrono::microseconds last_write_duration{0}; - media_hub.requestKeyFrame(track_id); - const auto request_key_frame_if_due = [&] { - const auto now = std::chrono::steady_clock::now(); - if (now - last_key_frame_request >= std::chrono::milliseconds(500)) { - media_hub.requestKeyFrame(track_id); - last_key_frame_request = now; - } - }; - const auto request_key_frame_now = [&] { - waiting_for_key_frame = true; - media_hub.requestKeyFrame(track_id); - last_key_frame_request = std::chrono::steady_clock::now(); - }; - const auto log_latency_event = [&]( - const char* reason, - const uint64_t discarded, - const uint64_t frame_age_ns, - const std::chrono::microseconds write_duration) { - discarded_since_log += discarded; - const auto now = std::chrono::steady_clock::now(); - if (last_latency_log != std::chrono::steady_clock::time_point{} && - now - last_latency_log < std::chrono::seconds(1)) { - return; - } - const double frame_age_ms = static_cast(frame_age_ns) / 1'000'000.0; - const double write_ms = static_cast(write_duration.count()) / 1'000.0; - CMVR_LOG(WARNING) << "[gRPCCameraServiceImpl] low-latency camera stream event" - << ", id=" << dev_id - << ", reason=" << reason - << ", discarded=" << discarded_since_log - << ", age_ms=" << frame_age_ms - << ", write_ms=" << write_ms - << ", max_pending_frames=" - << stream_config_.max_pending_frames - << ", max_frame_age_ms=" - << stream_config_.max_frame_age.count(); - discarded_since_log = 0; - last_latency_log = now; - }; - while (true) - { - if (client_eof_requested.load(std::memory_order_acquire)) { - exit_reason = "client_eof"; - break; - } - if (context->IsCancelled()) - { - exit_reason = "context_cancelled"; - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled" - << ", id=" << dev_id - << ", peer=" << context->peer() - << ", client_eof=" << client_eof_requested.load() - << ", request_stream_closed=" << request_stream_closed.load(); - break; - } - - const auto read = subscription.waitRead(std::chrono::milliseconds(100)); - if (!read || !read->value || read->value->empty()) { - if (!subscription.valid()) { - exit_reason = "subscription_invalid"; - break; - } - if (waiting_for_key_frame) { - request_key_frame_if_due(); - } - continue; - } - const auto& frame = *read->value; - const auto descriptor = frame.descriptor; - if (!descriptor) { - continue; - } - const uint64_t now_ns = static_cast( - std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()).count()); - const auto frame_age = cameraFrameAgeNs(frame.capture_time_ns, now_ns); - if (frame_age && - cameraFrameExceedsAgeLimit( - frame.capture_time_ns, - now_ns, - stream_config_.max_frame_age)) { - // This frame is already outside the latency budget. Flush all - // currently queued frames and wait for a fresh IDR; sending any - // P/B frame after an intentional gap would break decoder continuity. - const uint64_t discarded = - 1 + subscription.discardPendingIfExceeds(0); - request_key_frame_now(); - log_latency_event( - "stale_frame", - discarded, - *frame_age, - last_write_duration); - continue; - } - const bool inter_frame_codec = descriptor->codec == cmvr::media::Codec::H264 || - descriptor->codec == cmvr::media::Codec::H265; - if (!inter_frame_codec || descriptor->payload_format != cmvr::media::PayloadFormat::ANNEX_B) { - api::GetRGBImageStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message( - "Unsupported camera stream codec or payload format: " + dev_id); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - exit_reason = "unsupported_stream"; - break; - } - if (read->dropped_since_last_read > 0 || read->generation_changed || frame.discontinuity) { - request_key_frame_now(); - } - if (waiting_for_key_frame && !frame.key_frame) { - request_key_frame_if_due(); - continue; - } - waiting_for_key_frame = false; - - api::GetRGBImageStreamCommand_Feedback response; - response.mutable_header()->set_success(true); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - response.mutable_color_frame()->set_data(frame.data(), frame.size()); - response.mutable_color_frame()->set_is_key_frame(frame.key_frame); - response.mutable_color_frame()->set_codec( - descriptor->codec == cmvr::media::Codec::H264 ? "h264" : - descriptor->codec == cmvr::media::Codec::H265 ? "h265" : "unknown"); - response.mutable_color_frame()->set_width(static_cast(descriptor->width)); - response.mutable_color_frame()->set_height(static_cast(descriptor->height)); - response.mutable_color_frame()->set_capture_utc_ns(frame.capture_utc_ns); - response.mutable_color_frame()->set_source_sequence(frame.sequence); - response.mutable_color_frame()->set_pts(frame.pts); - response.mutable_color_frame()->set_dts(frame.dts); - response.mutable_color_frame()->set_source_fps(descriptor->nominal_rate); - response.mutable_color_frame()->set_source_timestamp(frame.source_timestamp); - response.mutable_color_frame()->set_source_frame_number(frame.source_frame_number); - - response.mutable_intrinsics()->set_fx(descriptor->fx); - response.mutable_intrinsics()->set_fy(descriptor->fy); - response.mutable_intrinsics()->set_cx(descriptor->cx); - response.mutable_intrinsics()->set_cy(descriptor->cy); - for (const float coefficient : descriptor->distortion) { - response.mutable_intrinsics()->add_coeffs(coefficient); - } - - response.set_seq_no(static_cast(std::min( - frame.sequence, - static_cast(std::numeric_limits::max())))); - - const auto write_started = std::chrono::steady_clock::now(); - if (!stream->Write(response)) { - exit_reason = "write_failed"; - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed" - << ", id=" << dev_id - << ", peer=" << context->peer() - << ", context_cancelled=" << context->IsCancelled() - << ", client_eof=" << client_eof_requested.load() - << ", request_stream_closed=" << request_stream_closed.load() - << ", control_requests=" << control_requests_read.load() - << ", write_ms=" - << std::chrono::duration_cast( - std::chrono::steady_clock::now() - write_started).count() / 1000.0; - break; - } - last_write_duration = std::chrono::duration_cast( - std::chrono::steady_clock::now() - write_started); - - // A successful synchronous Write may have been flow-controlled long - // enough for the source to outpace this consumer. Once the pending - // count crosses the configured trigger, discard the whole pending - // batch and require a fresh key frame before resuming. - const uint64_t discarded = subscription.discardPendingIfExceeds( - stream_config_.max_pending_frames); - if (discarded > 0) { - request_key_frame_now(); - log_latency_event( - "write_backpressure", - discarded, - frame_age.value_or(0), - last_write_duration); - } - } - join_request_reader(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end" - << ", id=" << dev_id - << ", reason=" << exit_reason - << ", peer=" << context->peer() - << ", context_cancelled=" << context->IsCancelled() - << ", client_eof=" << client_eof_requested.load() - << ", request_stream_closed=" << request_stream_closed.load() - << ", control_requests=" << control_requests_read.load(); - return grpc::Status::OK; - } - catch (const exception &e) { - api::GetRGBImageStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } -} diff --git a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp deleted file mode 100644 index 84bf5003..00000000 --- a/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp +++ /dev/null @@ -1,484 +0,0 @@ -#include "common/base/logging/logger.h" -// -// Created by linbo on 2025/7/3. -// - -#include "../include/grpc_dexhand_service.h" - -#include -#include -#include -#include -#include - -#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h" - -using namespace std; -using namespace cmvr::service; -using namespace cmvr::device; - -#define DEXHAND_MAX_POSITION 2000 -#define DEXHAND_MAX_ANGLE 1000 -#define DEXHAND_MAX_FORCE 3000 -#define DEXHAND_MAX_SPEED 1000 - -namespace { - -constexpr int kDexHandDofCount = 6; - -cmvr::api::SensorData::FingerType toProtoFingerType(const AbstractDexHand::FingerType finger) { - switch (finger) { - case AbstractDexHand::FingerType::PINKY: - return cmvr::api::SensorData::PINKY; - case AbstractDexHand::FingerType::RING: - return cmvr::api::SensorData::RING; - case AbstractDexHand::FingerType::MIDDLE: - return cmvr::api::SensorData::MIDDLE_FINGER; - case AbstractDexHand::FingerType::INDEX: - return cmvr::api::SensorData::INDEX; - case AbstractDexHand::FingerType::THUMB: - return cmvr::api::SensorData::THUMB; - case AbstractDexHand::FingerType::PALM: - return cmvr::api::SensorData::PALM; - } - return cmvr::api::SensorData::PINKY; -} - -cmvr::api::SensorData::PartType toProtoPartType(const AbstractDexHand::TactileRegion region) { - switch (region) { - case AbstractDexHand::TactileRegion::TIP: - return cmvr::api::SensorData::TIP; - case AbstractDexHand::TactileRegion::FINGER: - return cmvr::api::SensorData::FINGER; - case AbstractDexHand::TactileRegion::PAD: - return cmvr::api::SensorData::PAD; - case AbstractDexHand::TactileRegion::THUMB_MIDDLE: - return cmvr::api::SensorData::THUMB_MIDDLE; - case AbstractDexHand::TactileRegion::PALM_PAD: - return cmvr::api::SensorData::PALM_PAD; - } - return cmvr::api::SensorData::TIP; -} - -void fillSensorData(const AbstractDexHand::TactileRegionData& tactile_data, - cmvr::api::SensorData* sensor_data) { - sensor_data->set_rows(tactile_data.view.rows); - sensor_data->set_cols(tactile_data.view.cols); - sensor_data->set_finger_type(toProtoFingerType(tactile_data.finger)); - sensor_data->set_part_type(toProtoPartType(tactile_data.region)); - sensor_data->set_sensor_name(tactile_data.name == nullptr ? "" : tactile_data.name); - - for (int row = 0; row < tactile_data.view.rows; ++row) { - auto* row_data = sensor_data->add_data(); - const auto* values = tactile_data.view.rowData(row); - for (int col = 0; col < tactile_data.view.cols; ++col) { - // Keep the existing scalar wire format by exposing the normal-force projection. - row_data->add_values(static_cast(values[col].fz)); - } - } -} - -template -void appendSensorData(const std::vector& tactile_regions, - ResponseT* response) { - for (const auto& tactile_region : tactile_regions) { - if (!tactile_region.valid()) { - continue; - } - fillSensorData(tactile_region, response->add_sensor()); - } -} - -template -bool applyFreedomValues(const FreedomCollection& freedoms, - const int scale, - std::vector& targets, - std::string* error_message) { - for (const auto& freedom : freedoms) { - if (freedom.id() < 0 || freedom.id() >= static_cast(targets.size())) { - if (error_message) { - *error_message = "Invalid dexhand DOF id: " + std::to_string(freedom.id()); - } - return false; - } - if (!std::isfinite(freedom.value())) { - if (error_message) { - *error_message = "Invalid dexhand command value: not finite."; - } - return false; - } - targets[static_cast(freedom.id())] = static_cast(freedom.value() * scale); - } - return true; -} - -template -grpc::Status failResponse(ResponseT* response, const std::string& message) { - CMVR_LOG(ERROR) << "[gRPCDexHandServiceImpl] " << message; - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(message); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; -} - -std::vector readCurrentAngles(const std::shared_ptr& dev) { - DexHandState state{}; - dev->getState(state); - - std::vector current_angles(static_cast(kDexHandDofCount), 0); - for (int i = 0; i < kDexHandDofCount; ++i) { - current_angles[static_cast(i)] = state.hands[i].angle; - } - return current_angles; -} - -bool respondUnsupportedForRh56(const std::shared_ptr& dev, - const char* rpc_name, - const char* hint, - cmvr::api::CommandHeader_Feedback* header) { - if (std::dynamic_pointer_cast(dev) == nullptr) { - return false; - } - - header->set_success(false); - header->set_error_message(std::string(rpc_name) + " is not supported by RH56DFTPDexhand. " + hint); - setCurrentTimestamp(header->mutable_timestamp()); - return true; -} - -std::vector buildRh56AllTactileRegions() { - using FingerType = AbstractDexHand::FingerType; - using TactileRegion = AbstractDexHand::TactileRegion; - - return { - {FingerType::PINKY, TactileRegion::TIP}, - {FingerType::PINKY, TactileRegion::FINGER}, - {FingerType::PINKY, TactileRegion::PAD}, - {FingerType::RING, TactileRegion::TIP}, - {FingerType::RING, TactileRegion::FINGER}, - {FingerType::RING, TactileRegion::PAD}, - {FingerType::MIDDLE, TactileRegion::TIP}, - {FingerType::MIDDLE, TactileRegion::FINGER}, - {FingerType::MIDDLE, TactileRegion::PAD}, - {FingerType::INDEX, TactileRegion::TIP}, - {FingerType::INDEX, TactileRegion::FINGER}, - {FingerType::INDEX, TactileRegion::PAD}, - {FingerType::THUMB, TactileRegion::TIP}, - {FingerType::THUMB, TactileRegion::FINGER}, - {FingerType::THUMB, TactileRegion::THUMB_MIDDLE}, - {FingerType::THUMB, TactileRegion::PAD}, - {FingerType::PALM, TactileRegion::PALM_PAD} - }; -} - -void maybeConfigureRh56FullTactilePolling(const std::shared_ptr& dev) { - auto rh56 = std::dynamic_pointer_cast(dev); - if (!rh56) { - return; - } - rh56->setTactilePollingRegions(buildRh56AllTactileRegions()); -} - -} // namespace - -gRPCDexHandServiceImpl::gRPCDexHandServiceImpl(): dmgr_(DeviceManager::getInstance()) {} - -grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context, - const api::GetDexHandStateCommand_Request* request, api::GetDexHandStateCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetStatus): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "DexHand device not found: " + dev_id); - } - DexHandState state{}; - dev->getState(state); - response->mutable_state()->set_is_initialized(state.is_initialized); - for (int i = 0; i < kDexHandDofCount; i++) { - auto hand = response->mutable_state()->add_hands(); - hand->set_dof_id(i); - hand->set_angle(state.hands[i].angle); - hand->set_current(state.hands[i].current); - hand->set_force(state.hands[i].force); - hand->set_position(state.hands[i].position); - hand->set_speed(state.hands[i].speed); - hand->set_temperature(state.hands[i].temperature); - hand->set_error(state.hands[i].error); - for (auto& errormessage : state.hands[i].error_message) { - hand->add_error_message(errormessage); - } - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetStatus): success, id=" << dev_id - << ", initialized=" << state.is_initialized - << ", dof=" << response->state().hands_size(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context - , const cmvr::api::SetDexHandPositionsCommand_Request* request - , cmvr::api::SetDexHandPositionsCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPos): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "DexHand device not found: " + dev_id); - } - if (respondUnsupportedForRh56(dev, - "SetDexHandPos", - "Use SetDexHandAngle for RH56 joint commands.", - response->mutable_header())) { - return grpc::Status::OK; - } - - std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); - std::string error_message; - if (!applyFreedomValues(request->values(), DEXHAND_MAX_POSITION, finger_joint_targets, &error_message)) { - return failResponse(response, error_message); - } - dev->setPositions(finger_joint_targets); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPos): success, id=" << dev_id - << ", values=" << request->values_size(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* context - , const cmvr::api::SetDexHandAnglesCommand_Request* request - , cmvr::api::SetDexHandAnglesCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "DexHand device not found: " + dev_id); - } - - if (const auto rh56 = std::dynamic_pointer_cast(dev)) { - std::vector finger_joint_targets = readCurrentAngles(dev); - std::string error_message; - if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) { - return failResponse(response, error_message); - } - rh56->setAngles(finger_joint_targets); - } else { - std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); - std::string error_message; - if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) { - return failResponse(response, error_message); - } - dev->setAngles(finger_joint_targets); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): success, id=" << dev_id - << ", values=" << request->values_size(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* context - , const cmvr::api::SetDexHandForceCommand_Request* request - , cmvr::api::SetDexHandForceCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandForce): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "DexHand device not found: " + dev_id); - } - if (respondUnsupportedForRh56(dev, - "SetDexHandForce", - "RH56DFTPDexhand currently exposes angle and tactile APIs only.", - response->mutable_header())) { - return grpc::Status::OK; - } - - std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); - std::string error_message; - if (!applyFreedomValues(request->values(), DEXHAND_MAX_FORCE, finger_joint_targets, &error_message)) { - return failResponse(response, error_message); - } - dev->setForce(finger_joint_targets); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandForce): success, id=" << dev_id - << ", values=" << request->values_size(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* context - , const cmvr::api::SetDexHandSpeedCommand_Request* request - , cmvr::api::SetDexHandSpeedCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "DexHand device not found: " + dev_id); - } - if (respondUnsupportedForRh56(dev, - "SetDexHandSpeed", - "RH56DFTPDexhand currently exposes angle and tactile APIs only.", - response->mutable_header())) { - return grpc::Status::OK; - } - - std::vector finger_joint_targets(static_cast(kDexHandDofCount), -1); - std::string error_message; - if (!applyFreedomValues(request->values(), DEXHAND_MAX_SPEED, finger_joint_targets, &error_message)) { - return failResponse(response, error_message); - } - dev->setVelocities(finger_joint_targets); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): success, id=" << dev_id - << ", values=" << request->values_size(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* context - , const cmvr::api::SetDexHandPresetActCommand_Request* request - , cmvr::api::SetDexHandPresetActCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "DexHand device not found: " + dev_id); - } - if (respondUnsupportedForRh56(dev, - "SetDexHandPresetAct", - "RH56DFTPDexhand currently exposes angle and tactile APIs only.", - response->mutable_header())) { - return grpc::Status::OK; - } - - auto presetActId = request->presetactid(); - dev->setPresetAct(presetActId); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): success, id=" << dev_id - << ", preset_act_id=" << presetActId; - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context - , const cmvr::api::GetSensorDataCommand_Request* request - , cmvr::api::GetSensorDataCommand_Feedback* response) { - - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorData): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "DexHand device not found: " + dev_id); - } - maybeConfigureRh56FullTactilePolling(dev); - appendSensorData(dev->getSensorData(), response); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorData): success, id=" << dev_id - << ", sensors=" << response->sensor_size(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* context - , grpc::ServerReaderWriter* stream) -{ - try { - api::GetSensorDataStreamCommand_Request request; - if (!stream->Read(&request)) { - return grpc::Status::OK; - } - - string dev_id = request.header().device_id(); - CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): start,id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - api::GetSensorDataStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message("DexHand device not found: " + dev_id); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; - } - maybeConfigureRh56FullTactilePolling(dev); - CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): streaming success, id=" << dev_id; - - while (!context->IsCancelled()) - { - api::GetSensorDataStreamCommand_Feedback response; - appendSensorData(dev->getSensorData(), &response); - response.mutable_header()->set_success(true); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - - if (!stream->Write(response)) { - CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (stream->Write) failed,id=" << dev_id; - break; - } - - std::this_thread::sleep_for(std::chrono::milliseconds(33)); - } - CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): finished, id=" << dev_id; - return grpc::Status::OK; - } - catch (const std::exception& e) { - CMVR_LOG(ERROR) << "[gRPCDexHandServiceImpl] (GetSensorDataStream) exception: " << e.what(); - return grpc::Status::OK; - } - -} diff --git a/cmvr-es/service/grpc/src/grpc_head_service.cpp b/cmvr-es/service/grpc/src/grpc_head_service.cpp deleted file mode 100644 index 6ad3aff8..00000000 --- a/cmvr-es/service/grpc/src/grpc_head_service.cpp +++ /dev/null @@ -1,475 +0,0 @@ -#include "common/base/logging/logger.h" -#include "../include/grpc_head_service.h" - -#include "cmvr/api/biohead_service.grpc.pb.h" -#include "manager/device_manager/include/device_manager.h" -#include "common/base/grpc_utils.h" -#include "biohead/biohead_esp32/include/biohead_esp32.h" -#include -#include -#include - -using namespace std; -using namespace cmvr::service; -using namespace cmvr::device; -using namespace cmvr::api; - -namespace { -template -grpc::Status failResponse(ResponseT* response, const std::string& message) { - CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] " << message; - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(message); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; -} - -void logSuccess(const char* rpc_name, const std::string& device_id) { - CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (" << rpc_name - << "): success, id=" << device_id; -} -} - -gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl() - : dmgr_(DeviceManager::getInstance()) {} - - -// 设置表情（一次性） -grpc::Status gRPCMBioHeadServiceImpl::SetExpression( - grpc::ServerContext* context, - const SetFacialExpression_Request* request, - SetFacialExpression_Feedback* response) { - - try { - std::string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - FacialExpressionState& expression_state = robot->expression_state_; - - expression_state.left_eyebrow_outside_y = request->expression().eyebrow().left_outside_y(); - expression_state.left_eyebrow_inside_y = request->expression().eyebrow().left_inside_y(); - expression_state.right_eyebrow_outside_y = request->expression().eyebrow().right_outside_y(); - expression_state.right_eyebrow_inside_y = request->expression().eyebrow().right_inside_y(); - - expression_state.left_eye_upper_lid_y = request->expression().eyelid().left_upper_y(); - expression_state.left_eye_lower_lid_y = request->expression().eyelid().left_lower_y(); - expression_state.right_eye_upper_lid_y = request->expression().eyelid().right_upper_y(); - expression_state.right_eye_lower_lid_y = request->expression().eyelid().right_lower_y(); - - expression_state.left_eye_ball_x = request->expression().eyeball().left_x(); - expression_state.left_eye_ball_y = request->expression().eyeball().left_y(); - expression_state.right_eye_ball_x = request->expression().eyeball().right_x(); - expression_state.right_eye_ball_y = request->expression().eyeball().right_y(); - - expression_state.left_nose_y = request->expression().nose().left_y(); - expression_state.right_nose_y = request->expression().nose().right_y(); - - expression_state.upper_lip_y = request->expression().mouth().upper_lip_y(); - expression_state.lower_lip_y = request->expression().mouth().lower_lip_y(); - - robot->setExpressionPose(expression_state); - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("SetExpression", dev_id); - return grpc::Status::OK; - } catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - - -// 流式控制接口 -grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream) -{ - StreamFacialExpression_Feedback feedback_msg; - std::string dev_id; - std::shared_ptr robot; - bool first_message = true; - - try { - StreamFacialExpression_Request request_msg; - constexpr float control_frequency = 10; - const auto time_interval = std::chrono::milliseconds(static_cast(1000 / control_frequency)); - auto last_control_time = std::chrono::steady_clock::now(); - - CMVR_LOG(INFO) << "StreamExpression started."; - - while (stream->Read(&request_msg)) { - if (first_message) { - dev_id = request_msg.header().device_id(); - if (dev_id.empty()) { - CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] Device ID is empty in first message"; - feedback_msg.mutable_header()->set_success(false); - feedback_msg.mutable_header()->set_error_message("Device ID is empty in first message"); - setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp()); - stream->Write(feedback_msg); - return grpc::Status::OK; - } - robot = dmgr_.getDevice(dev_id); - if (!robot) { - const std::string message = "Biohead device not found: " + dev_id; - CMVR_LOG(ERROR) << "[gRPCMBioHeadServiceImpl] " << message; - feedback_msg.mutable_header()->set_success(false); - feedback_msg.mutable_header()->set_error_message(message); - setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp()); - stream->Write(feedback_msg); - return grpc::Status::OK; - } - - // ✅ 重置紧急停止标志 - robot->emergency_stop_requested = false; - - first_message = false; - CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): streaming success, id=" << dev_id; - } - - // ✅ 如果紧急停止触发，直接退出 - if (robot->emergency_stop_requested) { - CMVR_LOG(WARNING) << "[Stream] Emergency stop requested. Terminating stream for device: " << dev_id; - break; - } - - auto current_time = std::chrono::steady_clock::now(); - auto elapsed_time = std::chrono::duration_cast(current_time - last_control_time); - if (elapsed_time < time_interval) continue; - - FacialExpressionState expression_state; - - // 眉毛 - expression_state.left_eyebrow_outside_y = request_msg.expr().eyebrow().left_outside_y(); - expression_state.left_eyebrow_inside_y = request_msg.expr().eyebrow().left_inside_y(); - expression_state.right_eyebrow_outside_y = request_msg.expr().eyebrow().right_outside_y(); - expression_state.right_eyebrow_inside_y = request_msg.expr().eyebrow().right_inside_y(); - - // 眼睑 - expression_state.left_eye_upper_lid_y = request_msg.expr().eyelid().left_upper_y(); - expression_state.left_eye_lower_lid_y = request_msg.expr().eyelid().left_lower_y(); - expression_state.right_eye_upper_lid_y = request_msg.expr().eyelid().right_upper_y(); - expression_state.right_eye_lower_lid_y = request_msg.expr().eyelid().right_lower_y(); - - // 眼球 - expression_state.left_eye_ball_y = request_msg.expr().eyeball().left_y(); - expression_state.right_eye_ball_y = request_msg.expr().eyeball().right_y(); - - // 鼻子 - expression_state.left_nose_y = request_msg.expr().nose().left_y(); - expression_state.right_nose_y = request_msg.expr().nose().right_y(); - - // 嘴部 - expression_state.upper_lip_y = request_msg.expr().mouth().upper_lip_y(); - expression_state.lower_lip_y = request_msg.expr().mouth().lower_lip_y(); - - // 嘴角 - expression_state.left_corner_lip_x = request_msg.expr().mouth().left_lip().upper_y(); - expression_state.left_corner_lip_y = request_msg.expr().mouth().left_lip().corner_y(); - expression_state.lower_left_lip_y = request_msg.expr().mouth().left_lip().lower_y(); - - expression_state.right_corner_lip_x = request_msg.expr().mouth().right_lip().upper_y(); - expression_state.lower_right_lip_y = request_msg.expr().mouth().right_lip().corner_y(); - expression_state.right_corner_lip_y = request_msg.expr().mouth().right_lip().lower_y(); - - // 下巴 - expression_state.jaw_x = request_msg.expr().jaw().x(); - expression_state.jaw_y = request_msg.expr().jaw().y(); - - robot->streamFacialPose(expression_state, 0, 0); - last_control_time = current_time; - - feedback_msg.mutable_header()->set_success(true); - feedback_msg.mutable_header()->clear_error_message(); - setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp()); - if (!stream->Write(feedback_msg)) break; - } - - CMVR_LOG(INFO) << "StreamExpression finished for device: " << dev_id; - CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): finished, id=" << dev_id; - return grpc::Status::OK; - } catch (const std::exception& e) { - CMVR_LOG(ERROR) << "StreamExpression error: " << e.what(); - feedback_msg.mutable_header()->set_success(false); - feedback_msg.mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(feedback_msg.mutable_header()->mutable_timestamp()); - if (stream) stream->Write(feedback_msg); - return grpc::Status::OK; - } -} - - -// 获取设备状态 -grpc::Status gRPCMBioHeadServiceImpl::GetSystemStatus( - grpc::ServerContext* context, - const GetStatus_Request* request, - GetStatus_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("GetSystemStatus", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - - -// 紧急停止 -grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop( - grpc::ServerContext* context, - const EmergencyStop_Request* request, - EmergencyStop_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - robot->eStop(); // 停止执行 - robot->emergency_stop_requested = true; // ✅ 设置中断标志 - - - - - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("EmergencyStop", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response) -{ - - { - - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - - - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - robot->speakstart(); // kaish开始 - - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("SpeakStart", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - } -} - - - -grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - robot->speakstop(); // 停止执行 - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("SpeakStop", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - - - -grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - robot->expressionHappy(); // 停止执行 - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("Happy", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - robot->expressionSurprised(); // - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("Surprise", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - - -grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - robot->expressionTired(); // 停止执行 - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("ExpressionTired", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - - - -grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - robot->expressionAngry(); // 停止执行 - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("ExpressionAngry", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - - - -grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - robot->expressionSadness(); // 停止执行 - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("ExpressionSadness", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - - -grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response) -{ - try { - string dev_id = request->header().device_id(); - auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse(response, "Biohead device not found: " + dev_id); - } - - robot->expressionYawn(); // 停止执行 - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - logSuccess("ExpressionYawn", dev_id); - return grpc::Status::OK; - } - catch (const exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} diff --git a/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp b/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp deleted file mode 100644 index 3c564f37..00000000 --- a/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// -// Created by lgv on 2025/8/25. -// -#include "gtest/gtest.h" -#include "common/base/logging/logger.h" -#include -#include "../include/grpc_hlc_service.h" -#include "google/protobuf/timestamp.pb.h" -#include -#include - -using namespace cmvr::api; - -TEST(GrpcHlcClientTest, MyTest) { - // 连接服务端 - auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials()); - auto stub = cmvr::api::HlcService::NewStub(channel); - - - grpc::ClientContext context; - cmvr::api::Touch_Request request; - cmvr::api::Touch_Response response; - - - request.mutable_header()->set_device_id("hc01"); - *request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime(); - - - request.set_u(600); - request.set_v(360); - request.set_max_force(1300); - - - - // 调用 - grpc::Status status = stub->touch(&context, request, &response); - - if (status.ok()) { - CMVR_LOG(INFO) << "Touch RPC succeeded." << std::endl; - CMVR_LOG(INFO) << "Success: " << response.mutable_header()->success() << std::endl; - CMVR_LOG(INFO) << "Error message: " << response.mutable_header()->error_message() << std::endl; - CMVR_LOG(INFO) << "Timestamp: " << response.mutable_header()->timestamp().seconds() << std::endl; - } else { - CMVR_LOG(ERROR) << "Touch RPC failed: " << status.error_message() << std::endl; - } -} \ No newline at end of file diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp deleted file mode 100644 index 5edc18b0..00000000 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// -// Created by lgv on 2025/8/25. -// - - -#include "../include/grpc_hlc_service.h" - -#include -#include -#include - -#include - -#include "common/base/logging/logger.h" -#include "manager/task_manager/include/task_manager.h" -#include "task/touch_screen_task/include/touch_screen_task.h" - - -using namespace cmvr::service; -using namespace cmvr::api; -using google::protobuf::util::TimeUtil; - -namespace { - -std::string buildTouchFailureMessage(const cmvr::task::TouchScreenTask& task, - const std::string& prefix) { - return prefix + ", phase=" + - cmvr::task::TouchScreenTask::phaseToString(task.phase()) + - ", status=" + - cmvr::task::TouchScreenTask::statusToString(task.lastStatus()); -} - -void fillTouchResponse(Touch_Response* response, - const bool success, - const std::string& error_message) { - response->mutable_header()->set_success(success); - response->mutable_header()->set_error_message(error_message); - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); -} - -} // namespace - -gRPCHlcServiceImpl::gRPCHlcServiceImpl() = default; - -grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) { - try { - auto touch_task = task::TaskManager::getInstance().getTouchScreenTask(); - if (!touch_task) { - const std::string error = "TouchScreenTask not found or not initialized"; - fillTouchResponse(response, false, error); - return grpc::Status(grpc::StatusCode::NOT_FOUND, error); - } - - if (!touch_task->touch(request->u(), request->v())) { - const std::string error = - buildTouchFailureMessage(*touch_task, "TouchScreenTask touch request rejected"); - fillTouchResponse(response, false, error); - return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); - } - - while (touch_task->isBusy()) { - if (context != nullptr && context->IsCancelled()) { - const std::string error = "touch request cancelled"; - fillTouchResponse(response, false, error); - return grpc::Status(grpc::StatusCode::CANCELLED, error); - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - - if (!touch_task->isFinished()) { - const std::string error = - buildTouchFailureMessage(*touch_task, "TouchScreenTask touch failed"); - fillTouchResponse(response, false, error); - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } - - fillTouchResponse(response, true, ""); - CMVR_LOG(DEBUG) << "[gRPCHlcServiceImpl] (touch): success, u=" << request->u() - << ", v=" << request->v() - << ", phase=" << cmvr::task::TouchScreenTask::phaseToString(touch_task->phase()) - << ", status=" << cmvr::task::TouchScreenTask::statusToString(touch_task->lastStatus()); - return grpc::Status::OK; - } catch (const std::exception& e) { - fillTouchResponse(response, false, e.what()); - return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } -} diff --git a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp deleted file mode 100644 index d75bfd18..00000000 --- a/cmvr-es/service/grpc/src/grpc_microphone_service.cpp +++ /dev/null @@ -1,382 +0,0 @@ -#include "common/base/logging/logger.h" -#include "devices/microphone/microphone_device_discovery.h" -#include "manager/media_source_hub/include/device_media_source_adapter.h" -#include -#include -#include -#include -// -// Created by linbo on 2025/6/13. -// Created by xtkuang on 2025/6/13. -// - -#include "../include/grpc_microphone_service.h" -using namespace std; -using namespace cmvr::device; -using namespace cmvr::service; - -namespace { -template -grpc::Status failResponse(ResponseT* response, const std::string& message) { - CMVR_LOG(ERROR) << "[gRPCMicroPhoneServiceImpl] " << message; - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(message); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; -} - -std::string selectInputDevice( - const std::shared_ptr& microphone, - const std::string& input_device) { - if (input_device.empty()) { - return "Microphone input_device is required"; - } - - const auto available_devices = listAvailableMicrophoneInputDevices(); - const auto selected = std::find_if( - available_devices.begin(), - available_devices.end(), - [&input_device](const MicrophoneInputDeviceInfo& device) { - return device.input_device == input_device; - }); - if (selected == available_devices.end()) { - return "Microphone input device is unavailable: " + input_device; - } - if (!microphone->selectInputDevice(input_device)) { - return "Cannot select microphone input device while capture is active: " + input_device; - } - return {}; -} - -} - -gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl(): dmgr_(DeviceManager::getInstance()) {} - -grpc::Status gRPCMicroPhoneServiceImpl::ListDevices( - grpc::ServerContext* context, - const api::ListMicrophoneDevicesCommand_Request* request, - api::ListMicrophoneDevicesCommand_Feedback* response) { - (void)context; - (void)request; - try { - const auto devices = listAvailableMicrophoneInputDevices(); - for (const auto& device : devices) { - auto* item = response->add_devices(); - item->set_input_device(device.input_device); - item->set_display_name(device.display_name); - item->set_backend(device.backend); - item->set_is_default(device.is_default); - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (ListDevices): found " - << devices.size() << " microphone input device(s)"; - return grpc::Status::OK; - } catch (const std::exception& e) { - return failResponse(response, e.what()); - } -} - -grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context, - const api::GetMicStateCommand_Request* request, api::GetMicStateCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetStatus): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Microphone device not found: " + dev_id); - } - MicrophoneState state; - dev->getState(state); - response->mutable_state()->set_is_initialized(state.is_initialized); - response->mutable_state()->set_is_running(state.is_running); - response->mutable_state()->set_is_recording(state.is_recording); - response->mutable_state()->set_volume(state.volume); - response->mutable_state()->set_error_message(state.error_message); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (GetStatus): success, id=" << dev_id - << ", initialized=" << state.is_initialized - << ", running=" << state.is_running - << ", recording=" << state.is_recording - << ", volume=" << state.volume; - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context, - const api::StartMicRecordingCommand_Request* request, api::StartMicRecordingCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StartRecord): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Microphone device not found: " + dev_id); - } - const std::string selection_error = selectInputDevice(dev, request->input_device()); - if (!selection_error.empty()) { - return failResponse(response, selection_error); - } - if (!dev->start()) { - return failResponse(response, "Failed to start microphone: " + dev_id); - } - if (!dev->startRecording(request->file_path())) { - MicrophoneState state; - dev->getState(state); - return failResponse( - response, - state.error_message.empty() - ? "Failed to start microphone recording: " + dev_id - : state.error_message); - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StartRecord): success, id=" << dev_id - << ", path=" << request->file_path(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCMicroPhoneServiceImpl::StopRecord(grpc::ServerContext* context, - const api::StopMicRecordingCommand_Request* request, api::StopMicRecordingCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StopRecord): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Microphone device not found: " + dev_id); - } - dev->stopRecording(); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StopRecord): success, id=" << dev_id; - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCMicroPhoneServiceImpl::PauseRecord(grpc::ServerContext* context, - const api::PauseMicRecordingCommand_Request* request, api::PauseMicRecordingCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (PauseRecord): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Microphone device not found: " + dev_id); - } - dev->pause(); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (PauseRecord): success, id=" << dev_id; - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* context, - const api::ResumeMicRecordingCommand_Request* request, api::ResumeMicRecordingCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Microphone device not found: " + dev_id); - } - dev->resume(); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): success, id=" << dev_id; - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context, - const api::StreamMicAudioCommand_Request* request, - grpc::ServerWriter* writer) { - try { - const string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StreamAudio): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - api::StreamMicAudioCommand_Feedback feedback; - feedback.mutable_header()->set_success(false); - feedback.mutable_header()->set_error_message("Microphone device not found: " + dev_id); - setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp()); - writer->Write(feedback); - return grpc::Status::OK; - } - - const std::string selection_error = selectInputDevice(dev, request->input_device()); - if (!selection_error.empty()) { - api::StreamMicAudioCommand_Feedback feedback; - feedback.mutable_header()->set_success(false); - feedback.mutable_header()->set_error_message(selection_error); - setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp()); - writer->Write(feedback); - return grpc::Status::OK; - } - - auto& media_hub = cmvr::media::globalMediaSourceHub(); - const std::string track_id = cmvr::media::microphoneTrackId(dev_id); - if (!cmvr::media::ensureMicrophoneMediaSource(media_hub, dev)) { - api::StreamMicAudioCommand_Feedback feedback; - feedback.mutable_header()->set_success(false); - feedback.mutable_header()->set_error_message("Failed to register microphone media source: " + dev_id); - setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp()); - writer->Write(feedback); - return grpc::Status::OK; - } - - auto subscription = media_hub.subscribe( - track_id, - cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED, - [context] { return context->IsCancelled(); }); - if (!subscription) { - api::StreamMicAudioCommand_Feedback feedback; - feedback.mutable_header()->set_success(false); - feedback.mutable_header()->set_error_message("Failed to subscribe microphone media source: " + dev_id); - setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp()); - writer->Write(feedback); - return grpc::Status::OK; - } - - while (!context->IsCancelled()) { - const auto read = subscription.waitRead(std::chrono::milliseconds(100)); - if (!read || !read->value || read->value->empty()) { - if (!subscription.valid()) { - break; - } - continue; - } - const auto& frame = *read->value; - const auto descriptor = frame.descriptor; - if (!descriptor) { - continue; - } - - api::StreamMicAudioCommand_Feedback feedback; - feedback.mutable_header()->set_success(true); - setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp()); - auto* audio = feedback.mutable_audio(); - audio->set_data(frame.data(), frame.size()); - audio->set_sample_rate(static_cast(std::min( - descriptor->sample_rate, - static_cast(std::numeric_limits::max())))); - audio->set_channels(static_cast(std::min( - descriptor->channels, - static_cast(std::numeric_limits::max())))); - if (descriptor->codec == cmvr::media::Codec::PCM_S16LE) { - audio->set_format(cmvr::api::AudioData_AudioFormat_PCM); - audio->set_codec("pcm_s16le"); - } else if (descriptor->codec == cmvr::media::Codec::OPUS) { - audio->set_format(cmvr::api::AudioData_AudioFormat_OPUS); - audio->set_codec("opus"); - } else if (descriptor->codec == cmvr::media::Codec::AAC) { - audio->set_format(cmvr::api::AudioData_AudioFormat_AAC); - audio->set_codec("aac"); - } else { - feedback.mutable_header()->set_success(false); - feedback.mutable_header()->set_error_message( - "Unsupported microphone stream codec: " + dev_id); - feedback.clear_audio(); - writer->Write(feedback); - break; - } - audio->set_pts(frame.pts); - const int64_t sample_count = frame.duration > 0 - ? frame.duration - : static_cast(descriptor->nominal_rate); - audio->set_nb_samples(static_cast(std::clamp( - sample_count, - 0, - std::numeric_limits::max()))); - if (!writer->Write(feedback)) { - break; - } - } - return grpc::Status::OK; - } catch (const std::exception& error) { - api::StreamMicAudioCommand_Feedback feedback; - feedback.mutable_header()->set_success(false); - feedback.mutable_header()->set_error_message(error.what()); - setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp()); - writer->Write(feedback); - return grpc::Status::OK; - } -} - -grpc::Status gRPCMicroPhoneServiceImpl::SetVolume(grpc::ServerContext* context, - const api::SetMicPhoneVolumeCommand_Request* request, api::SetMicPhoneVolumeCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (SetVolume): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Microphone device not found: " + dev_id); - } - dev->setVolume(request->volume()); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (SetVolume): success, id=" << dev_id - << ", volume=" << request->volume(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCMicroPhoneServiceImpl::GetVolume(grpc::ServerContext* context, - const api::GetMicPhoneVolumeCommand_Request* request, api::GetMicPhoneVolumeCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetVolume): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Microphone device not found: " + dev_id); - } - response->set_volume(dev->getVolume()); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (GetVolume): success, id=" << dev_id - << ", volume=" << response->volume(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} diff --git a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp deleted file mode 100644 index 756fab83..00000000 --- a/cmvr-es/service/grpc/src/grpc_speaker_service.cpp +++ /dev/null @@ -1,271 +0,0 @@ -#include "common/base/logging/logger.h" -#include -// -// Created by xtkuang on 2025/6/10. -// - -#include "../include/grpc_speaker_service.h" - -using namespace std; -using namespace cmvr::device; -using namespace cmvr::service; - -namespace { -template -grpc::Status failResponse(ResponseT* response, const std::string& message) { - CMVR_LOG(ERROR) << "[gRPCSpeakerServiceImpl] " << message; - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(message); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; -} - -AudioStreamFormat fromProtoAudioFormat(cmvr::api::AudioData_AudioFormat format) { - switch (format) { - case cmvr::api::AudioData_AudioFormat_PCM: - return AudioStreamFormat::PCM; - case cmvr::api::AudioData_AudioFormat_MP3: - return AudioStreamFormat::MP3; - case cmvr::api::AudioData_AudioFormat_AAC: - return AudioStreamFormat::AAC; - case cmvr::api::AudioData_AudioFormat_WAV: - return AudioStreamFormat::WAV; - default: - return AudioStreamFormat::UNKNOWN; - } -} - -AudioStreamFrameData fromProtoAudioData(const cmvr::api::AudioData& audio) { - AudioStreamFrameData frame; - frame.data.assign(audio.data().begin(), audio.data().end()); - frame.sample_rate = audio.sample_rate() > 0 ? audio.sample_rate() : 44100; - frame.channels = audio.channels() > 0 ? audio.channels() : 2; - frame.format = fromProtoAudioFormat(audio.format()); - frame.codec = audio.codec(); - frame.pts = audio.pts(); - frame.nb_samples = audio.nb_samples(); - return frame; -} -} - -gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl(): dmgr_(DeviceManager::getInstance()) {} - -grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context, - const api::GetSpeakerStateCommand_Request* request, api::GetSpeakerStateCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - //CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetStatus): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Speaker device not found: " + dev_id); - } - SpeakerState state; - dev->getState(state); - response->mutable_state()->set_is_initialized(state.is_initialized); - response->mutable_state()->set_is_running(state.is_running); - response->mutable_state()->set_is_decoding(state.is_decoding); - response->mutable_state()->set_is_paused(state.is_paused); - response->mutable_state()->set_volume(state.volume); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (GetStatus): success, id=" << dev_id - << ", initialized=" << state.is_initialized - << ", running=" << state.is_running - << ", decoding=" << state.is_decoding - << ", paused=" << state.is_paused - << ", volume=" << state.volume; - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context, - const api::PlayAudioCommand_Request* request, api::PlayAudioCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (PlayAudio): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Speaker device not found: " + dev_id); - } - //dev->start(); - dev->play(request->audio_path()); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PlayAudio): success, id=" << dev_id - << ", path=" << request->audio_path(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, - grpc::ServerReader* reader, - api::StreamSpeakerAudioCommand_Feedback* response) { - try { - api::StreamSpeakerAudioCommand_Request request; - std::shared_ptr dev; - std::string dev_id; - - while (reader->Read(&request)) { - if (!dev) { - dev_id = request.header().device_id(); - CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (StreamAudio): id=" << dev_id; - dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Speaker device not found: " + dev_id); - } - if (!dev->start()) { - return failResponse(response, "Failed to start speaker: " + dev_id); - } - } - - if (!dev->pushAudioFrame(fromProtoAudioData(request.audio()))) { - dev->stopStreaming(); - return failResponse(response, "Failed to push speaker audio frame: " + dev_id); - } - } - - if (dev) { - dev->stopStreaming(); - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context, - const api::StopSpeakerCommand_Request* request, api::StopSpeakerCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (StopPlayback): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Speaker device not found: " + dev_id); - } - if (!dev->stop()) { - return failResponse(response, "Failed to stop speaker: " + dev_id); - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (StopPlayback): success, id=" << dev_id; - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSpeakerServiceImpl::PausePlayback(grpc::ServerContext* context, - const api::PauseSpeakerCommand_Request* request, api::PauseSpeakerCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (PausePlayback): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Speaker device not found: " + dev_id); - } - dev->pause(); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PausePlayback): success, id=" << dev_id; - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context, - const api::ResumeSpeakerCommand_Request* request, api::ResumeSpeakerCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (ResumePlayback): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Speaker device not found: " + dev_id); - } - dev->resume(); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (ResumePlayback): success, id=" << dev_id; - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSpeakerServiceImpl::SetVolume(grpc::ServerContext* context, - const api::SetSpeakerVolumeCommand_Request* request, api::SetSpeakerVolumeCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (SetVolume): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Speaker device not found: " + dev_id); - } - dev->setVolume(request->volume()); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (SetVolume): success, id=" << dev_id - << ", volume=" << request->volume(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSpeakerServiceImpl::GetVolume(grpc::ServerContext* context, - const api::GetSpeakerVolumeCommand_Request* request, api::GetSpeakerVolumeCommand_Feedback* response) { - try { - string dev_id = request->header().device_id(); - CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetVolume): id=" << dev_id; - const auto dev = dmgr_.getDevice(dev_id); - if (!dev) { - return failResponse(response, "Speaker device not found: " + dev_id); - } - response->set_volume(dev->getVolume()); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (GetVolume): success, id=" << dev_id - << ", volume=" << response->volume(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp deleted file mode 100644 index a9ac6a31..00000000 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ /dev/null @@ -1,141 +0,0 @@ -// -// Created by xtkuang on 2025/6/6. -// - -#include "../include/grpc_system_service.h" - -#include "common/base/logging/logger.h" - -using namespace cmvr::device; -using namespace cmvr::device; -using namespace cmvr::service; - -gRPCSystemServiceImpl::gRPCSystemServiceImpl(): dmgr_(DeviceManager::getInstance()) {} - -grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, - const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) -{ - try { - response->set_version(dmgr_.version()); - response->set_system_name(dmgr_.name()); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemInfo): success, name=" - << response->system_name() << ", version=" << response->version(); - return grpc::Status::OK; - } - catch (std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context, - const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) -{ - try { - std::list> dev_list; - dmgr_.getDeviceList(dev_list); - for (auto &pair: dev_list) { - auto* dev = response->add_device_list(); - dev->set_device_id(pair.first); - if (pair.second == "AGV") { - dev->set_device_type(api::DeviceType::AGV); - } - else if (pair.second == "Battery") { - dev->set_device_type(api::DeviceType::Battery); - } - else if (pair.second == "Camera") { - dev->set_device_type(api::DeviceType::Camera); - } - else if (pair.second == "DexHand") { - dev->set_device_type(api::DeviceType::DexHand); - } - else if (pair.second == "Gripper") { - dev->set_device_type(api::DeviceType::Gripper); - } - else if (pair.second == "Microphone") { - dev->set_device_type(api::DeviceType::Microphone); - } - else if (pair.second == "Robot") { - dev->set_device_type(api::DeviceType::Robot); - } - else if (pair.second == "Speaker") { - dev->set_device_type(api::DeviceType::Speaker); - } - else if (pair.second == "Unknown") { - dev->set_device_type(api::DeviceType::Unknown); - } - } - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemStatus): success, devices=" - << response->device_list_size(); - return grpc::Status::OK; - } - catch (const std::exception &e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) -{ - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message("UpdateParams is no longer supported. Use typed device commands or reload configuration."); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; -} - -grpc::Status gRPCSystemServiceImpl::ExecuteJsonCommand(grpc::ServerContext* context, - const cmvr::api::JsonDeviceCommand_Request* request, cmvr::api::JsonDeviceCommand_Feedback* response) -{ - try { - const std::string& dev_id = request->header().device_id(); - auto dev = dmgr_.getDeviceBase(dev_id); - if (!dev) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message("Device not found: " + dev_id); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - - std::string response_json; - const bool success = dev->executeJsonCommand(request->request_json(), response_json); - response->mutable_header()->set_success(success); - if (!success) { - response->mutable_header()->set_error_message(response_json); - } - response->set_response_json(response_json); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - catch (std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, - const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) -{ - try { - dmgr_.stop(); - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (StopAll): success"; - return grpc::Status::OK; - } - catch (std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} diff --git a/cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp b/cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp deleted file mode 100644 index 1d84b942..00000000 --- a/cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include "service/grpc/include/grpc_camera_stream_policy.h" - -#include -#include -#include - -namespace { - -bool check(const bool condition, const char* expression, const int line) { - if (condition) { - return true; - } - std::cerr << "CHECK failed at line " << line << ": " << expression << '\n'; - return false; -} - -#define CHECK_TRUE(expression) \ - do { \ - if (!check(static_cast(expression), #expression, __LINE__)) { \ - return 1; \ - } \ - } while (false) - -} // namespace - -int main() { - using namespace std::chrono_literals; - using cmvr::service::cameraFrameAgeNs; - using cmvr::service::cameraFrameExceedsAgeLimit; - using cmvr::service::makeCameraStreamLowLatencyConfig; - - const auto defaults = makeCameraStreamLowLatencyConfig(0, 0); - CHECK_TRUE(defaults.max_pending_frames == 2); - CHECK_TRUE(defaults.max_frame_age == 250ms); - - const auto configured = makeCameraStreamLowLatencyConfig(7, 900); - CHECK_TRUE(configured.max_pending_frames == 7); - CHECK_TRUE(configured.max_frame_age == 900ms); - - CHECK_TRUE(!cameraFrameAgeNs(0, 1'000'000'000ULL).has_value()); - CHECK_TRUE(!cameraFrameAgeNs(2'000'000'000ULL, 1'000'000'000ULL).has_value()); - CHECK_TRUE(cameraFrameAgeNs(1'000'000'000ULL, 1'250'000'000ULL).value() == - 250'000'000ULL); - - CHECK_TRUE(!cameraFrameExceedsAgeLimit( - 1'000'000'000ULL, 1'250'000'000ULL, 250ms)); - CHECK_TRUE(cameraFrameExceedsAgeLimit( - 1'000'000'000ULL, 1'250'000'001ULL, 250ms)); - CHECK_TRUE(!cameraFrameExceedsAgeLimit( - 1'000'000'000ULL, 2'000'000'000ULL, 0ms)); - - std::cout << "grpc_camera_stream_policy_test: PASS\n"; - return 0; -} diff --git a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h index 055e2eed..0a275eed 100644 --- a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h +++ b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h @@ -11,6 +11,12 @@ #include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" #include "task/task.h" +namespace cmvr::service { +class ArmTeleopBackend; +class GrpcSecurityGateway; +class RecoveryAuditSink; +} + namespace cmvr::task { class GrpcServerTask final : public Task { @@ -20,6 +26,10 @@ public: const std::string& id() const override { return id_; } TaskRunMode runMode() const override { return TaskRunMode::BLOCKING_SERVICE; } + TaskShutdownPhase shutdownPhase() const override + { + return TaskShutdownPhase::COMMAND_INGRESS; + } bool init() override; bool start() override; bool step(double dt) override; @@ -55,6 +65,12 @@ private: std::unique_ptr dexhand_service_; std::unique_ptr biohand_service_; std::unique_ptr arm_service_; + std::unique_ptr arm_teleop_service_; + std::shared_ptr + arm_teleop_backend_; + std::shared_ptr security_gateway_; + std::shared_ptr recovery_audit_sink_; + std::unique_ptr motor_service_; std::unique_ptr agv_service_; std::unique_ptr hlc_service_; }; diff --git a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp index 60d8c697..c7799fc2 100644 --- a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp +++ b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp @@ -1,5 +1,6 @@ #include "task/grpc_server_task/include/grpc_server_task.h" +#include #include #include @@ -8,21 +9,36 @@ #include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "common/base/logging/logger.h" #include "common/config/config_files.h" -#include "service/grpc/include/grpc_agv_service.h" -#include "service/grpc/include/grpc_arm_service.h" -#include "service/grpc/include/grpc_camera_service.h" -#include "service/grpc/include/grpc_dexhand_service.h" -#include "service/grpc/include/grpc_head_service.h" -#include "service/grpc/include/grpc_hlc_service.h" -#include "service/grpc/include/grpc_microphone_service.h" -#include "service/grpc/include/grpc_speaker_service.h" -#include "service/grpc/include/grpc_system_service.h" +#include "devices/arm/robot_arm.h" +#include "manager/device_manager/include/device_manager.h" +#include "service/grpc/server/include/grpc_agv_service.h" +#include "service/grpc/server/include/grpc_arm_service.h" +#include "service/grpc/server/include/grpc_arm_teleop_service.h" +#include "service/grpc/server/include/grpc_robot_arm_teleop_backend.h" +#include "service/grpc/server/include/grpc_recovery_audit.h" +#include "service/grpc/server/include/grpc_security.h" +#include "service/grpc/server/include/grpc_camera_service.h" +#include "service/grpc/server/include/grpc_dexhand_service.h" +#include "service/grpc/server/include/grpc_error_logging_interceptor.h" +#include "service/grpc/server/include/grpc_head_service.h" +#include "service/grpc/server/include/grpc_hlc_service.h" +#include "service/grpc/server/include/grpc_microphone_service.h" +#include "service/grpc/server/include/grpc_motor_service.h" +#include "service/grpc/server/include/grpc_speaker_service.h" +#include "service/grpc/server/include/grpc_system_service.h" #include "task/task_factory.h" namespace cmvr::task { namespace { +std::uint64_t unixTimeMs() noexcept +{ + const auto value = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + return value > 0 ? static_cast(value) : 1U; +} + std::shared_ptr createGrpcServerTask(const config::TaskConfigEntry& entry) { if (entry.id().empty()) { @@ -98,18 +114,45 @@ bool GrpcServerTask::start() camera_service_ = std::make_unique( service::makeCameraStreamLowLatencyConfig( cfg_.camera_stream_max_pending_frames(), - cfg_.camera_stream_max_frame_age_ms())); - system_service_ = std::make_unique(); - speaker_service_ = std::make_unique(); - microphone_service_ = std::make_unique(); - dexhand_service_ = std::make_unique(); - biohand_service_ = std::make_unique(); - arm_service_ = std::make_unique(); - agv_service_ = std::make_unique(); - hlc_service_ = std::make_unique(); + cfg_.camera_stream_max_frame_age_ms()), + security_gateway_); + system_service_ = std::make_unique( + std::chrono::seconds(15), + security_gateway_, + recovery_audit_sink_); + speaker_service_ = + std::make_unique(security_gateway_); + microphone_service_ = + std::make_unique(security_gateway_); + dexhand_service_ = + std::make_unique(security_gateway_); + biohand_service_ = + std::make_unique(security_gateway_); + arm_service_ = + std::make_unique(security_gateway_); + arm_teleop_service_ = + std::make_unique( + arm_teleop_backend_ ? arm_teleop_backend_ + : service::makeDisabledArmTeleopBackend(), + nullptr, + security_gateway_, + &device::DeviceManager::getInstance().safetyManager()); + motor_service_ = + std::make_unique(security_gateway_); + agv_service_ = + std::make_unique(security_gateway_); + hlc_service_ = + std::make_unique(security_gateway_); grpc::ServerBuilder builder; builder.AddListeningPort(local_address, grpc::InsecureServerCredentials()); + std::vector> + interceptor_factories; + interceptor_factories.emplace_back( + service::makeGrpcErrorLoggingInterceptorFactory()); + builder.experimental().SetInterceptorCreators( + std::move(interceptor_factories)); builder.RegisterService(camera_service_.get()); builder.RegisterService(system_service_.get()); builder.RegisterService(speaker_service_.get()); @@ -117,6 +160,8 @@ bool GrpcServerTask::start() builder.RegisterService(dexhand_service_.get()); builder.RegisterService(biohand_service_.get()); builder.RegisterService(arm_service_.get()); + builder.RegisterService(arm_teleop_service_.get()); + builder.RegisterService(motor_service_.get()); builder.RegisterService(agv_service_.get()); builder.RegisterService(hlc_service_.get()); @@ -131,7 +176,15 @@ bool GrpcServerTask::start() address_ = local_address; state_ = TaskState::RUNNING; - CMVR_LOG(INFO) << "[GrpcServerTask] gRPC server started, address=" << address_; + const auto& security = security_gateway_->config(); + CMVR_LOG(INFO) << "[GrpcServerTask] gRPC server started, address=" << address_ + << ", transport=" << service::toString(security.transport) + << ", authentication=" + << service::toString(security.authentication) + << ", recovery=" + << service::toString(security.recovery_exposure) + << ", insecure_non_loopback=" + << security.insecure_non_loopback; wait_thread_ = std::thread(&GrpcServerTask::waitLoop, this); return true; } @@ -149,6 +202,101 @@ bool GrpcServerTask::init() state_ = TaskState::FAILED; return false; } + + const std::string effective_host = + cfg_.host().empty() ? "0.0.0.0" : cfg_.host(); + const auto security_result = + service::resolveGrpcSecurityConfig(cfg_, effective_host); + if (!security_result.valid) { + last_error_ = "invalid gRPC security config: " + security_result.error; + CMVR_LOG(ERROR) << "[GrpcServerTask] " << last_error_; + state_ = TaskState::FAILED; + return false; + } + for (const auto& warning : security_result.warnings) { + CMVR_LOG(WARNING) << "[GrpcServerTask] " << warning; + } + security_gateway_ = service::makeGrpcSecurityGateway( + security_result.config, + [](const service::GrpcSecurityAuditRecord& record) { + if (!record.allowed) { + CMVR_LOG(WARNING) + << "[gRPC security] request denied, correlation_id=" + << record.correlation_id + << ", method=" << record.full_method_name + << ", principal=" << record.principal_id + << ", peer=" << record.peer + << ", code=" << static_cast(record.status_code); + } + }); + recovery_audit_sink_.reset(); + if (security_result.config.recovery_exposure != + service::GrpcRecoveryExposure::Disabled) { + recovery_audit_sink_ = service::makeFileRecoveryAuditSink( + security_result.config.recovery_audit_file); + service::RecoveryAuditRecord audit_probe; + audit_probe.occurred_at_unix_ms = unixTimeMs(); + audit_probe.stage = "sink_initialized"; + audit_probe.principal_id = "system"; + audit_probe.result = "ready"; + std::string audit_error; + if (!recovery_audit_sink_->append(audit_probe, &audit_error)) { + last_error_ = + "recovery audit initialization failed: " + audit_error; + CMVR_LOG(ERROR) << "[GrpcServerTask] " << last_error_; + recovery_audit_sink_.reset(); + security_gateway_.reset(); + state_ = TaskState::FAILED; + return false; + } + } + + arm_teleop_backend_ = service::makeDisabledArmTeleopBackend(); + if (cfg_.has_arm_teleop_backend() && + cfg_.arm_teleop_backend().enable()) { + const auto& backend_config = cfg_.arm_teleop_backend(); + if (backend_config.device_id().empty()) { + last_error_ = + "enabled ArmTeleop backend requires device_id"; + state_ = TaskState::FAILED; + return false; + } + auto arm = + device::DeviceManager::getInstance() + .getDevice( + backend_config.device_id()); + if (!arm) { + last_error_ = + "ArmTeleop RobotArm device was not found: " + + backend_config.device_id(); + state_ = TaskState::FAILED; + return false; + } + auto backend = + service::makeRobotArmTeleopBackend( + std::move(arm), backend_config); + if (!backend->available()) { + last_error_ = + "ArmTeleop backend rejected configuration: " + + backend->unavailableReason(); + state_ = TaskState::FAILED; + return false; + } + // RobotArmTeleopBackend builds robot_id directly from device_id. Keep + // this assertion at the registration boundary so the process-wide + // control lease resource and unary ArmService device ID cannot drift. + if (backend->manifest().robot_id() != + backend_config.device_id()) { + last_error_ = + "ArmTeleop lease resource must equal device_id"; + state_ = TaskState::FAILED; + return false; + } + arm_teleop_backend_ = std::move(backend); + CMVR_LOG(INFO) + << "[GrpcServerTask] ArmTeleop RobotArm backend enabled for " + << backend_config.device_id(); + } last_error_.clear(); state_ = TaskState::IDLE; return true; @@ -164,6 +312,11 @@ void GrpcServerTask::stop() { { std::lock_guard lock(mutex_); + if (auto* system_service = + dynamic_cast( + system_service_.get())) { + system_service->prepareForShutdown(); + } if (server_) { server_->Shutdown(); } @@ -252,14 +405,19 @@ void GrpcServerTask::waitLoop() void GrpcServerTask::clearServices() { + // SystemService owns StopAll workers which can still be draining calls + // into operational backends after an RPC deadline. Join them before any + // peer service releases its activity registrations or backend state. + system_service_.reset(); hlc_service_.reset(); agv_service_.reset(); + motor_service_.reset(); + arm_teleop_service_.reset(); arm_service_.reset(); biohand_service_.reset(); dexhand_service_.reset(); microphone_service_.reset(); speaker_service_.reset(); - system_service_.reset(); camera_service_.reset(); } diff --git a/cmvr-es/task/task.h b/cmvr-es/task/task.h index a6786f4e..571fad9b 100644 --- a/cmvr-es/task/task.h +++ b/cmvr-es/task/task.h @@ -19,17 +19,30 @@ enum class TaskRunMode { BLOCKING_SERVICE }; +enum class TaskShutdownPhase { + COMMAND_INGRESS = 0, + DEPENDENT_ACTIVITY +}; + class Task { public: virtual ~Task() = default; virtual const std::string& id() const = 0; virtual TaskRunMode runMode() const { return TaskRunMode::PERIODIC_STEP; } + virtual TaskShutdownPhase shutdownPhase() const + { + return TaskShutdownPhase::DEPENDENT_ACTIVITY; + } virtual bool init() = 0; virtual bool start() { return true; } virtual bool step(double dt) = 0; virtual void stop() = 0; + // Cancels current command-driven activity while preserving task lifecycle. + // Tasks with no separately stoppable activity remain a successful no-op. + virtual bool stopActivity() { return true; } + virtual TaskState state() const = 0; virtual bool isBusy() const = 0; virtual bool isFinished() const = 0; diff --git a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h index 6e284d7e..e94cabf1 100644 --- a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h +++ b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h @@ -4,7 +4,9 @@ #define CMVR_ES_TOUCH_SCREEN_TASK_H #include +#include #include +#include #include #include #include @@ -17,12 +19,16 @@ #include "devices/camera/abstract_camera.h" #include "devices/dexhand/abstract_dexhand.h" #include "devices/arm/robot_arm.h" +#include "manager/control_authority_manager/include/control_authority_manager.h" #include "task/task.h" #include "algorithms/perception/apriltag/include/apriltag_perception.h" #include "algorithms/perception/apriltag/include/tag_relative_target_3d.h" namespace cmvr::task { +class TouchScreenTaskStopActivityTestPeer; +class TouchScreenTaskAdmissionTestPeer; + class TouchScreenTask : public Task { public: enum class Phase { @@ -55,11 +61,22 @@ public: RETRACTING, // 正在回退离开屏幕。 DONE, // 流程成功完成。 STOPPED, // 被外部 stop() 主动停止。 + SAFETY_ADMISSION_REVOKED, // 统一安全会话在硬件下发前失效。 ROBOT_STATE_FAILED, // 读取机器人状态失败。 ROBOT_COMMAND_FAILED, // 向机器人下发控制命令失败。 TASK_BUSY // 已有触屏流程正在运行，新的 touch 请求被拒绝。 }; + struct SafetyHooks { + using HardwareOperation = std::function; + using Dispatch = + std::function; + + std::function revalidate; + Dispatch dispatch_actuation; + Dispatch dispatch_stop; + }; + explicit TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg); ~TouchScreenTask() = default; @@ -70,11 +87,23 @@ public: const std::string& id() const override { return id_; } + bool touchIfCurrent( + int u, + int v, + const std::function& still_admitted, + SafetyHooks safety_hooks = {}); bool touch(int u, int v); bool startFromPixel(int u, int v); bool step(double dt) override; void stop() override; + // Stops only the current touch operation. The task remains initialized + // and can accept another touch after StopAll admission reopens. Returns + // true only after no old step can submit another arm command. When this + // task still owns control it also confirms the arm stop; when a safety + // barrier already displaced the task, that barrier owns the physical stop. + bool stopActivity() override; + Phase phase() const; Status lastStatus() const; static const char* phaseToString(Phase phase); @@ -93,18 +122,36 @@ public: int lastTouchNonzeroCount() const; int lastActiveTagId() const; Eigen::Vector3d lastAlignErrorCamera() const; + std::string controlDeviceId() const; const std::shared_ptr& perception() const { return perception_; } const perception::TagRelativeTarget3D& tracker() const { return tracker_; } const IbvsController& ibvs() const { return ibvs_; } private: + friend class TouchScreenTaskStopActivityTestPeer; + friend class TouchScreenTaskAdmissionTestPeer; + using Clock = std::chrono::steady_clock; static bool validateConfig(const cmvr::config::TouchScreenTaskConfig& config); bool isBusyUnlocked() const; + bool beginActivityIfCurrent(std::uint64_t activity_generation); + bool acquireActivityControlUnlocked(); + control::ControlLeaseToken activityControlToken() const; + bool activityControlCurrent() const; + std::function activityCancellationRequested() const; + control::ControlDispatchGuard tryBeginActivityDispatch() const; + bool activitySafetyCurrent() const; + bool runArmActuationIfCurrent( + const SafetyHooks::HardwareOperation& operation) const; + bool runArmStopIfCurrent( + const SafetyHooks::HardwareOperation& operation) const; + void clearActivitySafetyHooksUnlocked() noexcept; + void releaseActivityControlUnlocked() noexcept; + void finishActivityUnlocked(Phase phase, Status status) noexcept; bool startFromPixelUnlocked(int u, int v); - void stopUnlocked(); + void resetActivityUnlocked(); bool applyConfig(); bool validateControlJointNames() const; bool stepAligning(double dt); @@ -132,8 +179,11 @@ private: private: mutable std::mutex mutex_; + mutable std::mutex activity_arm_mutex_; + mutable std::mutex activity_control_mutex_; std::string id_; std::shared_ptr arm_{nullptr}; + std::shared_ptr activity_arm_{nullptr}; std::shared_ptr dexhand_{nullptr}; std::shared_ptr camera_{nullptr}; @@ -148,6 +198,11 @@ private: Status last_status_{Status::NOT_INITIALIZED}; bool initialized_{false}; + std::atomic stop_requested_{false}; + std::atomic activity_active_{false}; + std::atomic activity_generation_{1U}; + control::ControlLeaseToken activity_control_token_; + SafetyHooks activity_safety_hooks_; bool target_locked_{false}; bool ibvs_target_initialized_{false}; bool touch_command_started_{false}; diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp index 7640cc33..85572300 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -12,6 +12,8 @@ #include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" #include "cmvr/config/touch_screen_algorithm_config.pb.h" #include "manager/device_manager/include/device_manager.h" +#include "service/grpc/server/include/camera_operational_activity_registry.h" +#include "service/grpc/stop_all/include/stop_all_admission_gate.h" #include namespace cmvr::task { @@ -277,6 +279,17 @@ TouchScreenTask::TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg) } bool TouchScreenTask::init() { + auto& admission_gate = service::globalStopAllAdmissionGate(); + std::uint64_t admission_generation = 0U; + { + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting()) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + admission_generation = admission.generation(); + } + if (!config_valid_) { last_status_ = Status::INVALID_CONFIG; return false; @@ -294,12 +307,73 @@ bool TouchScreenTask::init() { auto arm = dm.getDevice(devices.arm_id()); auto dexhand = dm.getDevice(devices.dexhand_id()); auto camera = dm.getDevice(devices.camera_id()); - if (!camera || !camera->start()) { + if (!camera) { CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: " << devices.camera_id(); last_status_ = Status::NOT_INITIALIZED; return false; } - return init(arm, dexhand, camera); + + auto& camera_registry = + service::globalCameraOperationalActivityRegistry(); + service::CameraOperationalActivityRegistry::ActivityToken camera_token; + service::CameraOperationalActivityRegistry::DispatchResult camera_start; + try { + camera_start = camera_registry.start( + devices.camera_id(), camera, &camera_token); + } catch (...) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + if (camera_start != service::CameraOperationalActivityRegistry:: + DispatchResult::Success) { + CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: " + << devices.camera_id(); + last_status_ = Status::NOT_INITIALIZED; + return false; + } + + const auto admission_current = [&] { + auto admission = admission_gate.lockAdmission(); + return admission.accepting() && + admission.generation() == admission_generation; + }; + const auto rollback_camera = [&] { + if (camera_registry.stopIfCurrent(camera_token)) { + return; + } + const auto ticket = admission_gate.beginStopAll(); + (void)admission_gate.finishStopAll(ticket, false); + }; + const auto mark_interrupted = [this] { + std::lock_guard lock(mutex_); + initialized_ = false; + last_status_ = Status::NOT_INITIALIZED; + }; + + if (!admission_current()) { + rollback_camera(); + mark_interrupted(); + return false; + } + + bool initialized = false; + try { + initialized = init(arm, dexhand, camera); + } catch (...) { + rollback_camera(); + throw; + } + if (!initialized) { + rollback_camera(); + return false; + } + if (admission_current()) { + return true; + } + + rollback_camera(); + mark_interrupted(); + return false; } bool TouchScreenTask::init(const std::shared_ptr& arm, @@ -307,6 +381,10 @@ bool TouchScreenTask::init(const std::shared_ptr& arm, const std::shared_ptr& camera) { std::lock_guard lock(mutex_); arm_ = arm; + { + std::lock_guard arm_lock(activity_arm_mutex_); + activity_arm_ = arm; + } dexhand_ = dexhand; camera_ = camera; @@ -383,16 +461,283 @@ bool TouchScreenTask::init(const std::shared_ptr& arm, } bool TouchScreenTask::touch(const int u, const int v) { - std::lock_guard lock(mutex_); - if (isBusyUnlocked()) { + return touchIfCurrent(u, v, [] { return true; }); +} + +bool TouchScreenTask::touchIfCurrent( + const int u, + const int v, + const std::function& still_admitted, + SafetyHooks safety_hooks) +{ + auto& admission_gate = service::globalStopAllAdmissionGate(); + std::uint64_t admission_generation = 0U; + { + auto admission = admission_gate.lockAdmission(); + if (!admission.accepting()) { + return false; + } + admission_generation = admission.generation(); + } + if (stop_requested_.load(std::memory_order_acquire)) { return false; } - return startFromPixelUnlocked(u, v); + const auto activity_generation = + activity_generation_.load(std::memory_order_acquire); + std::lock_guard lock(mutex_); + if (!still_admitted || !still_admitted()) { + return false; + } + if (!initialized_ || !camera_ || camera_->id().empty()) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + + auto& camera_registry = + service::globalCameraOperationalActivityRegistry(); + service::CameraOperationalActivityRegistry::ActivityToken camera_token; + service::CameraOperationalActivityRegistry::DispatchResult camera_start; + try { + camera_start = camera_registry.start( + camera_->id(), camera_, &camera_token); + } catch (...) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + if (camera_start != service::CameraOperationalActivityRegistry:: + DispatchResult::Success) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + + const auto rollback_camera = [&] { + if (camera_registry.stopIfCurrent(camera_token)) { + return; + } + const auto ticket = admission_gate.beginStopAll(); + (void)admission_gate.finishStopAll(ticket, false); + }; + + bool admission_current = false; + bool admitted = false; + { + // The arm lease and activity marker are the publication point. Holding + // admission here makes that point linearizable with beginStopAll(). + auto admission = admission_gate.lockAdmission(); + admission_current = admission.accepting() && + admission.generation() == admission_generation; + if (admission_current) { + admitted = beginActivityIfCurrent(activity_generation); + } + } + if (!admission_current || !admitted) { + rollback_camera(); + return false; + } + + resetActivityUnlocked(); + activity_safety_hooks_ = std::move(safety_hooks); + if (!activitySafetyCurrent()) { + last_status_ = Status::SAFETY_ADMISSION_REVOKED; + activity_active_.store(false, std::memory_order_release); + clearActivitySafetyHooksUnlocked(); + releaseActivityControlUnlocked(); + rollback_camera(); + return false; + } + + const bool started = startFromPixelUnlocked(u, v); + if (!started) { + activity_active_.store(false, std::memory_order_release); + clearActivitySafetyHooksUnlocked(); + releaseActivityControlUnlocked(); + rollback_camera(); + return false; + } + + // startFromPixelUnlocked() may perform an interruptible initialization + // move. Do not retain the global gate across device work; reject and roll + // back if StopAll changed the generation while that work was in flight. + admission_current = false; + { + auto admission = admission_gate.lockAdmission(); + admission_current = admission.accepting() && + admission.generation() == admission_generation; + } + if (admission_current) { + return true; + } + activity_active_.store(false, std::memory_order_release); + resetActivityUnlocked(); + clearActivitySafetyHooksUnlocked(); + releaseActivityControlUnlocked(); + rollback_camera(); + return false; +} + +bool TouchScreenTask::beginActivityIfCurrent( + const std::uint64_t activity_generation) +{ + if (stop_requested_.load(std::memory_order_acquire) || + activity_generation_.load(std::memory_order_acquire) != + activity_generation) { + return false; + } + if (isBusyUnlocked()) { + last_status_ = Status::TASK_BUSY; + return false; + } + if (!acquireActivityControlUnlocked()) { + last_status_ = Status::TASK_BUSY; + return false; + } + if (stop_requested_.load(std::memory_order_acquire) || + activity_generation_.load(std::memory_order_acquire) != + activity_generation) { + releaseActivityControlUnlocked(); + return false; + } + activity_active_.store(true, std::memory_order_release); + return true; +} + +bool TouchScreenTask::acquireActivityControlUnlocked() +{ + if (!arm_ || arm_->id().empty() || activityControlToken().valid()) { + return false; + } + static std::atomic sequence{0U}; + const auto acquired = control::ControlAuthorityManager::instance() + .tryAcquire( + arm_->id(), + "touch-screen:" + id_ + ":" + + std::to_string( + sequence.fetch_add(1U, std::memory_order_relaxed) + 1U), + std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::hours(24))); + if (!acquired.acquired) { + return false; + } + { + std::lock_guard lock(activity_control_mutex_); + activity_control_token_ = acquired.token; + } + return true; +} + +control::ControlLeaseToken TouchScreenTask::activityControlToken() const +{ + std::lock_guard lock(activity_control_mutex_); + return activity_control_token_; +} + +bool TouchScreenTask::activityControlCurrent() const +{ + const auto token = activityControlToken(); + return token.valid() && + control::ControlAuthorityManager::instance().validate( + token); +} + +std::function TouchScreenTask::activityCancellationRequested() const +{ + const auto token = activityControlToken(); + return [this, token] { + return stop_requested_.load(std::memory_order_acquire) || + !control::ControlAuthorityManager::instance().validate(token); + }; +} + +control::ControlDispatchGuard +TouchScreenTask::tryBeginActivityDispatch() const +{ + const auto token = activityControlToken(); + return control::ControlAuthorityManager::instance().tryBeginDispatch( + token); +} + +bool TouchScreenTask::activitySafetyCurrent() const +{ + if (!activity_safety_hooks_.revalidate) { + return true; + } + try { + return activity_safety_hooks_.revalidate(); + } catch (...) { + return false; + } +} + +bool TouchScreenTask::runArmActuationIfCurrent( + const SafetyHooks::HardwareOperation& operation) const +{ + if (!operation || !activitySafetyCurrent()) { + return false; + } + auto authority_dispatch = tryBeginActivityDispatch(); + if (!authority_dispatch.acquired()) { + return false; + } + try { + return activity_safety_hooks_.dispatch_actuation + ? activity_safety_hooks_.dispatch_actuation(operation) + : operation(); + } catch (...) { + return false; + } +} + +bool TouchScreenTask::runArmStopIfCurrent( + const SafetyHooks::HardwareOperation& operation) const +{ + if (!operation) { + return false; + } + auto authority_dispatch = tryBeginActivityDispatch(); + if (!authority_dispatch.acquired()) { + return false; + } + try { + return activity_safety_hooks_.dispatch_stop + ? activity_safety_hooks_.dispatch_stop(operation) + : operation(); + } catch (...) { + return false; + } +} + +void TouchScreenTask::clearActivitySafetyHooksUnlocked() noexcept +{ + activity_safety_hooks_ = {}; +} + +void TouchScreenTask::releaseActivityControlUnlocked() noexcept +{ + control::ControlLeaseToken token; + { + std::lock_guard lock(activity_control_mutex_); + token = std::move(activity_control_token_); + activity_control_token_ = {}; + } + control::ControlAuthorityManager::instance().release(token); +} + +void TouchScreenTask::finishActivityUnlocked( + const Phase phase, + const Status status) noexcept +{ + phase_ = phase; + last_status_ = status; + touch_command_started_ = false; + retract_command_started_ = false; + activity_active_.store(false, std::memory_order_release); + clearActivitySafetyHooksUnlocked(); + releaseActivityControlUnlocked(); } bool TouchScreenTask::startFromPixel(const int u, const int v) { - std::lock_guard lock(mutex_); - return startFromPixelUnlocked(u, v); + return touchIfCurrent(u, v, [] { return true; }); } bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { @@ -405,7 +750,6 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { return false; } - stopUnlocked(); if (!moveToInitPositionBeforeStartIfEnabled()) { return false; } @@ -442,12 +786,32 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { bool TouchScreenTask::step(const double dt) { std::lock_guard lock(mutex_); + if (stop_requested_.load(std::memory_order_acquire)) { + return true; + } + if (activity_active_.load(std::memory_order_acquire) && + !activityControlCurrent()) { + activity_active_.store(false, std::memory_order_release); + resetActivityUnlocked(); + clearActivitySafetyHooksUnlocked(); + releaseActivityControlUnlocked(); + return true; + } + if (activity_active_.load(std::memory_order_acquire) && + !activitySafetyCurrent()) { + (void)runArmStopIfCurrent([this] { + return arm_ && arm_->stopMotion().ok(); + }); + finishActivityUnlocked( + Phase::FAILED, Status::SAFETY_ADMISSION_REVOKED); + return false; + } if (!initialized_) { last_status_ = Status::NOT_INITIALIZED; return false; } if (!std::isfinite(dt) || dt <= 0.0) { - last_status_ = Status::INVALID_CONFIG; + enterFailed(Status::INVALID_CONFIG); return false; } @@ -494,26 +858,102 @@ bool TouchScreenTask::step(const double dt) { case Phase::FAILED: return false; } - last_status_ = Status::INVALID_CONFIG; + enterFailed(Status::INVALID_CONFIG); return false; } void TouchScreenTask::stop() { - std::lock_guard lock(mutex_); - stopUnlocked(); + (void)stopActivity(); } -void TouchScreenTask::stopUnlocked() { - if (arm_) { +bool TouchScreenTask::stopActivity() { + activity_generation_.fetch_add(1U, std::memory_order_acq_rel); + stop_requested_.store(true, std::memory_order_release); + + std::shared_ptr arm; + { + std::lock_guard lock(activity_arm_mutex_); + arm = activity_arm_; + } + + static std::atomic stop_sequence{0U}; + auto& authority = control::ControlAuthorityManager::instance(); + const auto expected_token = activityControlToken(); + control::ControlAcquireResult stop_barrier; + bool barrier_error = false; + if (arm && expected_token.valid()) { try { - arm_->stopL(); + stop_barrier = authority.preemptAcquireIfCurrent( + expected_token, + "touch-screen-stop:" + id_ + ":" + + std::to_string( + stop_sequence.fetch_add( + 1U, std::memory_order_relaxed) + 1U), + std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::hours(24))); + } catch (...) { + barrier_error = true; + (void)authority.quarantineIfCurrent(expected_token); + } + } + + // Only the caller which atomically converted this task's exact lease may + // touch the driver. If StopAll already owns the safety barrier, its arm + // stop runs independently while this task only drains its old step. + if (stop_barrier.acquired) { + try { + (void)arm->stopMotion(); } catch (...) { } } - sendZeroJointVelocity(); + bool was_active = false; + { + // A step holds this mutex through all of its arm submissions. Taking + // it here proves that the old step has exited before state is reset. + std::lock_guard lock(mutex_); + was_active = activity_active_.exchange( + false, std::memory_order_acq_rel); + if (was_active || activityControlToken().valid() || + isBusyUnlocked()) { + resetActivityUnlocked(); + } + clearActivitySafetyHooksUnlocked(); + releaseActivityControlUnlocked(); + } + + bool stopped = !barrier_error; + if (stop_barrier.acquired) { + const bool handler_released = authority.waitForPreemptedRelease( + stop_barrier.token, + control::ControlAuthorityManager::Duration::zero()); + if (handler_released) { + // A backend may have allowed the cancellation request to return + // without fully quiescing. Confirm once more after the old task + // step and every guarded dispatch have drained. + try { + stopped = arm->stopMotion().ok(); + } catch (...) { + stopped = false; + } + } else { + stopped = false; + } + + if (stopped) { + authority.release(stop_barrier.token); + } else { + (void)authority.retireSafetyHolder(stop_barrier.token); + } + } + + stop_requested_.store(false, std::memory_order_release); + return stopped; +} + +void TouchScreenTask::resetActivityUnlocked() { ibvs_.resetTwistCommandState(); - holdCurrentControlledPosition(); phase_ = Phase::IDLE; phase_after_retract_ = Phase::DONE; @@ -617,6 +1057,14 @@ Eigen::Vector3d TouchScreenTask::lastAlignErrorCamera() const { return last_align_error_camera_; } +std::string TouchScreenTask::controlDeviceId() const { + std::lock_guard lock(mutex_); + if (arm_ && !arm_->id().empty()) { + return arm_->id(); + } + return config_.devices().arm_id(); +} + std::string TouchScreenTask::stateString() const { return taskStateToString(state()); } @@ -660,6 +1108,7 @@ const char* TouchScreenTask::statusToString(const Status status) { case Status::RETRACTING: return "RETRACTING"; case Status::DONE: return "DONE"; case Status::STOPPED: return "STOPPED"; + case Status::SAFETY_ADMISSION_REVOKED: return "SAFETY_ADMISSION_REVOKED"; case Status::ROBOT_STATE_FAILED: return "ROBOT_STATE_FAILED"; case Status::ROBOT_COMMAND_FAILED: return "ROBOT_COMMAND_FAILED"; case Status::TASK_BUSY: return "TASK_BUSY"; @@ -1328,21 +1777,22 @@ bool TouchScreenTask::stepRetracting() { << ", final_tcp_delta_base=unavailable"; } - try { - arm_->stopL(); - } catch (...) { + if (!runArmStopIfCurrent([this] { + return arm_ && arm_->stopL().ok(); + })) { enterFailed(Status::ROBOT_COMMAND_FAILED); return false; } holdCurrentControlledPosition(); if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) && !moveToInitPositionIfEnabled()) { - phase_ = Phase::FAILED; - last_status_ = Status::ROBOT_COMMAND_FAILED; + finishActivityUnlocked( + Phase::FAILED, Status::ROBOT_COMMAND_FAILED); return false; } - phase_ = phase_after_retract_; - last_status_ = final_status_after_retract_; + const auto completed_phase = phase_after_retract_; + const auto completed_status = final_status_after_retract_; + finishActivityUnlocked(completed_phase, completed_status); return phase_ != Phase::FAILED; } @@ -1379,11 +1829,9 @@ bool TouchScreenTask::sendJointVelocity(const std::vector& qdot) const { device::JointVelocityCommand cmd; cmd.velocity = qdot; - const auto result = arm_->speedJ(cmd, 0.0, 0.0); - if (!result.ok()) { - return false; - } - return true; + return runArmActuationIfCurrent([this, &cmd] { + return arm_ && arm_->speedJ(cmd, 0.0, 0.0).ok(); + }); } bool TouchScreenTask::sendZeroJointVelocity() const { @@ -1470,11 +1918,9 @@ bool TouchScreenTask::holdCurrentControlledPosition() const { joints.position.push_back(it->second); } - const auto result = arm_->servoJ(joints); - if (!result.ok()) { - return false; - } - return true; + return runArmActuationIfCurrent([this, &joints] { + return arm_ && arm_->servoJ(joints).ok(); + }); } bool TouchScreenTask::buildInitJointPositions(std::vector& positions_out) const { @@ -1500,8 +1946,10 @@ bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() { device::MotionOptions options; options.velocity = config_.initialization().velocity(); options.acceleration = config_.initialization().acceleration(); - const auto result = arm_->moveJ(init_cmd, options); - if (!result.ok()) { + options.cancellation_requested = activityCancellationRequested(); + if (!runArmActuationIfCurrent([this, &init_cmd, &options] { + return arm_ && arm_->moveJ(init_cmd, options).ok(); + })) { last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } @@ -1521,17 +1969,17 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const { device::MotionOptions options; options.velocity = config_.initialization().velocity(); options.acceleration = config_.initialization().acceleration(); - const auto result = arm_->moveJ(init_cmd, options); - if (!result.ok()) { - return false; - } - return true; + options.cancellation_requested = activityCancellationRequested(); + return runArmActuationIfCurrent([this, &init_cmd, &options] { + return arm_ && arm_->moveJ(init_cmd, options).ok(); + }); } bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) { if (stop_forward_motion) { - const auto result = arm_->stopL(); - if (!result.ok()) { + if (!runArmStopIfCurrent([this] { + return arm_ && arm_->stopL().ok(); + })) { return false; } } @@ -1565,12 +2013,15 @@ bool TouchScreenTask::startTouchPhase() { last_status_ = Status::ROBOT_STATE_FAILED; return false; } - const auto result = arm_->speedL(toCartesianVelocity( - cmvr::common::math::toEigenVec6(speed_l.twist_tool())), - speed_l.acceleration(), - 0.0, - device::FrameType::Tool); - if (!result.ok()) { + const auto velocity = toCartesianVelocity( + cmvr::common::math::toEigenVec6(speed_l.twist_tool())); + if (!runArmActuationIfCurrent([this, velocity, &speed_l] { + return arm_ && arm_->speedL( + velocity, + speed_l.acceleration(), + 0.0, + device::FrameType::Tool).ok(); + })) { last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } @@ -1596,8 +2047,11 @@ bool TouchScreenTask::startTouchPhase() { options.jerk = move_l.jerk(); options.joint_velocity_limits.assign(move_l.joint_velocity_limits().begin(), move_l.joint_velocity_limits().end()); - const auto result = arm_->moveL(pose_cmd, options, device::FrameType::Tool); - if (!result.ok()) { + options.cancellation_requested = activityCancellationRequested(); + if (!runArmActuationIfCurrent([this, &pose_cmd, &options] { + return arm_ && arm_->moveL( + pose_cmd, options, device::FrameType::Tool).ok(); + })) { last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } @@ -1607,12 +2061,9 @@ bool TouchScreenTask::startTouchPhase() { return false; } - phase_ = Phase::DONE; - touch_command_started_ = false; - retract_command_started_ = false; + finishActivityUnlocked(Phase::DONE, Status::DONE); retract_start_position_valid_ = false; retract_start_position_base_.setZero(); - last_status_ = Status::DONE; return true; } @@ -1644,13 +2095,14 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, << ", start_tcp_base=unavailable"; } - const auto result = arm_->speedL(retract_cmd, - retract.acceleration(), - 0.0, - device::FrameType::Tool); - if (!result.ok()) { - CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] speedL failed: " - << result.message; + if (!runArmActuationIfCurrent([this, &retract_cmd, &retract] { + return arm_ && arm_->speedL( + retract_cmd, + retract.acceleration(), + 0.0, + device::FrameType::Tool).ok(); + })) { + CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] speedL failed"; return false; } @@ -1665,18 +2117,15 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, } void TouchScreenTask::enterFailed(const Status status) { - try { - if (arm_) { - arm_->stopL(); - } - } catch (...) { - } + (void)runArmStopIfCurrent([this] { + return arm_ && arm_->stopL().ok(); + }); hardStopIbvsMotion(); holdCurrentControlledPosition(); - phase_ = Phase::FAILED; - touch_command_started_ = false; - retract_command_started_ = false; - last_status_ = moveToInitPositionIfEnabled() ? status : Status::ROBOT_COMMAND_FAILED; + const auto final_status = moveToInitPositionIfEnabled() + ? status + : Status::ROBOT_COMMAND_FAILED; + finishActivityUnlocked(Phase::FAILED, final_status); } bool TouchScreenTask::updateTouchPressure() { diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp index 55a45b27..4ca05918 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,33 @@ #include "manager/task_manager/include/task_manager.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" +namespace cmvr::task { + +class TouchScreenTaskStopActivityTestPeer final { +public: + static void setActive( + TouchScreenTask& task, + const std::shared_ptr& arm, + const control::ControlLeaseToken& token) + { + { + std::lock_guard lock(task.activity_arm_mutex_); + task.activity_arm_ = arm; + } + { + std::lock_guard lock(task.activity_control_mutex_); + task.activity_control_token_ = token; + } + { + std::lock_guard lock(task.mutex_); + task.phase_ = TouchScreenTask::Phase::ALIGNING; + task.activity_active_.store(true, std::memory_order_release); + } + } +}; + +} // namespace cmvr::task + namespace { constexpr int kRealTargetU = 1280 / 2.0; @@ -44,6 +72,168 @@ constexpr std::array kJointNames = { "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" }; +class StopCountingRobotArm final : public cmvr::device::RobotArm { +public: + explicit StopCountingRobotArm(std::string id) + { + id_ = std::move(id); + } + + std::string typeName() const override { return "StopCountingRobotArm"; } + cmvr::device::RobotModel getRobotModel() const override { return {}; } + std::size_t getDof() const override { return 0U; } + cmvr::device::ArmState getRobotState() const override { return {}; } + cmvr::device::JointGroupState getJointState() const override { return {}; } + cmvr::device::CartesianPose getTcpPose( + cmvr::device::FrameType = cmvr::device::FrameType::Base) const override + { + return {}; + } + cmvr::device::RobotMode getRobotMode() const override + { + return cmvr::device::RobotMode::Idle; + } + cmvr::device::SafetyMode getSafetyMode() const override + { + return cmvr::device::SafetyMode::Normal; + } + cmvr::device::ControlMode getControlMode() const override + { + return cmvr::device::ControlMode::None; + } + cmvr::device::Result torqueOn() override { return success(); } + cmvr::device::Result torqueOff() override { return success(); } + cmvr::device::Result calibrateZeroQ(const std::string&) override + { + return success(); + } + cmvr::device::Result emergencyStop() override { return success(); } + cmvr::device::Result protectiveStop() override { return success(); } + cmvr::device::Result setSpeedScaling(double) override { return success(); } + double getSpeedScaling() const override { return 1.0; } + bool isProtectiveStopped() const override { return false; } + bool isEmergencyStopped() const override { return false; } + bool isFault() const override { return false; } + cmvr::device::Result moveJ( + const cmvr::device::JointPositionCommand&, + const cmvr::device::MotionOptions&) override + { + return success(); + } + cmvr::device::Result speedJ( + const cmvr::device::JointVelocityCommand&, double, double) override + { + return success(); + } + cmvr::device::Result stopJ(double) override { return success(); } + cmvr::device::Result moveL( + const cmvr::device::CartesianPose&, + const cmvr::device::MotionOptions&, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result speedL( + const cmvr::device::CartesianVelocity&, + double, + double, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result stopL(std::optional = std::nullopt) override + { + return success(); + } + cmvr::device::Result stopMotion() override + { + stop_motion_calls_.fetch_add(1, std::memory_order_relaxed); + return success(); + } + cmvr::device::Result startServoMode( + const cmvr::device::ServoOptions&) override + { + return success(); + } + cmvr::device::Result servoJ( + const cmvr::device::JointPositionCommand&) override + { + return success(); + } + cmvr::device::Result servoL( + const cmvr::device::CartesianPose&, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result servoSpeedJ( + const cmvr::device::JointVelocityCommand&) override + { + return success(); + } + cmvr::device::Result servoSpeedL( + const cmvr::device::CartesianVelocity&, + cmvr::device::FrameType = cmvr::device::FrameType::Base) override + { + return success(); + } + cmvr::device::Result stopServoMode() override { return success(); } + cmvr::device::Result connect(const std::string&, int) override + { + return success(); + } + cmvr::device::Result disconnect() override { return success(); } + bool isConnected() const override { return true; } + cmvr::device::Result powerOn() override { return success(); } + cmvr::device::Result powerOff() override { return success(); } + cmvr::device::Result brakeRelease() override { return success(); } + cmvr::device::Result shutdown() override { return success(); } + cmvr::device::Result clearFault() override { return success(); } + cmvr::device::Result unlockProtectiveStop() override { return success(); } + cmvr::device::Result loadProgram(const std::string&) override + { + return success(); + } + cmvr::device::Result playProgram() override { return success(); } + cmvr::device::Result pauseProgram() override { return success(); } + cmvr::device::Result stopProgram() override { return success(); } + std::vector ik( + const std::string&, + const std::string&, + const cmvr::device::CartesianPose&) override + { + return {}; + } + std::shared_ptr kinematicsSolver() const override + { + return nullptr; + } + cmvr::device::CartesianPose fk( + const std::string&, const std::string&) override + { + return {}; + } + cmvr::device::CartesianPose fk(bool = true) override { return {}; } + cmvr::device::CartesianVelocity getSpeedLCommandTwistBase() const override + { + return {}; + } + bool busy() const override { return false; } + + int stopMotionCalls() const + { + return stop_motion_calls_.load(std::memory_order_relaxed); + } + +private: + static cmvr::device::Result success() + { + return cmvr::device::Result::success(); + } + + std::atomic stop_motion_calls_{0}; +}; + std::filesystem::path findProjectRoot() { const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml"; @@ -422,6 +612,60 @@ void run_touch_once(int u, int v) { } // namespace +TEST(TouchScreenTaskTest, StandaloneStopOwnsAndConfirmsPhysicalArmStop) +{ + auto& authority = cmvr::control::ControlAuthorityManager::instance(); + authority.clear(); + const auto arm = std::make_shared( + "touch-standalone-stop-arm"); + const auto lease = authority.tryAcquire( + arm->id(), "touch-task-owner", std::chrono::hours(1)); + ASSERT_TRUE(lease.acquired) << lease.detail; + + cmvr::task::TouchScreenTask task( + cmvr::config::TouchScreenTaskConfig{}); + cmvr::task::TouchScreenTaskStopActivityTestPeer::setActive( + task, arm, lease.token); + + EXPECT_TRUE(task.stopActivity()); + EXPECT_EQ(arm->stopMotionCalls(), 2); + EXPECT_EQ(task.phase(), cmvr::task::TouchScreenTask::Phase::IDLE); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::STOPPED); + EXPECT_FALSE(authority.isLeased(arm->id())); + authority.clear(); +} + +TEST(TouchScreenTaskTest, StopSkipsArmWhenExternalSafetyBarrierAlreadyPreempted) +{ + auto& authority = cmvr::control::ControlAuthorityManager::instance(); + authority.clear(); + const auto arm = std::make_shared( + "touch-external-stop-arm"); + const auto lease = authority.tryAcquire( + arm->id(), "touch-task-owner", std::chrono::hours(1)); + ASSERT_TRUE(lease.acquired) << lease.detail; + + cmvr::task::TouchScreenTask task( + cmvr::config::TouchScreenTaskConfig{}); + cmvr::task::TouchScreenTaskStopActivityTestPeer::setActive( + task, arm, lease.token); + const auto external_barrier = authority.preemptAcquire( + arm->id(), "system-stop-all", std::chrono::hours(1)); + ASSERT_TRUE(external_barrier.acquired) << external_barrier.detail; + + EXPECT_TRUE(task.stopActivity()); + EXPECT_EQ(arm->stopMotionCalls(), 0); + EXPECT_EQ(task.phase(), cmvr::task::TouchScreenTask::Phase::IDLE); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::STOPPED); + EXPECT_TRUE(authority.validate(external_barrier.token)); + EXPECT_TRUE(authority.waitForPreemptedRelease( + external_barrier.token, + cmvr::control::ControlAuthorityManager::Duration::zero())); + authority.release(external_barrier.token); + EXPECT_FALSE(authority.isLeased(arm->id())); + authority.clear(); +} + TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) { const auto project_root = findProjectRoot(); ASSERT_FALSE(project_root.empty()); diff --git a/docs/teleoperation/ume_cmvr_architecture.md b/docs/teleoperation/ume_cmvr_architecture.md new file mode 100644 index 00000000..b86f9c4f --- /dev/null +++ b/docs/teleoperation/ume_cmvr_architecture.md @@ -0,0 +1,111 @@ +# UME to CMVR-ES Teleoperation Architecture + +## Scope + +This document freezes the first implementation stage of the wired, +cross-machine teleoperation path: + +- both edge computers run `cmvr_es`; +- the UME computer owns the Damiao SocketCAN-FD interfaces; +- `UmeRobotArm` is a `RobotArm` backend; +- the UME computer performs the leader/follower model calculations; +- the robot computer validates and executes joint-servo references; +- the transport is a versioned gRPC bidirectional stream; +- the original UME algorithm is migrated before any SEW fusion work. + +SEW fusion, passivity research, paper experiments, and physical human testing +are deliberately outside this implementation stage. + +## Target process and ownership boundary + +```text +UME cmvr_es + UmeRobotArm + Damiao SocketCAN-FD + UmeHapticLoop + local safety guard + | + +-- UmeLegacyAlgorithm + | + UmeTeleopTask + GrpcArmTeleopClient + | + | wired Ethernet / gRPC bidi stream + v +Robot cmvr_es + ArmTeleopService + control lease + latest-only command slot + watchdog + safe servo executor + | + v + RobotArm +``` + +The UME high-frequency loop never performs a network RPC. Network workers and +hardware loops exchange only bounded latest-state snapshots. + +## Implemented boundary in this revision + +This revision establishes the device, algorithm-library, session-transport and +robot-backend boundaries, but it intentionally does not connect them into a +physical end-to-end controller: + +- `UmeRobotArm` owns one eight-axis Damiao bus and a bounded local torque loop. + It starts passive, requires an explicit fresh torque command before + `torqueOn`, and latches watchdog/transport faults. +- a successful SocketCAN send means that the complete frame batch was accepted + by the local kernel before its deadline. The current MIT transport has no + reviewed Disable acknowledgement, so software must not describe that result + as actuator-confirmed torque-off. +- the original UME dynamics, friction, stiction and haptic projection code is a + standalone tested library under `algorithms/controllers/ume_legacy`; +- `UmeTeleopTask` owns reconnect, heartbeat, sequence and a capacity-one + outbound mailbox. Its `submitSetpoint()` input is deliberately an algorithm + boundary; no production source calls it yet; +- the server-side `RobotArmTeleopBackend` is implemented and tested behind an + explicit capability gate. The current `MotorRobotArm` remains unavailable + because its `servoJ` path is sequential per joint rather than an accepted + atomic/timed group primitive; +- server state is currently returned with session events. The configured + requested state rate is not yet an independent periodic publisher; +- follower effort is validated and transported when a backend declares a + verified source, but it is not yet consumed by a UME haptic coordinator. + +Accordingly, this code is an M0-M9 fail-closed framework and original-algorithm +migration, not a claim of runnable force-feedback teleoperation. The next +integration step must add a concrete UME-to-follower retargeting producer, +connect verified follower effort to the local haptic coordinator, and retain +the high-frequency/network-thread separation above. + +## Safety invariants + +1. Opening a CAN interface never enables a motor. +2. Clearing a fault never arms a motor. +3. A reconnect creates a new session and never restores active motion. +4. Cross-machine `steady_clock` values are diagnostic only. A receiver derives + command expiry from its local receive time plus `valid_for_us`. +5. Commands are strictly increasing by sequence within one session. +6. Queues on the cyclic path are latest-only and bounded. +7. Invalid or stale follower effort ramps haptic feedback to zero. +8. Model, joint order, units, and calibration hashes must match before motion. +9. New physical hardware configurations remain disabled by default. +10. A software stop does not replace an independent physical emergency stop. +11. UME hardware enable also requires an explicit firmware-reviewed feedback + status whitelist and raw temperature thresholds; empty values never mean + "accept all". +12. UME shutdown timing is diagnosed against a configured budget, but physical + torque removal still requires an independent emergency-stop path until a + reviewed actuator Disable acknowledgement exists. + +## Initial rate boundary + +- UME local hardware/haptic loop: configurable, initially 800 Hz to match the + legacy UME setup. +- network command rate: configurable independently of the local loop. +- robot servo rate: selected from the robot backend capability and never + inferred from the network rate. + +No hard real-time or stability claim is made until the target computers and +physical devices have completed staged validation. diff --git a/docs/teleoperation/ume_cmvr_validation.md b/docs/teleoperation/ume_cmvr_validation.md new file mode 100644 index 00000000..8fb1cda2 --- /dev/null +++ b/docs/teleoperation/ume_cmvr_validation.md @@ -0,0 +1,118 @@ +# UME / CMVR-ES validation gates + +This checklist is part of the first UME migration. Passing a software gate +does not authorize a physical power-on. The checked-in UME device entries and +their `hardware_enabled` fields remain `false`. + +The current revision has no production setpoint producer for +`UmeTeleopTask::submitSetpoint()` and no haptic consumer for returned follower +effort. M9 therefore validates the framework, protocol and migrated original +algorithm separately; it is not an end-to-end motion or force-feedback test. + +## M9: software and network validation + +Run these gates on both target CPU architectures before deploying: + +1. Build the complete `cmvr_es` target with tests enabled. +2. Run the UME legacy controller and Pinocchio model-adapter golden tests. +3. Run the Damiao codec, CAN-FD chain, and `UmeRobotArm` lifecycle tests. +4. Run the process-wide control-authority tests. +5. Run the gRPC client, `UmeTeleopTask`, and `ArmTeleopService` tests. +6. Repeat the concurrent client/task/service tests to screen for shutdown and + reconnect races. +7. Start each checked-in edge profile without UME hardware and verify that it + never opens `can4`/`can5` or issues actuator enable frames. + +The communication tests must demonstrate all of the following: + +- an `OpenSession` manifest mismatch is rejected before backend activation; +- a second controller cannot acquire the same robot control resource; +- sequence numbers are non-zero and strictly increasing per session; +- the sender and receiver use capacity-one, latest-only command storage; +- a setpoint whose local validity has expired is never dispatched; +- heartbeat loss, lease loss, stream cancellation, and backend failure call + the robot safe-stop boundary; +- reconnect clears pending motion intent and starts a new sequence space; +- `StopSession` is attempted before client cancellation; +- the legacy unary ArmService cannot issue motion, enable, calibration, or + fault-reset commands while the teleoperation lease is active; +- `torqueOff` and `stopMotion` remain available as safety overrides. + +Before a real follower backend can be enabled, control authority must also be +extended to every local arm task and to the underlying MotorService resources. +The current process-wide lease covers ArmTeleop and unary ArmService only. + +For a wired two-computer run, record at least: + +- one-way command age at the robot ingress; +- command mailbox overwrite and rejection counters; +- heartbeat and control-lease remaining time; +- servo apply duration and deadline misses; +- disconnect detection-to-safe-stop time; +- packet loss, reordering, and delay from an explicit network impairment + profile rather than an assumed LAN condition. + +No end-to-end stability or transparency claim is supported until those logs +are tied to a specified controller rate, robot servo period, payload, motion +envelope, and network impairment profile. + +## M10: staged physical commissioning + +Every row is a separate sign-off. Do not combine first power-on with a human +wearing the UME. + +- [ ] Independent physical emergency stop is installed and verified. +- [ ] CAN arbitration/data bitrates and CAN-FD+BRS MTU are verified for each + interface. +- [ ] Motor product, firmware, command ID, feedback ID, and reported motor ID + are read back and matched to the configuration. +- [ ] The four-bit Damiao feedback status meanings and raw temperature limits + are verified for the exact product/firmware and entered as an explicit + per-joint whitelist/threshold contract. +- [ ] Joint direction and zero reference are verified one joint at a time with + the mechanism unloaded. +- [ ] Mechanical position, velocity, and torque limits replace the checked-in + placeholders and receive an independent review. +- [ ] Motor feedback timestamps and the 800 Hz cycle are measured on the UME + target computer under load. +- [ ] The exact Damiao firmware's Disable acknowledgement semantics are + documented and verified. Until then, SocketCAN send success is only + evidence that the local kernel accepted the complete frame batch. +- [ ] Because MIT feedback has no sequence field, stale request/reply + correlation is resolved by a reviewed firmware marker or by measured, + enforced bus timing; draining only the frames already queued is not + sufficient evidence. +- [ ] Every UME control-thread I/O operation is shown to be deadline-bounded + and interruptible on the target kernel. The configured shutdown timeout + is currently a diagnostic failure threshold, not a C++ timed-join + primitive. +- [ ] With torque disabled, both edge profiles run for at least 30 minutes + without sequence, deadline, lease, or reconnect anomalies. +- [ ] With the UME fixed to a stand, each joint is enabled independently at a + low torque limit and its watchdog disable path is measured. +- [ ] Both UME arms are tested together on stands; CAN and CPU deadline margins + are recorded. +- [ ] The robot backend's group `servoJ` semantics and worst-case call duration + are measured. Sequential per-joint dispatch is not accepted as an + atomic group backend without a documented skew bound. +- [ ] gRPC writer backpressure cannot stall the independent robot watchdog, + and an expired lease cannot be regranted until safe stop is confirmed. +- [ ] TouchScreenTask and direct MotorService commands are either disabled by + the deployment profile or participate in the same resource authority. +- [ ] Robot-only low-speed setpoint execution is validated before connecting + the leader-side algorithm. +- [ ] Wired-network cable removal, peer process kill, delayed packets, stale + commands, duplicate sequences, lease theft, and robot fault injection + all lead to a bounded safe stop. +- [ ] The original UME gravity/friction/feedback controller is commissioned on + a stand with force feedback initially clamped to zero, then increased in + reviewed steps. +- [ ] Only after all previous evidence is archived may a supervised human test + be considered under a separate risk assessment. + +## Evidence record + +For each completed physical gate, archive the exact Git revision, installed +`output/` checksum, configuration files, model and calibration SHA-256 values, +test operator, hardware serial numbers, raw logs, and pass/fail decision. A +successful build or simulator run must not be recorded as physical validation. diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto index 5edc2004..47578e2e 100644 --- a/protos/cmvr/api/agv_command.proto +++ b/protos/cmvr/api/agv_command.proto @@ -3,7 +3,7 @@ syntax = "proto3"; package cmvr.api; import "cmvr/api/common.proto"; -import "cmvr/msgs/agv.proto"; +import "cmvr/api/agv_utils.proto"; // 查询 AGV 运行状态命令。 message AgvRuntimeStateCommand { @@ -62,7 +62,7 @@ message AgvNavigateToPoseCommand { CommandHeader.Request header = 1; // 目标位姿。x/y 单位：米，theta 单位：弧度。 cmvr.msgs.AgvPose2d pose = 2; - // 通用运动约束和执行选项。 + // 通用运动约束和执行选项；默认同步阻塞至任务终态并确认停车。 cmvr.msgs.AgvMotionOptions options = 3; // AGV 适配器扩展参数，用于传递厂商特有选项。 cmvr.msgs.AgvAdapterParams adapter_params = 4; @@ -82,7 +82,7 @@ message AgvNavigateToStationCommand { CommandHeader.Request header = 1; // 目标站点 id。 string station_id = 2; - // 通用运动约束和执行选项。 + // 通用运动约束和执行选项；默认同步阻塞至任务终态并确认停车。 cmvr.msgs.AgvMotionOptions options = 3; // AGV 适配器扩展参数，用于传递厂商特有选项。 cmvr.msgs.AgvAdapterParams adapter_params = 4; @@ -102,6 +102,8 @@ message AgvFollowPathCommand { CommandHeader.Request header = 1; // 路径段列表。每段包含起点站点 id 和终点站点 id。 repeated cmvr.msgs.AgvPathSegment path = 2; + // 通用执行选项；默认同步阻塞至整条路径终态并确认停车。 + cmvr.msgs.AgvMotionOptions options = 3; } // 反馈体。 message Feedback { @@ -110,6 +112,23 @@ message AgvFollowPathCommand { } } + +// 按指定速度执行固定距离平移命令。 +message AgvTranslateCommand { + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + // 固定距离平移参数。 + cmvr.msgs.AgvTranslation translation = 2; + } + + message Feedback { + // 仅表示控制器是否接受命令，不表示平移已经完成。 + CommandHeader.Feedback header = 1; + } +} + + // 下发底盘速度命令。 message AgvSetVelocityCommand { // 请求体。 diff --git a/protos/cmvr/api/agv_service.proto b/protos/cmvr/api/agv_service.proto index 04847156..b8de93da 100644 --- a/protos/cmvr/api/agv_service.proto +++ b/protos/cmvr/api/agv_service.proto @@ -72,4 +72,8 @@ service AgvService { // 停止当前建图/扫图会话。 rpc stopMapping(CommandHeader.Request) returns (CommandHeader.Feedback); + + + // 按指定速度平移固定距离。成功返回仅表示控制器已接受命令。 + rpc translate(AgvTranslateCommand.Request) returns (AgvTranslateCommand.Feedback); } diff --git a/protos/cmvr/msgs/agv.proto b/protos/cmvr/api/agv_utils.proto similarity index 100% rename from protos/cmvr/msgs/agv.proto rename to protos/cmvr/api/agv_utils.proto diff --git a/protos/cmvr/api/arm_service.proto b/protos/cmvr/api/arm_service.proto index a07018f7..a050532a 100644 --- a/protos/cmvr/api/arm_service.proto +++ b/protos/cmvr/api/arm_service.proto @@ -16,9 +16,11 @@ service ArmService { rpc servoJ(ServoJ.Request) returns (ServoJ.Response); rpc stopMotion(CommandHeader.Request) returns (CommandHeader.Feedback); rpc getJointState(JointRequest) returns (JointResponse); - rpc getRobotState(GetRobotState.Request) returns (GetRobotState.Response); rpc getPose(GetPose.Request) returns (GetPose.Response); rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response); rpc getPoseMatrix(GetPoseMatrix.Request) returns (GetPoseMatrix.Response); rpc computeForwardKinematics(ComputeForwardKinematics.Request) returns (ComputeForwardKinematics.Response); + + // Vendor-specific arm extension currently used for AUBO cabinet IO. + rpc ExecuteJsonCommand(JsonDeviceCommand.Request) returns (JsonDeviceCommand.Feedback); } diff --git a/protos/cmvr/api/arm_teleop_v1.proto b/protos/cmvr/api/arm_teleop_v1.proto new file mode 100644 index 00000000..d7095a95 --- /dev/null +++ b/protos/cmvr/api/arm_teleop_v1.proto @@ -0,0 +1,133 @@ +syntax = "proto3"; + +package cmvr.api.armteleop.v1; + +// Versioned, session-oriented protocol for wired arm teleoperation. Existing +// unary ArmService RPCs intentionally remain unchanged. +service ArmTeleopService { + rpc Teleoperate(stream ClientFrame) returns (stream ServerFrame); +} + +enum SessionPhase { + SESSION_PHASE_UNSPECIFIED = 0; + SESSION_PHASE_OPENED = 1; + SESSION_PHASE_READY = 2; + SESSION_PHASE_ACTIVE = 3; + SESSION_PHASE_HOLDING = 4; + SESSION_PHASE_STOPPED = 5; + SESSION_PHASE_WATCHDOG_EXPIRED = 6; + SESSION_PHASE_LEASE_LOST = 7; + SESSION_PHASE_REJECTED = 8; + SESSION_PHASE_FAILED = 9; +} + +enum StopReason { + STOP_REASON_UNSPECIFIED = 0; + STOP_REASON_OPERATOR_REQUEST = 1; + STOP_REASON_CLIENT_SHUTDOWN = 2; + STOP_REASON_WATCHDOG = 3; + STOP_REASON_LEASE_REVOKED = 4; + STOP_REASON_ROBOT_FAULT = 5; + STOP_REASON_EMERGENCY_STOP = 6; + STOP_REASON_PROTOCOL_ERROR = 7; +} + +enum EffortSource { + EFFORT_SOURCE_UNSPECIFIED = 0; + EFFORT_SOURCE_MOTOR_ESTIMATE = 1; + EFFORT_SOURCE_JOINT_SENSOR = 2; + EFFORT_SOURCE_FORCE_TORQUE_SENSOR = 3; + EFFORT_SOURCE_OBSERVER = 4; +} + +message RobotManifest { + string robot_id = 1; + string model_sha256 = 2; + string calibration_sha256 = 3; + repeated string joint_names = 4; + string position_unit = 5; + string velocity_unit = 6; + string effort_unit = 7; + string base_frame = 8; + string tool_frame = 9; +} + +message OpenSession { + uint32 protocol_major = 1; + uint32 protocol_minor = 2; + string client_instance_id = 3; + RobotManifest expected_robot = 4; + uint32 requested_command_rate_hz = 5; + uint32 requested_state_rate_hz = 6; + uint32 watchdog_timeout_ms = 7; + uint32 requested_lease_ms = 8; + bool request_force_feedback = 9; +} + +message JointSetpoint { + // Strictly increasing and non-zero within a session. + uint64 sequence = 1; + repeated double position_rad = 2; + repeated double velocity_rad_s = 3; + // The receiver computes its deadline from local arrival time plus this + // duration. Zero is invalid for an active setpoint. + uint32 valid_for_us = 4; +} + +message ClientHeartbeat { + uint64 sequence = 1; +} + +message StopSession { + StopReason reason = 1; + string detail = 2; +} + +message ClientFrame { + oneof payload { + OpenSession open = 1; + JointSetpoint setpoint = 2; + ClientHeartbeat heartbeat = 3; + StopSession stop = 4; + } +} + +message JointState { + uint64 sample_sequence = 1; + repeated double position_rad = 2; + repeated double velocity_rad_s = 3; + repeated double effort_nm = 4; + bool position_valid = 5; + bool velocity_valid = 6; + bool effort_valid = 7; + EffortSource effort_source = 8; + uint64 sample_age_us = 9; +} + +message SessionStatus { + string session_id = 1; + SessionPhase phase = 2; + uint64 received_sequence = 3; + uint64 applied_sequence = 4; + uint64 dropped_setpoints = 5; + uint64 rejected_setpoints = 6; + uint32 negotiated_watchdog_ms = 7; + uint32 lease_remaining_ms = 8; + StopReason stop_reason = 9; + string detail = 10; +} + +message RobotSafetyState { + bool connected = 1; + bool powered_on = 2; + bool protective_stopped = 3; + bool emergency_stopped = 4; + bool fault = 5; + string fault_detail = 6; +} + +message ServerFrame { + SessionStatus status = 1; + JointState joint_state = 2; + RobotSafetyState safety = 3; +} diff --git a/protos/cmvr/api/motor_command.proto b/protos/cmvr/api/motor_command.proto new file mode 100644 index 00000000..a15c1811 --- /dev/null +++ b/protos/cmvr/api/motor_command.proto @@ -0,0 +1,151 @@ +syntax = "proto3"; + +package cmvr.api; + +import "cmvr/api/common.proto"; +import "cmvr/msgs/motor.proto"; + +// Selects exactly one motor inside the MotorManager named by header.device_id. +message MotorTarget { + CommandHeader.Request header = 1; + oneof selector { + uint32 motor_id = 2; + string joint_name = 3; + } +} + +message MotorWaitOptions { + // Zero selects the server default (30 seconds). + uint32 timeout_ms = 1; + // Zero selects the server default (10 milliseconds). + uint32 poll_period_ms = 2; + // Zero selects the server default. + double position_tolerance_rad = 3; + double velocity_tolerance_rad_s = 4; + // Number of consecutive in-tolerance samples. Zero selects the default (3). + uint32 settle_sample_count = 5; +} + +enum MotorControlType { + MOTOR_CONTROL_NONE = 0; + MOTOR_CONTROL_SET_ZERO = 1; + MOTOR_CONTROL_PROFILE_POSITION = 2; + MOTOR_CONTROL_PROFILE_VELOCITY = 3; + MOTOR_CONTROL_CYCLIC_POSITION = 4; + MOTOR_CONTROL_CYCLIC_VELOCITY = 5; + MOTOR_CONTROL_SET_ENABLED = 6; +} + +message MotorStatus { + uint32 motor_id = 1; + string joint_name = 2; + cmvr.msgs.RunMode run_mode = 3; + double position_rad = 4; + double velocity_rad_s = 5; + bool target_reached = 6; + bool service_busy = 7; + MotorControlType active_control = 8; + bool emergency_stopped = 9; + string last_error = 10; +} + +message MotorCommandResponse { + CommandHeader.Feedback header = 1; + MotorStatus status = 2; + uint64 elapsed_ms = 3; +} + +message SetMotorZeroRequest { + MotorTarget target = 1; +} + +message MoveMotorToZeroRequest { + MotorTarget target = 1; + double max_velocity_rad_s = 2; + double acceleration_rad_s2 = 3; + MotorWaitOptions wait = 4; +} + +message ProfilePositionRequest { + MotorTarget target = 1; + double target_position_rad = 2; + double max_velocity_rad_s = 3; + double acceleration_rad_s2 = 4; + MotorWaitOptions wait = 5; +} + +message ProfileVelocityRequest { + MotorTarget target = 1; + double target_velocity_rad_s = 2; + double acceleration_rad_s2 = 3; + MotorWaitOptions wait = 4; +} + +message EmergencyStopRequest { + MotorTarget target = 1; +} + +message GetMotorStatusRequest { + MotorTarget target = 1; +} + +message GetMotorStatusResponse { + CommandHeader.Feedback header = 1; + MotorStatus status = 2; +} + +message SetMotorEnabledRequest { + MotorTarget target = 1; + bool enabled = 2; +} + +message CyclicStreamOpen { + MotorTarget target = 1; + // The device/driver watchdog is authoritative. This service watchdog prevents a + // stalled gRPC client from retaining control indefinitely. + uint32 watchdog_timeout_ms = 2; +} + +message CyclicPositionSetpoint { + uint64 sequence = 1; + double target_position_rad = 2; + optional double target_velocity_rad_s = 3; +} + +message CyclicVelocitySetpoint { + uint64 sequence = 1; + double target_velocity_rad_s = 2; +} + +message CyclicPositionRequest { + oneof payload { + CyclicStreamOpen open = 1; + CyclicPositionSetpoint setpoint = 2; + } +} + +message CyclicVelocityRequest { + oneof payload { + CyclicStreamOpen open = 1; + CyclicVelocitySetpoint setpoint = 2; + } +} + +enum CyclicStreamPhase { + CYCLIC_STREAM_PHASE_UNSPECIFIED = 0; + CYCLIC_STREAM_OPENED = 1; + CYCLIC_STREAM_APPLIED = 2; + CYCLIC_STREAM_STOPPED = 3; + CYCLIC_STREAM_WATCHDOG_EXPIRED = 4; + CYCLIC_STREAM_FAILED = 5; +} + +message CyclicControlResponse { + CommandHeader.Feedback header = 1; + CyclicStreamPhase phase = 2; + uint64 sequence = 3; + uint64 dropped_setpoints = 4; + // Present for OPENED and terminal responses. APPLIED deliberately omits + // live status so one cyclic sample does not trigger extra fieldbus reads. + MotorStatus status = 5; +} diff --git a/protos/cmvr/api/motor_service.proto b/protos/cmvr/api/motor_service.proto new file mode 100644 index 00000000..01f127f3 --- /dev/null +++ b/protos/cmvr/api/motor_service.proto @@ -0,0 +1,21 @@ +syntax = "proto3"; + +package cmvr.api; + +import "cmvr/api/motor_command.proto"; + +service MotorService { + rpc setZero(SetMotorZeroRequest) returns (MotorCommandResponse); + rpc moveToZero(MoveMotorToZeroRequest) returns (MotorCommandResponse); + rpc profilePosition(ProfilePositionRequest) returns (MotorCommandResponse); + rpc profileVelocity(ProfileVelocityRequest) returns (MotorCommandResponse); + + rpc streamCyclicPosition(stream CyclicPositionRequest) + returns (stream CyclicControlResponse); + rpc streamCyclicVelocity(stream CyclicVelocityRequest) + returns (stream CyclicControlResponse); + + rpc emergencyStop(EmergencyStopRequest) returns (MotorCommandResponse); + rpc getStatus(GetMotorStatusRequest) returns (GetMotorStatusResponse); + rpc setEnabled(SetMotorEnabledRequest) returns (MotorCommandResponse); +} diff --git a/protos/cmvr/api/system_service.proto b/protos/cmvr/api/system_service.proto index 60476ad0..ac961ac2 100644 --- a/protos/cmvr/api/system_service.proto +++ b/protos/cmvr/api/system_service.proto @@ -1,6 +1,5 @@ syntax = "proto3"; -import "cmvr/api/common.proto"; import "cmvr/api/system_command.proto"; import "cmvr/api/safety_command.proto"; @@ -13,7 +12,6 @@ service SystemService { rpc GetDeviceList(GetDeviceListCommand.Request) returns (GetDeviceListCommand.Feedback) {} rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {} - rpc ExecuteJsonCommand(JsonDeviceCommand.Request) returns (JsonDeviceCommand.Feedback) {} rpc StopAll(StopAllCommand.Request) returns (StopAllCommand.Feedback) {} diff --git a/protos/cmvr/config/device_manager_config/device_manager_config.proto b/protos/cmvr/config/device_manager_config/device_manager_config.proto index d36d328b..ff60aac7 100644 --- a/protos/cmvr/config/device_manager_config/device_manager_config.proto +++ b/protos/cmvr/config/device_manager_config/device_manager_config.proto @@ -1,6 +1,25 @@ syntax = "proto3"; package cmvr.config; +message SafetyManagerConfig { + enum EnforcementMode { + ENFORCEMENT_MODE_UNSPECIFIED = 0; + LEGACY = 1; + SHADOW = 2; + ENFORCE_SELECTED = 3; + ENFORCE_ALL = 4; + } + + EnforcementMode mode = 1; + repeated string enforced_device_ids = 2; + uint32 stop_all_timeout_ms = 3; + uint32 recovery_timeout_ms = 4; + uint32 command_ledger_result_capacity = 5; + uint32 command_ledger_total_id_capacity = 6; + uint32 event_history_capacity = 7; + bool fail_startup_on_missing_control_capability = 8; +} + message DeviceConfigEntry { enum DeviceType { reserved 1, 2, 3, 4, 5, 6, 7, 10, 11; @@ -27,6 +46,10 @@ message DeviceConfigEntry { DeviceType type = 2; string config_file = 3; bool enable = 4; + bool safety_enforce = 5; + uint32 maximum_safety_snapshot_age_ms = 6; + uint32 safety_stop_timeout_ms = 7; + bool required_control_device = 8; } message DeviceManagerConfig { @@ -35,6 +58,7 @@ message DeviceManagerConfig { string description = 3; repeated DeviceConfigEntry devices = 4; bool init_all_motors_when_no_active_joints = 20; + SafetyManagerConfig safety = 21; } message DeviceManagerRootConfig { DeviceManagerConfig device_manager = 1; diff --git a/protos/cmvr/config/grpc_server_config/grpc_server_config.proto b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto index 0650c735..f054f223 100644 --- a/protos/cmvr/config/grpc_server_config/grpc_server_config.proto +++ b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto @@ -1,6 +1,68 @@ syntax = "proto3"; package cmvr.config; +message ArmTeleopBackendConfig { + // Two independent gates are required: this service-level switch and the + // RobotArm implementation's teleop group-servo capability. + bool enable = 1; + // Also becomes RobotManifest.robot_id and the process-wide control lease + // resource. It must exactly match RobotArm.id(). + string device_id = 2; + string model_sha256 = 3; + string calibration_sha256 = 4; + string base_frame = 5; + string tool_frame = 6; + double servo_period_s = 7; + // Maximum wall time allowed for one RobotArm::servoJ call. + uint32 max_apply_duration_us = 8; + // Omission is interpreted as true by the backend. Explicit false is intended + // only for simulation and independently supervised commissioning. + optional bool require_powered = 9; + // Bounds the first target relative to the cached measured position. + double max_initial_position_step_rad = 10; + // Bounds every later target relative to the last accepted target. + double max_position_step_rad = 11; +} + +message GRPCSecurityConfig { + enum TransportMode { + TRANSPORT_MODE_UNSPECIFIED = 0; + INSECURE = 1; + SERVER_TLS = 2; + MUTUAL_TLS = 3; + } + + enum AuthenticationMode { + AUTHENTICATION_MODE_UNSPECIFIED = 0; + DISABLED = 1; + STATIC_TOKEN = 2; + JWT = 3; + TLS_CLIENT_CERTIFICATE = 4; + } + + enum RecoveryExposure { + RECOVERY_EXPOSURE_UNSPECIFIED = 0; + RECOVERY_DISABLED = 1; + RECOVERY_LOCAL_ONLY = 2; + RECOVERY_AUTHORIZED = 3; + } + + TransportMode transport_mode = 1; + AuthenticationMode authentication_mode = 2; + RecoveryExposure recovery_exposure = 3; + bool allow_insecure_non_loopback = 4; + + // Reserved for optional providers. Selecting an unsupported provider causes + // startup to fail; it never falls back to DISABLED. + string server_certificate_file = 5; + string server_private_key_file = 6; + string client_ca_file = 7; + string static_token_file = 8; + string jwt_issuer = 9; + string jwt_audience = 10; + string audit_file = 11; +} + message GRPCServerConfig { string host = 1; string port = 2; @@ -12,6 +74,8 @@ message GRPCServerConfig { // Frames older than this monotonic age are not sent. Zero uses the service // default so configurations written before these fields remain low-latency. uint32 camera_stream_max_frame_age_ms = 6; + ArmTeleopBackendConfig arm_teleop_backend = 7; + GRPCSecurityConfig security = 8; } message GRPCServerRootConfig { GRPCServerConfig grpc_server = 1; diff --git a/protos/cmvr/config/task_manager_config/task_manager_config.proto b/protos/cmvr/config/task_manager_config/task_manager_config.proto index 8c60e370..cf6479e5 100644 --- a/protos/cmvr/config/task_manager_config/task_manager_config.proto +++ b/protos/cmvr/config/task_manager_config/task_manager_config.proto @@ -8,6 +8,7 @@ message TaskConfigEntry { TASK_TYPE_GRPC_SERVER = 3; TASK_TYPE_SELF_COLLISION = 4; TASK_TYPE_QUIC_EDGE = 5; + TASK_TYPE_UME_TELEOP = 6; } enum TaskRunMode { diff --git a/test/e2e/CMakeLists.txt b/test/e2e/CMakeLists.txt index 88649bd6..15b4b148 100644 --- a/test/e2e/CMakeLists.txt +++ b/test/e2e/CMakeLists.txt @@ -9,9 +9,9 @@ if(NOT TARGET cmvr_es::quic_test_gateway) endif() if(NOT TARGET cmvr_es::quic_edge_service OR - NOT TARGET cmvr_es::media_source_hub) + NOT TARGET cmvr_es::media_source_manager) message(FATAL_ERROR - "Real QUIC E2E requires the production QUIC service and MediaSourceHub") + "Real QUIC E2E requires the production QUIC service and MediaSourceManager") endif() add_executable(cmvr_quic_msquic_e2e_test @@ -22,7 +22,7 @@ target_link_libraries(cmvr_quic_msquic_e2e_test PRIVATE cmvr_es::quic_test_gateway cmvr_es::quic_edge_service - cmvr_es::media_source_hub + cmvr_es::media_source_manager ) if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") diff --git a/test/e2e/README.md b/test/e2e/README.md index e0e4e7ba..e88e8e84 100644 --- a/test/e2e/README.md +++ b/test/e2e/README.md @@ -11,7 +11,7 @@ fake transport，也不要求连接物理设备。 | `cmvr_quic_msquic_e2e_test` | 测试进程内同时运行 Gateway 和生产 `QuicEdgeService` | TLS/ALPN、注册、DeviceManager 合成快照、至少两次心跳 ACK、H.264/AAC descriptor 精确字段、真实 DATAGRAM、分片、序列号、flags、长度与载荷哈希 | | `cmvr_es_quic_process_smoke_test` | 分别启动测试 Gateway 和真实 `cmvr_es` 子进程 | 临时配置加载、`QuicEdgeTask` 工厂和生命周期、节点注册、IP、禁用设备过滤、已启用设备创建失败上报、本地心跳周期、至少两次心跳 ACK、SIGTERM 安全退出 | -第一项向生产 `MediaSourceHub` 注册两个有界 synthetic source： +第一项向生产 `MediaSourceManager` 注册两个有界 synthetic source： - 2500 字节的 H.264 Annex B IDR 视频帧，用于覆盖 DATAGRAM 分片； - 带 ADTS header 的 AAC-LC 48 kHz 双声道音频帧。