diff --git a/CMakeLists.txt b/CMakeLists.txt index 7eef405c..f87b4459 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,29 +9,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) #set(CMAKE_CXX_STANDARD_REQUIRED True) set(CMAKE_POSITION_INDEPENDENT_CODE ON) -# 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) @@ -69,22 +47,7 @@ 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 - ) -]=]) -if(BUILD_TESTING AND CMVR_EXTERNAL_LIBRARY_DIRS) - list(JOIN CMVR_EXTERNAL_LIBRARY_DIRS ":" CMVR_TEST_EXTERNAL_LIBRARY_PATH) -endif() # 在调用 setup_external_libs 之后 message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}") message(STATUS "CMAKE_SHARED_LINKER_FLAGS: ${CMAKE_SHARED_LINKER_FLAGS}") @@ -102,9 +65,8 @@ file(GLOB_RECURSE PROTO_FILES ${PROTO_IMPORT_DIR}/*.proto) set(Protobuf_PROTOC_EXECUTABLE "${CMAKE_INSTALL_PREFIX}/bin/protoc" CACHE FILEPATH "" FORCE) -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" +set_property(TARGET gRPC::grpc_cpp_plugin + PROPERTY IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin" ) # 1) 先做 OBJECT：只负责生成/编译 pb.cc diff --git a/README.md b/README.md index 5913f0fd..1bcb8565 100644 --- a/README.md +++ b/README.md @@ -138,14 +138,3 @@ $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/assets/toppra b/assets/toppra new file mode 160000 index 00000000..3089c789 --- /dev/null +++ b/assets/toppra @@ -0,0 +1 @@ +Subproject commit 3089c7897a5711aceb39d25919aca8c57b5c5948 diff --git a/cmake/FindExternalLib.cmake b/cmake/FindExternalLib.cmake index 3e2a0b2c..44a25ea7 100644 --- a/cmake/FindExternalLib.cmake +++ b/cmake/FindExternalLib.cmake @@ -139,7 +139,6 @@ 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 1d6f8080..0ff7c443 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -6,16 +6,11 @@ add_subdirectory(hardware) add_subdirectory(algorithms) add_subdirectory(simulate) add_subdirectory(devices) -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_manager) +add_subdirectory(manager/media_source_hub) add_subdirectory(service/quic_edge) add_subdirectory(task) add_subdirectory(task/quic_edge_task) -add_subdirectory(service/grpc/client) -add_subdirectory(task/ume_teleop_task) add_subdirectory(manager/task_manager) add_subdirectory(service) add_subdirectory(runtime) diff --git a/cmvr-es/algorithms/controllers/CMakeLists.txt b/cmvr-es/algorithms/controllers/CMakeLists.txt index 4cc76775..1921842a 100644 --- a/cmvr-es/algorithms/controllers/CMakeLists.txt +++ b/cmvr-es/algorithms/controllers/CMakeLists.txt @@ -1,6 +1,5 @@ add_subdirectory(arm_control) -add_subdirectory(ume_legacy) #find_package(VISP REQUIRED) diff --git a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h index 78fd5bb8..9dd0fc29 100644 --- a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h +++ b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h @@ -52,16 +52,8 @@ public: private: void ensureWorkerStarted_(); - void workerLoop_(std::uint64_t worker_generation); - bool workerGenerationCurrent_(std::uint64_t worker_generation) const; - std::optional sendVelocityIfCurrent_( - const JointVelocityCommand& velocity, - double acceleration, - std::uint64_t worker_generation); - void finishCommandIfCurrent_(std::uint64_t command_version, - std::uint64_t worker_generation); - void sendZeroIfCurrent_(std::uint64_t worker_generation); - void sendZeroNow_(); + void workerLoop_(); + void sendZero_(); static double velocityNorm_(const std::vector& velocity); static double twistNorm_(const CartesianVelocity& velocity); @@ -73,25 +65,15 @@ private: ReadStateCallback read_state_; SendVelocityCallback send_velocity_; - // lifecycle_mutex_ serializes worker creation, join, and reset. It is held - // across join so a new command cannot start until the retired worker exits. - mutable std::mutex lifecycle_mutex_; std::unique_ptr worker_; - std::atomic worker_running_{false}; - mutable std::mutex mutex_; std::condition_variable cv_; - bool stop_requested_{false}; + std::atomic stop_requested_{false}; bool command_active_{false}; CartesianVelocity target_twist_{}; FrameType target_frame_{FrameType::Base}; double target_acceleration_{0.25}; std::uint64_t command_version_{0}; - - // Every worker output is checked while holding output_mutex_. shutdown() - // advances the generation before sending zero, fencing stale worker writes. - mutable std::mutex output_mutex_; - std::atomic worker_generation_{0}; std::atomic busy_{false}; }; diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp index 16237be1..efad63b6 100644 --- a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp +++ b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp @@ -61,23 +61,20 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) { return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input"); } + if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); + } + + ensureWorkerStarted_(); + std::uint64_t command_version = 0; { - std::lock_guard lifecycle_lock(lifecycle_mutex_); - if (worker_ && worker_->joinable() && !worker_running_.load()) { - worker_->join(); - worker_.reset(); - } - ensureWorkerStarted_(); - { - std::lock_guard lock(mutex_); - target_twist_ = velocity; - target_acceleration_ = acceleration; - target_frame_ = frame; - command_active_ = true; - command_version = ++command_version_; - } - busy_.store(true); + std::lock_guard lock(mutex_); + target_twist_ = velocity; + target_acceleration_ = acceleration; + target_frame_ = frame; + command_active_ = true; + command_version = ++command_version_; } cv_.notify_all(); @@ -103,21 +100,16 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, Result CartesianVelocityController::stop(const std::optional acceleration) { + if (!worker_ || !worker_->joinable()) { + return Result::success(); + } { - std::lock_guard lifecycle_lock(lifecycle_mutex_); - if (!worker_ || !worker_->joinable() || !worker_running_.load()) { - return Result::success(); - } - { - std::lock_guard lock(mutex_); - target_twist_ = {}; - target_frame_ = FrameType::Base; - target_acceleration_ = - acceleration.has_value() ? *acceleration - : config_.stop_acceleration; - command_active_ = true; - ++command_version_; - } + std::lock_guard lock(mutex_); + target_twist_ = {}; + target_frame_ = FrameType::Base; + target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration; + command_active_ = true; + ++command_version_; } cv_.notify_all(); return Result::success(); @@ -125,35 +117,22 @@ Result CartesianVelocityController::stop(const std::optional acceleratio void CartesianVelocityController::shutdown() { - std::lock_guard lifecycle_lock(lifecycle_mutex_); if (!worker_ || !worker_->joinable()) { busy_.store(false); return; } - - // Revoke the worker before issuing zero. All worker outputs perform their - // final generation check under output_mutex_, so none can follow this zero. - worker_generation_.fetch_add(1, std::memory_order_acq_rel); { std::lock_guard lock(mutex_); - stop_requested_ = true; + stop_requested_.store(true); command_active_ = false; target_twist_ = {}; target_frame_ = FrameType::Base; - ++command_version_; } cv_.notify_all(); - - sendZeroNow_(); - busy_.store(false); - worker_->join(); worker_.reset(); - { - std::lock_guard lock(mutex_); - stop_requested_ = false; - command_active_ = false; - } + stop_requested_.store(false); + busy_.store(false); } CartesianVelocity CartesianVelocityController::getCommandTwistBase() const @@ -169,26 +148,12 @@ void CartesianVelocityController::ensureWorkerStarted_() if (worker_ && worker_->joinable()) { return; } - const auto worker_generation = - worker_generation_.fetch_add(1, std::memory_order_acq_rel) + 1; - { - std::lock_guard lock(mutex_); - stop_requested_ = false; - command_active_ = false; - } - worker_running_.store(true); - worker_ = std::make_unique( - &CartesianVelocityController::workerLoop_, this, worker_generation); + stop_requested_.store(false); + worker_ = std::make_unique(&CartesianVelocityController::workerLoop_, this); } -void CartesianVelocityController::workerLoop_( - const std::uint64_t worker_generation) +void CartesianVelocityController::workerLoop_() { - struct RunningGuard { - std::atomic& running; - ~RunningGuard() { running.store(false); } - } running_guard{worker_running_}; - const double dt = config_.control_period_s; auto next_tick = std::chrono::steady_clock::now(); @@ -199,10 +164,9 @@ void CartesianVelocityController::workerLoop_( { std::unique_lock lock(mutex_); cv_.wait(lock, [&]() { - return stop_requested_ || command_active_; + return stop_requested_.load() || command_active_; }); - if (stop_requested_ || - !workerGenerationCurrent_(worker_generation)) { + if (stop_requested_.load()) { break; } target_twist = target_twist_; @@ -212,44 +176,43 @@ void CartesianVelocityController::workerLoop_( next_tick = std::chrono::steady_clock::now(); while (true) { - bool stopping = false; - std::uint64_t active_command_version = 0; { std::lock_guard lock(mutex_); - stopping = stop_requested_; + if (stop_requested_.load()) { + sendZero_(); + busy_.store(false); + return; + } if (!command_active_) { break; } target_twist = target_twist_; acceleration = target_acceleration_; target_frame = target_frame_; - active_command_version = command_version_; - } - if (stopping || - !workerGenerationCurrent_(worker_generation)) { - return; } if (!planner_->updateSpeedLAcceleration(acceleration)) { if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) { - finishCommandIfCurrent_( - active_command_version, worker_generation); + std::lock_guard lock(mutex_); + command_active_ = false; + sendZero_(); + busy_.store(false); break; } CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration=" << acceleration; - finishCommandIfCurrent_( - active_command_version, worker_generation); - break; + sendZero_(); + busy_.store(false); + return; } std::vector q_now; std::vector qd_now; if (!read_state_(q_now, qd_now)) { CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed"; - finishCommandIfCurrent_( - active_command_version, worker_generation); - break; + sendZero_(); + busy_.store(false); + return; } std::vector qd_cmd; @@ -259,31 +222,31 @@ void CartesianVelocityController::workerLoop_( << target_twist.vz << ", " << target_twist.wx << ", " << target_twist.wy << ", " << target_twist.wz << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base"); - finishCommandIfCurrent_( - active_command_version, worker_generation); - break; + sendZero_(); + busy_.store(false); + return; } JointVelocityCommand velocity_command; velocity_command.velocity = qd_cmd; - const auto send_result = sendVelocityIfCurrent_( - velocity_command, acceleration, worker_generation); - if (!send_result.has_value()) { - return; - } - if (!send_result->ok()) { + const auto send_result = send_velocity_(velocity_command, acceleration); + if (!send_result.ok()) { CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: " - << send_result->message; - finishCommandIfCurrent_( - active_command_version, worker_generation); - break; + << send_result.message; + sendZero_(); + busy_.store(false); + return; } if (twistNorm_(target_twist) < config_.stop_twist_norm && velocityNorm_(qd_cmd) < config_.stop_command_velocity_norm && velocityNorm_(qd_now) < config_.stop_measured_velocity_norm) { - finishCommandIfCurrent_( - active_command_version, worker_generation); + { + std::lock_guard lock(mutex_); + command_active_ = false; + } + sendZero_(); + busy_.store(false); break; } @@ -293,67 +256,12 @@ void CartesianVelocityController::workerLoop_( } } - sendZeroIfCurrent_(worker_generation); - if (workerGenerationCurrent_(worker_generation)) { - busy_.store(false); - } + sendZero_(); + busy_.store(false); } -bool CartesianVelocityController::workerGenerationCurrent_( - const std::uint64_t worker_generation) const +void CartesianVelocityController::sendZero_() { - return worker_generation_.load(std::memory_order_acquire) == - worker_generation; -} - -std::optional CartesianVelocityController::sendVelocityIfCurrent_( - const JointVelocityCommand& velocity, - const double acceleration, - const std::uint64_t worker_generation) -{ - std::lock_guard lock(output_mutex_); - if (!workerGenerationCurrent_(worker_generation)) { - return std::nullopt; - } - return send_velocity_(velocity, acceleration); -} - -void CartesianVelocityController::finishCommandIfCurrent_( - const std::uint64_t command_version, - const std::uint64_t worker_generation) -{ - std::lock_guard output_lock(output_mutex_); - { - std::lock_guard lock(mutex_); - if (command_version_ != command_version) { - return; - } - command_active_ = false; - busy_.store(false); - } - if (!workerGenerationCurrent_(worker_generation)) { - return; - } - JointVelocityCommand zero; - zero.velocity.assign(dof_, 0.0); - (void)send_velocity_(zero, 0.0); -} - -void CartesianVelocityController::sendZeroIfCurrent_( - const std::uint64_t worker_generation) -{ - std::lock_guard lock(output_mutex_); - if (!workerGenerationCurrent_(worker_generation) || !send_velocity_) { - return; - } - JointVelocityCommand zero; - zero.velocity.assign(dof_, 0.0); - (void)send_velocity_(zero, 0.0); -} - -void CartesianVelocityController::sendZeroNow_() -{ - std::lock_guard lock(output_mutex_); if (!send_velocity_) { return; } diff --git a/cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt b/cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt deleted file mode 100644 index 73bc703c..00000000 --- a/cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt +++ /dev/null @@ -1,78 +0,0 @@ -add_library(ume_legacy_controller SHARED - src/ume_legacy_controller.cpp - src/pinocchio_ume_legacy_model_adapter.cpp -) - -target_include_directories(ume_legacy_controller - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/include -) - -target_link_libraries(ume_legacy_controller - PRIVATE - pinocchio_default - pinocchio_parsers -) - -add_library( - cmvr_es::algorithms::ume_legacy - ALIAS ume_legacy_controller -) - -install(TARGETS ume_legacy_controller LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - add_executable(ume_legacy_controller_golden_test - tests/ume_legacy_controller_golden_test.cpp - ) - add_executable(pinocchio_ume_legacy_model_adapter_test - tests/pinocchio_ume_legacy_model_adapter_test.cpp - ) - - target_link_libraries(ume_legacy_controller_golden_test - PRIVATE - cmvr_es::algorithms::ume_legacy - gtest - gtest_main - pthread - ) - target_link_libraries(pinocchio_ume_legacy_model_adapter_test - PRIVATE - cmvr_es::algorithms::ume_legacy - gtest - gtest_main - pthread - ) - - foreach(_ume_legacy_test_target - ume_legacy_controller_golden_test - pinocchio_ume_legacy_model_adapter_test) - target_compile_definitions(${_ume_legacy_test_target} - PRIVATE - CMVR_UME_FIXED_MODEL_PATH="${CMAKE_SOURCE_DIR}/model/ume/v6_bimanual/robot.xml" - CMVR_UME_FLOATING_MODEL_PATH="${CMAKE_SOURCE_DIR}/model/ume/v6_imu/robot.xml" - ) - add_test( - NAME ${_ume_legacy_test_target} - COMMAND ${_ume_legacy_test_target} - ) - endforeach() - - set(_ume_legacy_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _ume_legacy_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - - foreach(_ume_legacy_test_target - ume_legacy_controller_golden_test - pinocchio_ume_legacy_model_adapter_test) - set_tests_properties(${_ume_legacy_test_target} PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_ume_legacy_test_environment}" - ) - endforeach() -endif() diff --git a/cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h b/cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h deleted file mode 100644 index 720e0f5c..00000000 --- a/cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H -#define CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H - -#include -#include -#include - -#include "ume_legacy_model_adapter.h" - -namespace cmvr::ume_legacy { - -struct UmeLegacyModelContract { - std::size_t fixed_nq{0}; - std::size_t fixed_nv{0}; - std::size_t floating_nq{0}; - std::size_t floating_nv{0}; - Transform4x4RowMajor base_from_imu{}; -}; - -// Concrete adapter for the two original UME MJCF models. -// -// Construction parses both models and throws std::runtime_error if their -// dimensions, joint ordering, joint coordinate indices, or required frame -// topology differ from the frozen structural legacy contract. -// -// The floating model evaluates the original rnea(q, measured_arm_velocity, 0) -// path: base twist and all accelerations are zero. Consequently its result -// preserves the legacy velocity-dependent terms as well as gravity. -// -// Pinocchio Data objects are mutable workspaces. One adapter instance must be -// used by one controller thread at a time. All Eigen workspaces are allocated -// at construction and reused on the control path. -class PinocchioUmeLegacyModelAdapter final - : public UmeLegacyModelAdapter { -public: - PinocchioUmeLegacyModelAdapter( - std::string fixed_model_path, - std::string floating_model_path); - ~PinocchioUmeLegacyModelAdapter() override; - - PinocchioUmeLegacyModelAdapter( - const PinocchioUmeLegacyModelAdapter&) = delete; - PinocchioUmeLegacyModelAdapter& operator=( - const PinocchioUmeLegacyModelAdapter&) = delete; - PinocchioUmeLegacyModelAdapter( - PinocchioUmeLegacyModelAdapter&&) noexcept; - PinocchioUmeLegacyModelAdapter& operator=( - PinocchioUmeLegacyModelAdapter&&) noexcept; - - bool computeGravityCompensation( - const BimanualModelState& state, - JointVector& right_gravity_nm, - JointVector& left_gravity_nm) const override; - - bool projectHapticFeedback( - const BimanualModelState& state, - ArmSide side, - const RawHapticFeedback& feedback, - ProjectedHapticEffort& projected) const override; - - const UmeLegacyModelContract& contract() const noexcept; - const std::string& fixedModelPath() const noexcept; - const std::string& floatingModelPath() const noexcept; - -private: - class Impl; - std::unique_ptr impl_; -}; - -} // namespace cmvr::ume_legacy - -#endif // CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H diff --git a/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h deleted file mode 100644 index 4c706704..00000000 --- a/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef CMVR_ES_UME_LEGACY_CONTROLLER_H -#define CMVR_ES_UME_LEGACY_CONTROLLER_H - -#include "ume_legacy_types.h" - -namespace cmvr::ume_legacy { - -// Constants from UME commit e087df5cd3b281418722e155d9975695f163698e: -// ume/robot/ume/v6_imu/ume_leader/controller.py -// ume/robot/openarm1/teleop_leader_tuning.py -LegacyUmeTuning originalTuning() noexcept; - -JointVector frictionCompensation( - const JointVector& velocity_rad_s, - const LegacyUmeTuning& tuning) noexcept; - -JointVector stictionCompensation( - const JointVector& velocity_rad_s, - const LegacyUmeTuning& tuning) noexcept; - -// error_norm is non-negative in the legacy path because it is produced by -// np.linalg.norm. std::abs is retained here to match the subsequent Python -// expression exactly for direct unit-level use. -double feedbackScale( - double error_norm, - const LegacyUmeTuning& tuning) noexcept; - -SideControlOutput computeSideCommand( - const SideControlInput& input, - const LegacyUmeTuning& tuning = originalTuning()) noexcept; - -} // namespace cmvr::ume_legacy - -#endif // CMVR_ES_UME_LEGACY_CONTROLLER_H diff --git a/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h deleted file mode 100644 index 68a6d0a6..00000000 --- a/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H -#define CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H - -#include "ume_legacy_types.h" - -namespace cmvr::ume_legacy { - -// Boundary for the two model operations used by the original IMU controller: -// 1. floating-base RNEA gravity compensation; -// 2. fixed-base J_rot^T projection of shoulder/wrist moments. -// -// Concrete implementations must load and validate their model contract so the -// pure controller cannot silently substitute guessed kinematics or dynamics. -class UmeLegacyModelAdapter { -public: - virtual ~UmeLegacyModelAdapter() = default; - - virtual bool computeGravityCompensation( - const BimanualModelState& state, - JointVector& right_gravity_nm, - JointVector& left_gravity_nm) const = 0; - - virtual bool projectHapticFeedback( - const BimanualModelState& state, - ArmSide side, - const RawHapticFeedback& feedback, - ProjectedHapticEffort& projected) const = 0; -}; - -} // namespace cmvr::ume_legacy - -#endif // CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H diff --git a/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h deleted file mode 100644 index fc7dc211..00000000 --- a/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h +++ /dev/null @@ -1,110 +0,0 @@ -#ifndef CMVR_ES_UME_LEGACY_TYPES_H -#define CMVR_ES_UME_LEGACY_TYPES_H - -#include -#include - -namespace cmvr::ume_legacy { - -inline constexpr std::size_t kArmDof = 8; -inline constexpr std::size_t kTransformElementCount = 16; - -using JointVector = std::array; -using Vector3 = std::array; -using Transform4x4RowMajor = - std::array; - -enum class ArmSide { - Right, - Left -}; - -// Shoulder and wrist entries have already been projected by J_rot^T. The -// elbow and gripper entries are the scalar follower efforts received by the -// original UME controller. Keeping this type separate prevents a 3-D moment -// from being mislabeled as a 6-D Cartesian wrench. -struct ProjectedHapticEffort { - Vector3 shoulder_joint_torque{}; - double elbow_effort{0.0}; - Vector3 wrist_joint_torque{}; - double gripper_effort{0.0}; -}; - -struct TrackingError { - Vector3 shoulder_rotation{}; - double elbow{0.0}; - Vector3 wrist_rotation{}; - double gripper{0.0}; -}; - -struct FeedbackScales { - double shoulder{0.0}; - double elbow{0.0}; - double wrist{0.0}; - double gripper{0.0}; -}; - -struct LegacyUmeTuning { - JointVector friction_coefficient{}; - JointVector friction_max_compensation{}; - JointVector stiction_threshold_min_rad_s{}; - JointVector stiction_threshold_max_rad_s{}; - JointVector stiction_compensation{}; - - double feedback_error_tolerance_rad{0.0}; - double feedback_tanh_sharpness{0.0}; - double feedback_scale{0.0}; - double feedback_limit_dm4340_nm{0.0}; - double feedback_limit_dm4310_nm{0.0}; -}; - -struct SideControlInput { - ArmSide side{ArmSide::Right}; - JointVector joint_velocity_rad_s{}; - JointVector gravity_compensation_nm{}; - ProjectedHapticEffort projected_haptic{}; - TrackingError tracking_error{}; -}; - -struct SideControlOutput { - JointVector friction_compensation_nm{}; - JointVector stiction_compensation_nm{}; - JointVector feedforward_without_haptic_nm{}; - - // This is the interaction effort after the legacy left/right scalar sign - // conventions, but before scaling and clipping. - JointVector signed_interaction_nm{}; - FeedbackScales feedback_scales{}; - - // The legacy algorithm clips only this feedback contribution. It does not - // apply a final clamp to gravity, friction, stiction, or command_torque. - JointVector limited_feedback_nm{}; - JointVector command_torque_nm{}; -}; - -// Pure model inputs/outputs shared by the legacy controller and its -// Pinocchio/MJCF model adapter. -struct BimanualModelState { - // Joint arrays follow the frozen RJ1..RJ8 / LJ1..LJ8 MJCF order. - JointVector right_position_rad{}; - JointVector right_velocity_rad_s{}; - JointVector left_position_rad{}; - JointVector left_velocity_rad_s{}; - - // Homogeneous rigid transform from the IMU frame to the gravity/world - // frame. The adapter rejects non-finite and non-rigid matrices. - Transform4x4RowMajor world_from_imu{}; -}; - -struct RawHapticFeedback { - // Moments use the LOCAL_WORLD_ALIGNED frame expected by the original - // Pinocchio J_rot^T mapping. - Vector3 shoulder_moment{}; - double elbow_effort{0.0}; - Vector3 wrist_moment{}; - double gripper_effort{0.0}; -}; - -} // namespace cmvr::ume_legacy - -#endif // CMVR_ES_UME_LEGACY_TYPES_H diff --git a/cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp b/cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp deleted file mode 100644 index f298ef3c..00000000 --- a/cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp +++ /dev/null @@ -1,585 +0,0 @@ -#include "pinocchio_ume_legacy_model_adapter.h" - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::ume_legacy { -namespace { - -using ExpectedArmJointNames = std::array; - -const ExpectedArmJointNames& expectedArmJointNames() -{ - static const ExpectedArmJointNames names{ - "RJ1", "RJ2", "RJ3", "RJ4", - "RJ5", "RJ6", "RJ7", "RJ8", - "LJ1", "LJ2", "LJ3", "LJ4", - "LJ5", "LJ6", "LJ7", "LJ8"}; - return names; -} - -std::runtime_error contractError( - const std::string& model_kind, - const std::string& detail) -{ - return std::runtime_error( - "UME " + model_kind + " MJCF contract violation: " + detail); -} - -void requireDimensions( - const pinocchio::Model& model, - const std::string& model_kind, - int nq, - int nv, - pinocchio::JointIndex njoints) -{ - if (model.nq != nq || - model.nv != nv || - model.njoints != njoints) { - std::ostringstream detail; - detail << "expected nq/nv/njoints " - << nq << "/" << nv << "/" << njoints - << ", got " << model.nq << "/" << model.nv - << "/" << model.njoints; - throw contractError(model_kind, detail.str()); - } -} - -void requireJoint( - const pinocchio::Model& model, - const std::string& model_kind, - pinocchio::JointIndex joint_index, - const std::string& expected_name, - int expected_idx_q, - int expected_nq, - int expected_idx_v, - int expected_nv) -{ - if (joint_index >= model.njoints) { - throw contractError( - model_kind, - "missing joint " + expected_name); - } - - if (model.names[joint_index] != expected_name || - model.idx_qs[joint_index] != expected_idx_q || - model.nqs[joint_index] != expected_nq || - model.idx_vs[joint_index] != expected_idx_v || - model.nvs[joint_index] != expected_nv) { - std::ostringstream detail; - detail << "joint[" << joint_index << "] expected " - << expected_name << " q(" << expected_idx_q - << "," << expected_nq << ") v(" << expected_idx_v - << "," << expected_nv << "), got " - << model.names[joint_index] << " q(" - << model.idx_qs[joint_index] << "," - << model.nqs[joint_index] << ") v(" - << model.idx_vs[joint_index] << "," - << model.nvs[joint_index] << ")"; - throw contractError(model_kind, detail.str()); - } -} - -pinocchio::FrameIndex requireUniqueFrame( - const pinocchio::Model& model, - const std::string& model_kind, - const std::string& frame_name, - const std::string& expected_parent_joint_name) -{ - pinocchio::FrameIndex found = model.nframes; - std::size_t count = 0; - for (pinocchio::FrameIndex index = 0; - index < model.nframes; - ++index) { - if (model.frames[index].name == frame_name) { - found = index; - ++count; - } - } - if (count != 1) { - std::ostringstream detail; - detail << "expected exactly one frame " << frame_name - << ", got " << count; - throw contractError(model_kind, detail.str()); - } - - const auto parent_joint = model.frames[found].parentJoint; - if (parent_joint >= model.njoints || - model.names[parent_joint] != expected_parent_joint_name) { - std::ostringstream detail; - detail << "frame " << frame_name - << " expected parent joint " - << expected_parent_joint_name; - if (parent_joint < model.njoints) { - detail << ", got " << model.names[parent_joint]; - } else { - detail << ", got invalid index " << parent_joint; - } - throw contractError(model_kind, detail.str()); - } - return found; -} - -void validateFixedModel( - const pinocchio::Model& model, - std::array& frame_ids) -{ - requireDimensions(model, "fixed", 16, 16, 17); - const auto& names = expectedArmJointNames(); - for (std::size_t index = 0; index < names.size(); ++index) { - requireJoint( - model, - "fixed", - static_cast(index + 1), - names[index], - static_cast(index), - 1, - static_cast(index), - 1); - } - - frame_ids[0] = - requireUniqueFrame(model, "fixed", "R_shoulder", "RJ3"); - frame_ids[1] = - requireUniqueFrame(model, "fixed", "R_wrist", "RJ7"); - frame_ids[2] = - requireUniqueFrame(model, "fixed", "L_shoulder", "LJ3"); - frame_ids[3] = - requireUniqueFrame(model, "fixed", "L_wrist", "LJ7"); -} - -pinocchio::FrameIndex validateFloatingModel( - const pinocchio::Model& model) -{ - requireDimensions(model, "floating", 23, 22, 18); - requireJoint( - model, - "floating", - 1, - "dm_j4340_2ec_freejoint", - 0, - 7, - 0, - 6); - - const auto& names = expectedArmJointNames(); - for (std::size_t index = 0; index < names.size(); ++index) { - requireJoint( - model, - "floating", - static_cast(index + 2), - names[index], - static_cast(index + 7), - 1, - static_cast(index + 6), - 1); - } - - requireUniqueFrame( - model, "floating", "R_shoulder", "RJ3"); - requireUniqueFrame( - model, "floating", "R_wrist", "RJ7"); - requireUniqueFrame( - model, "floating", "L_shoulder", "LJ3"); - requireUniqueFrame( - model, "floating", "L_wrist", "LJ7"); - return requireUniqueFrame( - model, - "floating", - "imu", - "dm_j4340_2ec_freejoint"); -} - -bool finite(const JointVector& values) noexcept -{ - for (const double value : values) { - if (!std::isfinite(value)) { - return false; - } - } - return true; -} - -bool finite(const Vector3& values) noexcept -{ - for (const double value : values) { - if (!std::isfinite(value)) { - return false; - } - } - return true; -} - -bool toIsometry( - const Transform4x4RowMajor& source, - Eigen::Isometry3d& destination) noexcept -{ - Eigen::Matrix4d matrix; - for (Eigen::Index row = 0; row < 4; ++row) { - for (Eigen::Index column = 0; column < 4; ++column) { - matrix(row, column) = - source[static_cast(row * 4 + column)]; - } - } - if (!matrix.allFinite()) { - return false; - } - - constexpr double kTransformTolerance = 1e-6; - if (std::abs(matrix(3, 0)) > kTransformTolerance || - std::abs(matrix(3, 1)) > kTransformTolerance || - std::abs(matrix(3, 2)) > kTransformTolerance || - std::abs(matrix(3, 3) - 1.0) > kTransformTolerance) { - return false; - } - - const Eigen::Matrix3d rotation = - matrix.template block<3, 3>(0, 0); - if (!(rotation.transpose() * rotation) - .isApprox(Eigen::Matrix3d::Identity(), - kTransformTolerance) || - std::abs(rotation.determinant() - 1.0) > - kTransformTolerance) { - return false; - } - - destination = Eigen::Isometry3d::Identity(); - destination.linear() = rotation; - destination.translation() = - matrix.template block<3, 1>(0, 3); - return true; -} - -Transform4x4RowMajor toRowMajor( - const Eigen::Matrix4d& matrix) noexcept -{ - Transform4x4RowMajor result{}; - for (Eigen::Index row = 0; row < 4; ++row) { - for (Eigen::Index column = 0; column < 4; ++column) { - result[static_cast(row * 4 + column)] = - matrix(row, column); - } - } - return result; -} - -Eigen::Vector3d toEigen(const Vector3& value) noexcept -{ - return {value[0], value[1], value[2]}; -} - -Vector3 fromEigen(const Eigen::Vector3d& value) noexcept -{ - return {value.x(), value.y(), value.z()}; -} - -} // namespace - -class PinocchioUmeLegacyModelAdapter::Impl { -public: - Impl(std::string fixed_path, std::string floating_path) - : fixed_model_path(std::move(fixed_path)), - floating_model_path(std::move(floating_path)) - { - try { - pinocchio::mjcf::buildModel( - fixed_model_path, fixed_model, false); - } catch (const std::exception& error) { - throw std::runtime_error( - "Failed to load fixed UME MJCF '" + - fixed_model_path + "': " + error.what()); - } - try { - pinocchio::mjcf::buildModel( - floating_model_path, floating_model, false); - } catch (const std::exception& error) { - throw std::runtime_error( - "Failed to load floating UME MJCF '" + - floating_model_path + "': " + error.what()); - } - - validateFixedModel(fixed_model, fixed_frame_ids); - const auto imu_frame_id = - validateFloatingModel(floating_model); - - fixed_data = - std::make_unique(fixed_model); - floating_data = - std::make_unique(floating_model); - fixed_q = - Eigen::VectorXd::Zero(fixed_model.nq); - floating_q = - Eigen::VectorXd::Zero(floating_model.nq); - floating_velocity = - Eigen::VectorXd::Zero(floating_model.nv); - floating_acceleration = - Eigen::VectorXd::Zero(floating_model.nv); - shoulder_jacobian = - Eigen::Matrix::Zero( - 6, fixed_model.nv); - wrist_jacobian = - Eigen::Matrix::Zero( - 6, fixed_model.nv); - - Eigen::VectorXd neutral = - pinocchio::neutral(floating_model); - pinocchio::framesForwardKinematics( - floating_model, *floating_data, neutral); - base_from_imu = - floating_data->oMf[imu_frame_id]; - - const Eigen::Matrix4d base_from_imu_matrix = - base_from_imu.toHomogeneousMatrix(); - if (!base_from_imu_matrix.allFinite()) { - throw contractError( - "floating", "non-finite base_from_imu transform"); - } - - contract_info.fixed_nq = - static_cast(fixed_model.nq); - contract_info.fixed_nv = - static_cast(fixed_model.nv); - contract_info.floating_nq = - static_cast(floating_model.nq); - contract_info.floating_nv = - static_cast(floating_model.nv); - contract_info.base_from_imu = - toRowMajor(base_from_imu_matrix); - } - - std::string fixed_model_path; - std::string floating_model_path; - pinocchio::Model fixed_model; - pinocchio::Model floating_model; - std::unique_ptr fixed_data; - std::unique_ptr floating_data; - std::array fixed_frame_ids{}; - pinocchio::SE3 base_from_imu{pinocchio::SE3::Identity()}; - UmeLegacyModelContract contract_info; - - // Reused by the single controller thread. This keeps the 2 kHz legacy - // model path free of avoidable Eigen heap allocation after construction. - Eigen::VectorXd fixed_q; - Eigen::VectorXd floating_q; - Eigen::VectorXd floating_velocity; - Eigen::VectorXd floating_acceleration; - Eigen::Matrix shoulder_jacobian; - Eigen::Matrix wrist_jacobian; -}; - -PinocchioUmeLegacyModelAdapter::PinocchioUmeLegacyModelAdapter( - std::string fixed_model_path, - std::string floating_model_path) - : impl_(std::make_unique( - std::move(fixed_model_path), - std::move(floating_model_path))) -{ -} - -PinocchioUmeLegacyModelAdapter:: - ~PinocchioUmeLegacyModelAdapter() = default; - -PinocchioUmeLegacyModelAdapter::PinocchioUmeLegacyModelAdapter( - PinocchioUmeLegacyModelAdapter&&) noexcept = default; - -PinocchioUmeLegacyModelAdapter& -PinocchioUmeLegacyModelAdapter::operator=( - PinocchioUmeLegacyModelAdapter&&) noexcept = default; - -bool PinocchioUmeLegacyModelAdapter::computeGravityCompensation( - const BimanualModelState& state, - JointVector& right_gravity_nm, - JointVector& left_gravity_nm) const -{ - right_gravity_nm = {}; - left_gravity_nm = {}; - - if (!impl_ || - !finite(state.right_position_rad) || - !finite(state.right_velocity_rad_s) || - !finite(state.left_position_rad) || - !finite(state.left_velocity_rad_s)) { - return false; - } - - Eigen::Isometry3d world_from_imu; - if (!toIsometry(state.world_from_imu, world_from_imu)) { - return false; - } - - Eigen::Isometry3d base_from_imu = - Eigen::Isometry3d::Identity(); - base_from_imu.linear() = - impl_->base_from_imu.rotation(); - base_from_imu.translation() = - impl_->base_from_imu.translation(); - const Eigen::Isometry3d world_from_base = - world_from_imu * base_from_imu.inverse(); - - Eigen::Quaterniond world_q_base( - world_from_base.rotation()); - if (!world_q_base.coeffs().allFinite() || - world_q_base.norm() <= 0.0) { - return false; - } - world_q_base.normalize(); - - auto& q = impl_->floating_q; - auto& velocity = impl_->floating_velocity; - auto& acceleration = impl_->floating_acceleration; - q.setZero(); - velocity.setZero(); - acceleration.setZero(); - - q.segment<3>(0) = world_from_base.translation(); - q.segment<4>(3) = world_q_base.coeffs(); - for (std::size_t index = 0; index < kArmDof; ++index) { - q[static_cast(7 + index)] = - state.right_position_rad[index]; - q[static_cast(15 + index)] = - state.left_position_rad[index]; - velocity[static_cast(6 + index)] = - state.right_velocity_rad_s[index]; - velocity[static_cast(14 + index)] = - state.left_velocity_rad_s[index]; - } - - const auto& torque = pinocchio::rnea( - impl_->floating_model, - *impl_->floating_data, - q, - velocity, - acceleration); - if (!torque.allFinite() || torque.size() != 22) { - return false; - } - - for (std::size_t index = 0; index < kArmDof; ++index) { - right_gravity_nm[index] = - torque[static_cast(6 + index)]; - left_gravity_nm[index] = - torque[static_cast(14 + index)]; - } - return true; -} - -bool PinocchioUmeLegacyModelAdapter::projectHapticFeedback( - const BimanualModelState& state, - ArmSide side, - const RawHapticFeedback& feedback, - ProjectedHapticEffort& projected) const -{ - projected = {}; - if (!impl_ || - (side != ArmSide::Right && - side != ArmSide::Left) || - !finite(state.right_position_rad) || - !finite(state.left_position_rad) || - !finite(feedback.shoulder_moment) || - !finite(feedback.wrist_moment) || - !std::isfinite(feedback.elbow_effort) || - !std::isfinite(feedback.gripper_effort)) { - return false; - } - - auto& q = impl_->fixed_q; - q.setZero(); - for (std::size_t index = 0; index < kArmDof; ++index) { - q[static_cast(index)] = - state.right_position_rad[index]; - q[static_cast(8 + index)] = - state.left_position_rad[index]; - } - - pinocchio::framesForwardKinematics( - impl_->fixed_model, *impl_->fixed_data, q); - - const std::size_t frame_offset = - side == ArmSide::Right ? 0 : 2; - const Eigen::Index shoulder_column = - side == ArmSide::Right ? 0 : 8; - const Eigen::Index wrist_column = - side == ArmSide::Right ? 4 : 12; - - auto& shoulder_jacobian = impl_->shoulder_jacobian; - shoulder_jacobian.setZero(); - pinocchio::computeFrameJacobian( - impl_->fixed_model, - *impl_->fixed_data, - q, - impl_->fixed_frame_ids[frame_offset], - pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, - shoulder_jacobian); - - auto& wrist_jacobian = impl_->wrist_jacobian; - wrist_jacobian.setZero(); - pinocchio::computeFrameJacobian( - impl_->fixed_model, - *impl_->fixed_data, - q, - impl_->fixed_frame_ids[frame_offset + 1], - pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, - wrist_jacobian); - - if (!shoulder_jacobian.allFinite() || - !wrist_jacobian.allFinite()) { - return false; - } - - const Eigen::Vector3d shoulder_torque = - shoulder_jacobian - .block<3, 3>(3, shoulder_column) - .transpose() * - toEigen(feedback.shoulder_moment); - const Eigen::Vector3d wrist_torque = - wrist_jacobian - .block<3, 3>(3, wrist_column) - .transpose() * - toEigen(feedback.wrist_moment); - if (!shoulder_torque.allFinite() || - !wrist_torque.allFinite()) { - return false; - } - - projected.shoulder_joint_torque = - fromEigen(shoulder_torque); - projected.elbow_effort = feedback.elbow_effort; - projected.wrist_joint_torque = - fromEigen(wrist_torque); - projected.gripper_effort = feedback.gripper_effort; - return true; -} - -const UmeLegacyModelContract& -PinocchioUmeLegacyModelAdapter::contract() const noexcept -{ - return impl_->contract_info; -} - -const std::string& -PinocchioUmeLegacyModelAdapter::fixedModelPath() const noexcept -{ - return impl_->fixed_model_path; -} - -const std::string& -PinocchioUmeLegacyModelAdapter::floatingModelPath() const noexcept -{ - return impl_->floating_model_path; -} - -} // namespace cmvr::ume_legacy diff --git a/cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp b/cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp deleted file mode 100644 index f3192328..00000000 --- a/cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp +++ /dev/null @@ -1,193 +0,0 @@ -#include "ume_legacy_controller.h" - -#include - -namespace cmvr::ume_legacy { -namespace { - -constexpr double kPi = - 3.141592653589793238462643383279502884; - -double legacyClip(double value, double minimum, double maximum) noexcept -{ - // Explicit comparisons preserve NaN propagation: both comparisons are - // false and value is returned, matching np.clip for a NaN input. - if (value < minimum) { - return minimum; - } - if (value > maximum) { - return maximum; - } - return value; -} - -double norm(const Vector3& value) noexcept -{ - return std::sqrt(value[0] * value[0] + - value[1] * value[1] + - value[2] * value[2]); -} - -JointVector flattenInteraction( - ArmSide side, - const ProjectedHapticEffort& projected) noexcept -{ - JointVector interaction{ - projected.shoulder_joint_torque[0], - projected.shoulder_joint_torque[1], - projected.shoulder_joint_torque[2], - projected.elbow_effort, - projected.wrist_joint_torque[0], - projected.wrist_joint_torque[1], - projected.wrist_joint_torque[2], - projected.gripper_effort}; - - // Exact scalar sign conventions from the legacy controller: - // right elbow +, right gripper - - // left elbow -, left gripper + - if (side == ArmSide::Right) { - interaction[7] = -interaction[7]; - } else { - interaction[3] = -interaction[3]; - } - return interaction; -} - -} // namespace - -LegacyUmeTuning originalTuning() noexcept -{ - LegacyUmeTuning tuning; - tuning.friction_coefficient = - {1.6, 1.6, 1.6, 1.6, 0.032, 0.032, 0.032, 0.032}; - tuning.friction_max_compensation = - {0.4, 0.4, 0.4, 0.4, 0.1, 0.1, 0.1, 0.1}; - - const double one_degree = kPi / 180.0; - const double ten_degrees = 10.0 * one_degree; - tuning.stiction_threshold_min_rad_s = - {one_degree, one_degree, one_degree, one_degree, - one_degree, one_degree, one_degree, one_degree}; - tuning.stiction_threshold_max_rad_s = - {ten_degrees, ten_degrees, ten_degrees, ten_degrees, - ten_degrees, ten_degrees, ten_degrees, ten_degrees}; - tuning.stiction_compensation = - {0.5, 0.5, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0}; - - tuning.feedback_error_tolerance_rad = one_degree; - tuning.feedback_tanh_sharpness = 10.0; - tuning.feedback_scale = 0.5; - tuning.feedback_limit_dm4340_nm = 4.0; - tuning.feedback_limit_dm4310_nm = 1.0; - return tuning; -} - -JointVector frictionCompensation( - const JointVector& velocity_rad_s, - const LegacyUmeTuning& tuning) noexcept -{ - JointVector result{}; - for (std::size_t index = 0; index < kArmDof; ++index) { - const double maximum = - tuning.friction_max_compensation[index]; - result[index] = legacyClip( - tuning.friction_coefficient[index] * - velocity_rad_s[index], - -maximum, - maximum); - } - return result; -} - -JointVector stictionCompensation( - const JointVector& velocity_rad_s, - const LegacyUmeTuning& tuning) noexcept -{ - JointVector result{}; - for (std::size_t index = 0; index < kArmDof; ++index) { - const double velocity = velocity_rad_s[index]; - const double speed = std::abs(velocity); - - // Both inequalities are intentionally strict, matching: - // min < abs(qvel) < max. - if (tuning.stiction_threshold_min_rad_s[index] < speed && - speed < tuning.stiction_threshold_max_rad_s[index]) { - if (velocity > 0.0) { - result[index] = - tuning.stiction_compensation[index]; - } else if (velocity < 0.0) { - result[index] = - -tuning.stiction_compensation[index]; - } - } - } - return result; -} - -double feedbackScale( - double error_norm, - const LegacyUmeTuning& tuning) noexcept -{ - return tuning.feedback_scale * - (std::tanh( - tuning.feedback_tanh_sharpness * - (std::abs(error_norm) - - tuning.feedback_error_tolerance_rad)) + - 1.0) / - 2.0; -} - -SideControlOutput computeSideCommand( - const SideControlInput& input, - const LegacyUmeTuning& tuning) noexcept -{ - SideControlOutput output; - output.friction_compensation_nm = - frictionCompensation(input.joint_velocity_rad_s, tuning); - output.stiction_compensation_nm = - stictionCompensation(input.joint_velocity_rad_s, tuning); - output.signed_interaction_nm = - flattenInteraction(input.side, input.projected_haptic); - - output.feedback_scales.shoulder = - feedbackScale(norm(input.tracking_error.shoulder_rotation), - tuning); - output.feedback_scales.elbow = - feedbackScale(std::abs(input.tracking_error.elbow), tuning); - output.feedback_scales.wrist = - feedbackScale(norm(input.tracking_error.wrist_rotation), - tuning); - output.feedback_scales.gripper = - feedbackScale(std::abs(input.tracking_error.gripper), tuning); - - for (std::size_t index = 0; index < kArmDof; ++index) { - output.feedforward_without_haptic_nm[index] = - input.gravity_compensation_nm[index] + - output.friction_compensation_nm[index] + - output.stiction_compensation_nm[index]; - - double scale = output.feedback_scales.gripper; - double limit = tuning.feedback_limit_dm4310_nm; - if (index < 3) { - scale = output.feedback_scales.shoulder; - limit = tuning.feedback_limit_dm4340_nm; - } else if (index == 3) { - scale = output.feedback_scales.elbow; - limit = tuning.feedback_limit_dm4340_nm; - } else if (index < 7) { - scale = output.feedback_scales.wrist; - } - - output.limited_feedback_nm[index] = legacyClip( - scale * output.signed_interaction_nm[index], - -limit, - limit); - output.command_torque_nm[index] = - output.feedforward_without_haptic_nm[index] - - output.limited_feedback_nm[index]; - } - - return output; -} - -} // namespace cmvr::ume_legacy diff --git a/cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp b/cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp deleted file mode 100644 index 568ff28e..00000000 --- a/cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp +++ /dev/null @@ -1,392 +0,0 @@ -#include "pinocchio_ume_legacy_model_adapter.h" - -#include -#include -#include -#include -#include -#include - -#include - -#ifndef CMVR_UME_FIXED_MODEL_PATH -#error "CMVR_UME_FIXED_MODEL_PATH must identify the deployed fixed UME MJCF" -#endif - -#ifndef CMVR_UME_FLOATING_MODEL_PATH -#error "CMVR_UME_FLOATING_MODEL_PATH must identify the deployed floating UME MJCF" -#endif - -namespace cmvr::ume_legacy { -namespace { - -constexpr double kNumericalTolerance = 1e-10; - -PinocchioUmeLegacyModelAdapter makeAdapter() -{ - return PinocchioUmeLegacyModelAdapter( - CMVR_UME_FIXED_MODEL_PATH, - CMVR_UME_FLOATING_MODEL_PATH); -} - -BimanualModelState makeGoldenState( - const PinocchioUmeLegacyModelAdapter& adapter) -{ - BimanualModelState state; - state.world_from_imu = - adapter.contract().base_from_imu; - state.right_position_rad = - {0.1, -0.2, 0.3, -0.4, - 0.2, -0.1, 0.15, -0.05}; - state.left_position_rad = - {-0.1, 0.2, -0.3, 0.4, - -0.2, 0.1, -0.15, 0.05}; - return state; -} - -template -void expectFinite(const std::array& values) -{ - for (std::size_t index = 0; index < Size; ++index) { - EXPECT_TRUE(std::isfinite(values[index])) - << "index " << index; - } -} - -template -void expectNear( - const std::array& actual, - const std::array& expected, - double tolerance = kNumericalTolerance) -{ - for (std::size_t index = 0; index < Size; ++index) { - EXPECT_NEAR(actual[index], expected[index], tolerance) - << "index " << index; - } -} - -Transform4x4RowMajor multiplyTransforms( - const Transform4x4RowMajor& left, - const Transform4x4RowMajor& right) -{ - Transform4x4RowMajor result{}; - for (std::size_t row = 0; row < 4; ++row) { - for (std::size_t column = 0; column < 4; ++column) { - for (std::size_t inner = 0; inner < 4; ++inner) { - result[row * 4 + column] += - left[row * 4 + inner] * - right[inner * 4 + column]; - } - } - } - return result; -} - -Transform4x4RowMajor makeNoncommutingWorldFromBase() -{ - constexpr double roll = 0.2; - constexpr double pitch = -0.35; - constexpr double yaw = 0.47; - const double sr = std::sin(roll); - const double cr = std::cos(roll); - const double sp = std::sin(pitch); - const double cp = std::cos(pitch); - const double sy = std::sin(yaw); - const double cy = std::cos(yaw); - return { - cy * cp, - cy * sp * sr - sy * cr, - cy * sp * cr + sy * sr, - 0.4, - sy * cp, - sy * sp * sr + cy * cr, - sy * sp * cr - cy * sr, - -0.1, - -sp, - cp * sr, - cp * cr, - 0.8, - 0.0, 0.0, 0.0, 1.0}; -} - -TEST(PinocchioUmeLegacyModelAdapterTest, - LoadsOriginalMjcfWithoutGeometryAssetsAndFreezesContract) -{ - const auto adapter = makeAdapter(); - const auto& contract = adapter.contract(); - - EXPECT_EQ(contract.fixed_nq, 16U); - EXPECT_EQ(contract.fixed_nv, 16U); - EXPECT_EQ(contract.floating_nq, 23U); - EXPECT_EQ(contract.floating_nv, 22U); - EXPECT_EQ(adapter.fixedModelPath(), CMVR_UME_FIXED_MODEL_PATH); - EXPECT_EQ( - adapter.floatingModelPath(), - CMVR_UME_FLOATING_MODEL_PATH); - - // This transform comes from the original floating model's imu site. - // Pinocchio buildModel parses it without loading STL geometry. - const Transform4x4RowMajor expected_base_from_imu{ - 0.0, 0.0, -1.0, -0.0298, - 0.0, 1.0, 0.0, 0.0, - 1.0, 0.0, 0.0, -0.229564, - 0.0, 0.0, 0.0, 1.0}; - expectNear( - contract.base_from_imu, - expected_base_from_imu, - 1e-5); -} - -TEST(PinocchioUmeLegacyModelAdapterTest, - FloatingBaseRneaProducesFiniteFrozenJointEfforts) -{ - const auto adapter = makeAdapter(); - const auto state = makeGoldenState(adapter); - JointVector right{}; - JointVector left{}; - - ASSERT_TRUE( - adapter.computeGravityCompensation( - state, right, left)); - expectFinite(right); - expectFinite(left); - expectNear( - right, - {4.2579946000243867, - -3.101883528283977, - 5.8349126697703291, - -3.2335040596068012, - 0.54313804667559806, - -0.28419763993223052, - 0.075420409617272505, - -0.0050792810993999194}); - expectNear( - left, - {-4.2570142852347947, - 3.1002195124462104, - -5.8319486672094438, - 3.2334454894750602, - -0.54274278459746039, - 0.28419800074629464, - -0.075420524366616282, - 0.0050792800399334561}); -} - -TEST(PinocchioUmeLegacyModelAdapterTest, - ImuDerivedBaseOrientationReversesGravityUnderHalfTurn) -{ - const auto adapter = makeAdapter(); - auto state = makeGoldenState(adapter); - JointVector upright_right{}; - JointVector upright_left{}; - ASSERT_TRUE(adapter.computeGravityCompensation( - state, upright_right, upright_left)); - - const Transform4x4RowMajor world_from_base_half_turn_x{ - 1.0, 0.0, 0.0, 0.0, - 0.0, -1.0, 0.0, 0.0, - 0.0, 0.0, -1.0, 0.0, - 0.0, 0.0, 0.0, 1.0}; - state.world_from_imu = multiplyTransforms( - world_from_base_half_turn_x, - adapter.contract().base_from_imu); - - JointVector inverted_right{}; - JointVector inverted_left{}; - ASSERT_TRUE(adapter.computeGravityCompensation( - state, inverted_right, inverted_left)); - for (std::size_t index = 0; index < kArmDof; ++index) { - EXPECT_NEAR( - inverted_right[index], - -upright_right[index], - kNumericalTolerance) - << "right joint index " << index; - EXPECT_NEAR( - inverted_left[index], - -upright_left[index], - kNumericalTolerance) - << "left joint index " << index; - } -} - -TEST(PinocchioUmeLegacyModelAdapterTest, - NoncommutingImuPoseAndAsymmetricVelocitiesMatchFrozenRnea) -{ - const auto adapter = makeAdapter(); - auto state = makeGoldenState(adapter); - state.world_from_imu = multiplyTransforms( - makeNoncommutingWorldFromBase(), - adapter.contract().base_from_imu); - state.right_velocity_rad_s = - {0.7, -0.4, 0.2, -0.1, - 1.1, -0.8, 0.5, -0.3}; - state.left_velocity_rad_s = - {-0.6, 0.9, -0.2, 0.4, - -1.0, 0.7, -0.5, 0.25}; - - JointVector right{}; - JointVector left{}; - ASSERT_TRUE(adapter.computeGravityCompensation( - state, right, left)); - expectFinite(right); - expectFinite(left); - expectNear( - right, - {2.080988418159027, - -1.3277143244666021, - 2.3194639604892364, - -2.0699331198781317, - 0.40716561983947641, - -0.20631486209184946, - 0.19335176308712126, - -0.01319194690287678}); - expectNear( - left, - {-5.6976332776660232, - 2.9915426497221254, - -2.5700109870406949, - 2.1379083447512071, - -0.36441212045948712, - 0.19226687457105307, - -0.085601264411386338, - 0.0065129700338426369}); -} - -TEST(PinocchioUmeLegacyModelAdapterTest, - FixedModelRotationalProjectionMatchesFrozenValues) -{ - const auto adapter = makeAdapter(); - const auto state = makeGoldenState(adapter); - RawHapticFeedback feedback; - feedback.shoulder_moment = {0.5, -0.2, 0.3}; - feedback.elbow_effort = 1.2; - feedback.wrist_moment = {-0.4, 0.1, 0.6}; - feedback.gripper_effort = -0.7; - - ProjectedHapticEffort right{}; - ASSERT_TRUE(adapter.projectHapticFeedback( - state, ArmSide::Right, feedback, right)); - expectFinite(right.shoulder_joint_torque); - expectFinite(right.wrist_joint_torque); - expectNear( - right.shoulder_joint_torque, - {-0.5, - -0.12674237993402607, - -0.30915027509149773}); - expectNear( - right.wrist_joint_torque, - {-0.065872923184419674, - -0.028130723042130143, - -0.72743566625468636}); - EXPECT_DOUBLE_EQ(right.elbow_effort, feedback.elbow_effort); - EXPECT_DOUBLE_EQ( - right.gripper_effort, - feedback.gripper_effort); - - ProjectedHapticEffort left{}; - ASSERT_TRUE(adapter.projectHapticFeedback( - state, ArmSide::Left, feedback, left)); - expectFinite(left.shoulder_joint_torque); - expectFinite(left.wrist_joint_torque); - expectNear( - left.shoulder_joint_torque, - {-0.5, - -0.36032671657345572, - 0.083245914753940151}); - expectNear( - left.wrist_joint_torque, - {-0.13497315246130309, - 0.15764759010789803, - -0.70779204949804098}); - EXPECT_DOUBLE_EQ(left.elbow_effort, feedback.elbow_effort); - EXPECT_DOUBLE_EQ( - left.gripper_effort, - feedback.gripper_effort); -} - -TEST(PinocchioUmeLegacyModelAdapterTest, - RejectsNonRigidOrNonFiniteInputsAndZerosOutputs) -{ - const auto adapter = makeAdapter(); - auto state = makeGoldenState(adapter); - JointVector right; - JointVector left; - right.fill(1.0); - left.fill(1.0); - - state.world_from_imu = {}; - EXPECT_FALSE(adapter.computeGravityCompensation( - state, right, left)); - expectNear(right, JointVector{}); - expectNear(left, JointVector{}); - - state = makeGoldenState(adapter); - state.right_position_rad[3] = - std::numeric_limits::quiet_NaN(); - right.fill(1.0); - left.fill(1.0); - EXPECT_FALSE(adapter.computeGravityCompensation( - state, right, left)); - expectNear(right, JointVector{}); - expectNear(left, JointVector{}); - - state = makeGoldenState(adapter); - RawHapticFeedback feedback; - feedback.shoulder_moment[1] = - std::numeric_limits::infinity(); - ProjectedHapticEffort projected; - projected.elbow_effort = 1.0; - EXPECT_FALSE(adapter.projectHapticFeedback( - state, ArmSide::Right, feedback, projected)); - expectNear( - projected.shoulder_joint_torque, - Vector3{}); - expectNear(projected.wrist_joint_torque, Vector3{}); - EXPECT_DOUBLE_EQ(projected.elbow_effort, 0.0); - EXPECT_DOUBLE_EQ(projected.gripper_effort, 0.0); - - feedback = {}; - projected.elbow_effort = 1.0; - EXPECT_FALSE(adapter.projectHapticFeedback( - state, - static_cast(99), - feedback, - projected)); - expectNear( - projected.shoulder_joint_torque, - Vector3{}); - expectNear(projected.wrist_joint_torque, Vector3{}); - EXPECT_DOUBLE_EQ(projected.elbow_effort, 0.0); - EXPECT_DOUBLE_EQ(projected.gripper_effort, 0.0); -} - -TEST(PinocchioUmeLegacyModelAdapterTest, - MissingModelFailsAtConstruction) -{ - EXPECT_THROW( - PinocchioUmeLegacyModelAdapter( - "/definitely/missing/ume_fixed.xml", - CMVR_UME_FLOATING_MODEL_PATH), - std::runtime_error); -} - -TEST(PinocchioUmeLegacyModelAdapterTest, - RejectsModelRoleSwapEvenThoughBothMjcfFilesParse) -{ - try { - PinocchioUmeLegacyModelAdapter adapter( - CMVR_UME_FLOATING_MODEL_PATH, - CMVR_UME_FIXED_MODEL_PATH); - (void)adapter; - FAIL() << "swapped fixed/floating models were accepted"; - } catch (const std::runtime_error& error) { - EXPECT_NE( - std::string(error.what()).find( - "UME fixed MJCF contract violation: " - "expected nq/nv/njoints 16/16/17"), - std::string::npos); - } -} - -} // namespace -} // namespace cmvr::ume_legacy diff --git a/cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp b/cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp deleted file mode 100644 index b5610c20..00000000 --- a/cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp +++ /dev/null @@ -1,216 +0,0 @@ -#include "ume_legacy_controller.h" -#include "ume_legacy_model_adapter.h" - -#include -#include -#include -#include - -#include - -namespace cmvr::ume_legacy { -namespace { - -constexpr double kTolerance = 1e-12; - -void expectJointVectorNear( - const JointVector& actual, - const JointVector& expected, - double tolerance = kTolerance) -{ - for (std::size_t index = 0; index < kArmDof; ++index) { - EXPECT_NEAR(actual[index], expected[index], tolerance) - << "joint index " << index; - } -} - -TEST(UmeLegacyControllerGoldenTest, - OriginalTuningAndFrictionMatchPythonOracle) -{ - const auto tuning = originalTuning(); - EXPECT_DOUBLE_EQ(tuning.friction_coefficient[0], 1.6); - EXPECT_DOUBLE_EQ(tuning.friction_coefficient[4], 0.032); - EXPECT_DOUBLE_EQ(tuning.feedback_limit_dm4340_nm, 4.0); - EXPECT_DOUBLE_EQ(tuning.feedback_limit_dm4310_nm, 1.0); - - const JointVector velocity{ - -1.0, -0.1, 0.0, 2.0 * std::acos(-1.0) / 180.0, - -10.0, -1.0, 1.0, 10.0}; - const JointVector expected{ - -0.4, -0.16000000000000003, 0.0, - 0.055850536063818547, - -0.1, -0.032, 0.032, 0.1}; - - expectJointVectorNear( - frictionCompensation(velocity, tuning), - expected); -} - -TEST(UmeLegacyControllerGoldenTest, - StictionUsesStrictLegacyThresholds) -{ - const auto tuning = originalTuning(); - const double minimum = - tuning.stiction_threshold_min_rad_s[0]; - const double maximum = - tuning.stiction_threshold_max_rad_s[0]; - const JointVector at_threshold{ - minimum, - -minimum, - maximum, - -maximum, - 0.0, 0.0, 0.0, 0.0}; - expectJointVectorNear( - stictionCompensation(at_threshold, tuning), - JointVector{}); - - const JointVector strictly_inside{ - 2.0 * minimum, - -2.0 * minimum, - std::nextafter( - minimum, std::numeric_limits::infinity()), - std::nextafter(maximum, 0.0), - 2.0 * minimum, - -2.0 * minimum, - std::nextafter( - minimum, std::numeric_limits::infinity()), - std::nextafter(maximum, 0.0)}; - expectJointVectorNear( - stictionCompensation(strictly_inside, tuning), - {0.5, -0.5, 0.5, 0.5, - 0.0, 0.0, 0.0, 0.0}); -} - -TEST(UmeLegacyControllerGoldenTest, - FeedbackScaleMatchesLegacyNormAndTanhGoldenValues) -{ - const auto tuning = originalTuning(); - EXPECT_NEAR( - feedbackScale(0.0, tuning), - 0.20680448412090907, - kTolerance); - EXPECT_DOUBLE_EQ( - feedbackScale(tuning.feedback_error_tolerance_rad, tuning), - 0.25); - EXPECT_NEAR( - feedbackScale(0.05, tuning), - 0.32861047013244982, - kTolerance); - EXPECT_NEAR( - feedbackScale(-0.2, tuning), - 0.48734517579834447, - kTolerance); -} - -TEST(UmeLegacyControllerGoldenTest, - CompleteRightSideCommandMatchesPythonGoldenVector) -{ - SideControlInput input; - input.side = ArmSide::Right; - input.joint_velocity_rad_s = { - -1.0, -0.1, 0.0, 2.0 * std::acos(-1.0) / 180.0, - -10.0, -1.0, 1.0, 10.0}; - input.gravity_compensation_nm = - {0.5, -0.5, 1.0, -1.0, - 0.25, -0.25, 0.75, -0.75}; - input.projected_haptic.shoulder_joint_torque = - {1.2, -3.0, 10.0}; - input.projected_haptic.elbow_effort = 2.0; - input.projected_haptic.wrist_joint_torque = - {0.5, -2.0, 5.0}; - input.projected_haptic.gripper_effort = 3.0; - input.tracking_error.shoulder_rotation = {0.0, 0.0, 0.0}; - input.tracking_error.elbow = - originalTuning().feedback_error_tolerance_rad; - input.tracking_error.wrist_rotation = {0.03, 0.04, 0.0}; - input.tracking_error.gripper = -0.2; - - const auto output = computeSideCommand(input); - - expectJointVectorNear( - output.friction_compensation_nm, - {-0.4, -0.16000000000000003, 0.0, - 0.055850536063818547, - -0.1, -0.032, 0.032, 0.1}); - expectJointVectorNear( - output.stiction_compensation_nm, - {0.0, -0.5, 0.0, 0.5, - 0.0, 0.0, 0.0, 0.0}); - expectJointVectorNear( - output.feedforward_without_haptic_nm, - {0.099999999999999978, -1.1600000000000001, - 1.0, -0.44414946393618149, - 0.14999999999999999, -0.28200000000000003, - 0.78200000000000003, -0.65000000000000002}); - expectJointVectorNear( - output.signed_interaction_nm, - {1.2, -3.0, 10.0, 2.0, - 0.5, -2.0, 5.0, -3.0}); - expectJointVectorNear( - output.limited_feedback_nm, - {0.24816538094509089, -0.62041345236272716, - 2.0680448412090908, 0.5, - 0.16430523506622491, -0.65722094026489963, - 1.0, -1.0}); - expectJointVectorNear( - output.command_torque_nm, - {-0.14816538094509091, -0.53958654763727298, - -1.0680448412090908, -0.94414946393618149, - -0.014305235066224914, 0.37522094026489961, - -0.21799999999999997, 0.34999999999999998}); -} - -TEST(UmeLegacyControllerGoldenTest, - LeftAndRightScalarSignsAndGroupLimitsArePreserved) -{ - SideControlInput input; - input.projected_haptic.shoulder_joint_torque = - {100.0, -100.0, 100.0}; - input.projected_haptic.elbow_effort = 100.0; - input.projected_haptic.wrist_joint_torque = - {100.0, -100.0, 100.0}; - input.projected_haptic.gripper_effort = 100.0; - input.tracking_error.shoulder_rotation = {10.0, 0.0, 0.0}; - input.tracking_error.elbow = 10.0; - input.tracking_error.wrist_rotation = {10.0, 0.0, 0.0}; - input.tracking_error.gripper = 10.0; - - input.side = ArmSide::Right; - const auto right = computeSideCommand(input); - expectJointVectorNear( - right.signed_interaction_nm, - {100.0, -100.0, 100.0, 100.0, - 100.0, -100.0, 100.0, -100.0}); - expectJointVectorNear( - right.command_torque_nm, - {-4.0, 4.0, -4.0, -4.0, - -1.0, 1.0, -1.0, 1.0}); - - input.side = ArmSide::Left; - const auto left = computeSideCommand(input); - expectJointVectorNear( - left.signed_interaction_nm, - {100.0, -100.0, 100.0, -100.0, - 100.0, -100.0, 100.0, 100.0}); - expectJointVectorNear( - left.command_torque_nm, - {-4.0, 4.0, -4.0, 4.0, - -1.0, 1.0, -1.0, -1.0}); -} - -TEST(UmeLegacyControllerGoldenTest, - FeedbackClipDoesNotClampOtherFeedforwardTerms) -{ - SideControlInput input; - input.gravity_compensation_nm = - {50.0, -50.0, 0.0, 0.0, 0.0, 0.0, 20.0, -20.0}; - const auto output = computeSideCommand(input); - - EXPECT_DOUBLE_EQ(output.command_torque_nm[0], 50.0); - EXPECT_DOUBLE_EQ(output.command_torque_nm[1], -50.0); - EXPECT_DOUBLE_EQ(output.command_torque_nm[6], 20.0); - EXPECT_DOUBLE_EQ(output.command_torque_nm[7], -20.0); -} - -} // namespace -} // namespace cmvr::ume_legacy diff --git a/cmvr-es/common/README.md b/cmvr-es/common/README.md index a8a21010..cf60bb8d 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) 的 MediaSourceManager 章节。 +设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceHub 章节。 ## 环形队列选择 diff --git a/cmvr-es/common/base/logging/CMakeLists.txt b/cmvr-es/common/base/logging/CMakeLists.txt index 5322a2e5..c6c31e5c 100644 --- a/cmvr-es/common/base/logging/CMakeLists.txt +++ b/cmvr-es/common/base/logging/CMakeLists.txt @@ -13,26 +13,3 @@ target_link_libraries(logging PUBLIC add_library(cmvr_es::logging ALIAS logging) install(TARGETS logging ARCHIVE DESTINATION lib) - -if(BUILD_TESTING) - add_executable(logger_test - tests/logger_test.cpp - ) - target_link_libraries(logger_test PRIVATE - cmvr_es::logging - gtest - gtest_main - pthread - ) - add_test(NAME logger_test COMMAND logger_test) - set(_logger_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _logger_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(logger_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_logger_test_environment}" - ) -endif() diff --git a/cmvr-es/common/base/logging/logger.cpp b/cmvr-es/common/base/logging/logger.cpp index 594b95c1..0a6355ee 100644 --- a/cmvr-es/common/base/logging/logger.cpp +++ b/cmvr-es/common/base/logging/logger.cpp @@ -167,15 +167,6 @@ 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_); @@ -209,8 +200,7 @@ 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 d6bd17c5..a7ee3a74 100644 --- a/cmvr-es/common/base/logging/logger.h +++ b/cmvr-es/common/base/logging/logger.h @@ -43,7 +43,6 @@ 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/common/base/logging/tests/logger_test.cpp b/cmvr-es/common/base/logging/tests/logger_test.cpp deleted file mode 100644 index cc0aef19..00000000 --- a/cmvr-es/common/base/logging/tests/logger_test.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include "common/base/logging/logger.h" - -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace cmvr::logging { -namespace { - -class LoggerTest : public testing::Test { -protected: - void SetUp() override - { - std::array pattern{}; - const std::string value = "/tmp/cmvr-logger-test-XXXXXX"; - std::copy(value.begin(), value.end(), pattern.begin()); - if (char* created = ::mkdtemp(pattern.data())) { - directory_ = created; - } - ASSERT_FALSE(directory_.empty()); - } - - void TearDown() override - { - shutdownLogging(); - std::error_code error; - std::filesystem::remove_all(directory_, error); - } - - config::LoggerConfig warningFileConfig() const - { - config::LoggerConfig config; - config.set_minimum_level(config::LOG_LEVEL_DEBUG); - config.set_directory(directory_.string()); - config.set_flush_interval_seconds(3600); - auto* route = config.add_routes(); - route->set_level(config::LOG_LEVEL_WARNING); - route->set_terminal(false); - route->set_file(true); - return config; - } - - std::string fileContents() const - { - std::ifstream input(directory_ / "logger_test.log"); - return {std::istreambuf_iterator(input), - std::istreambuf_iterator()}; - } - - std::filesystem::path directory_; -}; - -TEST_F(LoggerTest, ExplicitFlushMakesWarningVisibleInFile) -{ - ASSERT_TRUE(initLogging( - warningFileConfig(), "logger_test", directory_)); - - Logger::instance().write( - Level::WARNING, __FILE__, __LINE__, "warning sentinel"); - Logger::instance().flush(); - - EXPECT_NE(fileContents().find("warning sentinel"), std::string::npos); -} - -TEST_F(LoggerTest, ErrorIsVisibleInFileImmediately) -{ - auto config = warningFileConfig(); - config.mutable_routes(0)->set_level(config::LOG_LEVEL_ERROR); - ASSERT_TRUE(initLogging(config, "logger_test", directory_)); - - Logger::instance().write( - Level::ERROR, __FILE__, __LINE__, "error sentinel"); - - EXPECT_NE(fileContents().find("error sentinel"), std::string::npos); -} - -TEST_F(LoggerTest, FlushIsSafeOutsideInitializedLifetime) -{ - Logger::instance().flush(); - ASSERT_TRUE(initLogging( - warningFileConfig(), "logger_test", directory_)); - shutdownLogging(); - Logger::instance().flush(); -} - -} // namespace -} // namespace cmvr::logging diff --git a/cmvr-es/common/types/agv/agv_types.h b/cmvr-es/common/types/agv/agv_types.h index d5e02b4d..5c7ef69c 100644 --- a/cmvr-es/common/types/agv/agv_types.h +++ b/cmvr-es/common/types/agv/agv_types.h @@ -2,7 +2,6 @@ #define CMVR_ES_AGV_TYPES_H #include -#include #include #include #include @@ -91,14 +90,6 @@ enum class AgvTaskType { Custom }; -/** - * @brief 固定距离平移使用的距离参考模式。 - */ -enum class AgvTranslationMode { - Odometry = 0, - Localization -}; - /** * @brief AGV 车体坐标系下的平面速度。 * @@ -110,20 +101,6 @@ struct AgvVelocity { double wz{0.0}; }; - - -/** - * @brief AGV 车体坐标系下的固定距离平移参数。 - */ -struct AgvTranslation { - double distance{0.0}; - double vx{0.0}; - double vy{0.0}; - AgvTranslationMode mode{AgvTranslationMode::Odometry}; -}; - - - /** * @brief 导航通用运动约束和执行选项。 * @@ -137,13 +114,7 @@ struct AgvMotionOptions { double reach_distance{0.0}; double reach_angle{0.0}; double speed_ratio{1.0}; - // 导航默认同步阻塞；调用方只有显式设为 true 才在任务接受后立即返回。 - bool asynchronous{false}; - int wait_timeout_ms{0}; - int poll_interval_ms{0}; - // 不带 RPC 框架依赖的取消检查。同步导航等待期间可由 - // 上层绑定 deadline/cancel；驱动不得在函数返回后保留该回调。 - std::function cancellation_requested; + bool asynchronous{true}; }; /** @@ -249,7 +220,7 @@ struct AgvPathSegment { /** * @brief AGV 扫图过程中产生的数据文件。 * - * content 可保存控制器返回的二进制内容，例如 SEER Robokit 的 rawmap zip 包。 + * content 可保存控制器返回的二进制内容，例如 SRC1100 的 rawmap zip 包。 */ struct AgvMappingDataFile { std::string name; diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 89f74346..4728236c 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -2,7 +2,6 @@ #define CMVR_ES_ARM_TYPES_H #include -#include #include #include @@ -104,11 +103,6 @@ struct JointGroupState { std::vector position; std::vector velocity; std::vector effort; - std::uint64_t sequence{0}; - std::int64_t sample_monotonic_ns{0}; - bool position_valid{false}; - bool velocity_valid{false}; - bool effort_valid{false}; bool validForModel(const RobotModel& model) const { @@ -171,11 +165,6 @@ struct MotionOptions { double jerk{5.0}; std::vector joint_velocity_limits; bool asynchronous{false}; - // Framework-independent cancellation check used by queued synchronous - // motion. Cancellation after device acceptance retains a typed motion - // barrier; the owner must call stopMotion() to confirm physical idle. - // Drivers must not retain this callback after moveJ/moveL returns. - std::function cancellation_requested; }; struct ServoOptions { @@ -184,14 +173,6 @@ struct ServoOptions { double gain{300.0}; }; -struct TorqueServoOptions { - // The UME legacy loop runs at 800 Hz by default. - double period{0.00125}; - // A producer must continuously refresh the latest torque command. A stale - // command latches a fault and disables the actuator chain. - std::uint32_t command_watchdog_ms{20}; -}; - enum class RobotMode { Unknown = 0, Disconnected, @@ -224,14 +205,6 @@ enum class ControlMode { Freedrive }; -enum class JointEffortSource { - Unspecified = 0, - MotorEstimate, - JointSensor, - ForceTorqueSensor, - Observer -}; - struct ArmState { double timestamp{0.0}; RobotMode robot_mode{RobotMode::Unknown}; diff --git a/cmvr-es/config/README.md b/cmvr-es/config/README.md index 2375dbdc..c1414757 100644 --- a/cmvr-es/config/README.md +++ b/cmvr-es/config/README.md @@ -29,10 +29,10 @@ cmvr_es.pb.txt /config/cmvr_es.pb.txt ``` -安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时使用 `--config`： +安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时： ```bash -./output/bin/cmvr_es --config /etc/cmvr-es/cmvr_es.pb.txt +./output/bin/cmvr_es /etc/cmvr-es/cmvr_es.pb.txt ``` 设备、任务和证书等相对配置路径均以根配置文件所在目录解析。模型等资源通过 `ConfigHelper::resolveResourceFile()` 在配置根及父目录中查找；生产部署仍建议使用明确绝对路径。 @@ -73,45 +73,9 @@ cmvr_es.pb.txt - 新增 loader 对不认识的 enum 和未设置的 oneof 必须明确失败；当前个别历史路径仍有退化默认行为，不应复制； - 设备端口、坐标系、速度和单位写入注释； - `enable` 应由 manager 层控制，后端内部的 enable 字段不能替代 manager 开关； -- QUIC 任务需要在 TaskManager 中显式开启； +- QUIC 需要 TaskManager 与 `QuicEdgeConfig.enable` 同时开启； - QUIC 零媒体轨道是合法配置。 -### QUIC 多平台 - -`QuicEdgeTask` 可以同时连接多个平台。原有顶层 -`server_host`、`server_port`、`tls` 继续表示主平台；每个 `platforms` 条目会与主平台 -并行运行。也可以不配置顶层目标，只使用一个或多个 `platforms` 条目： - -```protobuf -platforms { - id: "operations" - server_host: "192.168.0.222" - server_port: 4433 - enable_media: false - tls { - ca_file: "certs/cmvr-quic-ca.crt" - server_name: "192.168.0.222" - } -} -platforms { - id: "analytics" - server_host: "192.168.0.223" - server_port: 4433 - enable_media: true - tls { - ca_file: "certs/cmvr-quic-ca.crt" - server_name: "192.168.0.223" - } -} -``` - -平台 ID 和 `host:port` 必须分别唯一；配置兼容主平台时，其平台 ID 使用任务 -`QuicEdgeConfig.id`，新增条目也不能与它重名。每个平台拥有独立的 QUIC 连接、 -注册会话、心跳序号、ACK 超时和重连退避，一个平台断线不会阻塞其他平台。 -`enable_media` 默认为 `false`，此时仍发送注册、心跳、网络接口和设备状态，但不会 -复制音视频；设为 `true` 才会把全局 `tracks` 转发到该平台。兼容的顶层主平台保持 -原有媒体行为。 - ### gRPC 相机实时流 [`tasks/grpc_server_task/grpc_server_task.pb.txt`](tasks/grpc_server_task/grpc_server_task.pb.txt) @@ -145,49 +109,6 @@ output/bin/protoc \ 该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。 -## 双边遥操配置 - -源码仓库保持唯一根入口 [`cmvr_es.pb.txt`](cmvr_es.pb.txt)。统一的 -[`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) 已声明 -`ume_left`、`ume_right`、`ti5_motors` 和 `right_arm`；统一的 -[`manager/task_manager.pb.txt`](manager/task_manager.pb.txt) 已声明 -`ume_teleop` 和 gRPC server。角色差异不通过增加新的源码根配置文件表达，而由 -两台机器各自的外部部署配置决定。 - -UME 主端部署配置应只启用本机需要的 UME 设备和 `ume_teleop` Task： - -- `ume_left`、`ume_right` 在 DeviceManager 层默认关闭； -- [`devices/arm/ume_arms.pb.txt`](devices/arm/ume_arms.pb.txt) 内部的 - `hardware_enabled` 也默认关闭； -- 两层硬件门必须在完成 CAN 映射、限位标定和安全验收后分别启用； -- 当前 Task 只实现会话、重连、心跳和 latest-only 指令邮箱，尚无生产算法调用 - `submitSetpoint()`，返回 effort 也尚未接入本地触觉协调器。 - -机器人从端部署配置应只启用经过验收的机械臂设备以及所需的 gRPC server： - -- `ti5_motors`、`right_arm` 默认关闭； -- `ArmTeleop` 服务后端默认关闭，模型哈希必须由部署配置明确给出； -- 当前 `MotorRobotArm::servoJ()` 仍是逐关节顺序写，不满足遥操作组伺服能力门； -- 启动 gRPC server 不代表允许遥操作执行，也不能绕过设备层硬件门。 - -部署时应把完整配置树分别复制到两台机器的外部目录，并继续使用相同的标准文件名： - -```text -/etc/cmvr-es/ume/cmvr_es.pb.txt -/etc/cmvr-es/robot/cmvr_es.pb.txt -``` - -两套根配置都继续引用各自目录下同名的 -`manager/device_manager.pb.txt` 和 `manager/task_manager.pb.txt`。运行命令为： - -```bash -./output/bin/cmvr_es --config /etc/cmvr-es/ume/cmvr_es.pb.txt -./output/bin/cmvr_es --config /etc/cmvr-es/robot/cmvr_es.pb.txt -``` - -UME 主端需要填写从端地址、会话 manifest 和认证配置；机器人从端需要填写现场 -机械臂配置。不要把生产 IP、token、私钥或设备标定值提交到仓库默认配置。 - ## 生产配置 `cmake --install` 会重建 `output/bin/config/`。生产配置应复制到 `/etc/cmvr-es/` 等外部目录并显式传入。 diff --git a/cmvr-es/config/devices/agv/seer_robokit.pb.txt b/cmvr-es/config/devices/agv/src1100.pb.txt similarity index 88% rename from cmvr-es/config/devices/agv/seer_robokit.pb.txt rename to cmvr-es/config/devices/agv/src1100.pb.txt index a22209e9..833bde58 100644 --- a/cmvr-es/config/devices/agv/seer_robokit.pb.txt +++ b/cmvr-es/config/devices/agv/src1100.pb.txt @@ -8,9 +8,8 @@ agv { } agvs { - # 当前部署的控制器型号为 SRC1100；该值是设备实例 ID，不是后端类型名。 id: "src1100" - seer_robokit_agv { + src1100_agv { ip: "192.168.192.5" port_status: 19204 port_control: 19205 @@ -19,7 +18,6 @@ agv { port_other: 19210 port_push: 19301 recv_timeout_ms: 1000 - control_nick_name: "cmvr-es" enable_state_push: true state_push_interval_ms: 200 state_push_included_fields: "x" diff --git a/cmvr-es/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt index 3c62d8dd..67430cb3 100644 --- a/cmvr-es/config/devices/arm/arm.pb.txt +++ b/cmvr-es/config/devices/arm/arm.pb.txt @@ -17,10 +17,6 @@ arm { buffer_size: 50 default_vel: 1.0 default_acc: 2.0 - # Reserved only: current MotorRobotArm servoJ writes joints sequentially, - # so code rejects the teleop group-servo capability even if this is true. - # A reviewed atomic/timed group primitive is required before changing it. - enable_teleop_group_servo: false } kinematics { @@ -160,15 +156,9 @@ arm { motor { motor_system_id: "ethercat_motors" - motor_group_ids: "dual_arm_ethercat" + motor_group_ids: "right_arm_ethercat" dof: 7 joint_names: "R_SHOULDER_P" - joint_names: "R_SHOULDER_R" - joint_names: "R_SHOULDER_Y" - joint_names: "R_ELBOW_R" - joint_names: "R_WRIST_P" - joint_names: "R_WRIST_Y" - joint_names: "R_WRIST_R" upd_freq: 1000 buffer_size: 50 default_vel: 1.0 diff --git a/cmvr-es/config/devices/arm/arm_eyou_left.pb.txt b/cmvr-es/config/devices/arm/arm_eyou_left.pb.txt deleted file mode 100644 index 27fcdd50..00000000 --- a/cmvr-es/config/devices/arm/arm_eyou_left.pb.txt +++ /dev/null @@ -1,152 +0,0 @@ -arm { - robot_arms { - id: "eyou_left_arm" - - motor { - motor_system_id: "ethercat_motors" - motor_group_ids: "dual_arm_ethercat" - dof: 7 - joint_names: "L_SHOULDER_P" - joint_names: "L_SHOULDER_R" - joint_names: "L_SHOULDER_Y" - joint_names: "L_ELBOW_R" - joint_names: "L_WRIST_P" - joint_names: "L_WRIST_Y" - joint_names: "L_WRIST_R" - upd_freq: 1000 - buffer_size: 50 - default_vel: 1.0 - default_acc: 2.0 - } - - kinematics { - pinocchio_dls_ik_solver { - urdf_path: "model/xiaoyan_description/dual_arm.urdf" - base_frame_name: "PELVIS_S" - flange_frame_name: "L_WRIST_R_S" - tcp_frame_name: "L_FINGER_TIP_FIXED" - max_iters: 100 - pos_eps: 1e-6 - rot_eps: 1e-6 - damping: 1e-6 - joint_limit_policy { - limits { - enable: true - source: JOINT_LIMIT_SOURCE_CUSTOM - joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } - } - soft_limit { - enable: true - margin_ratio: 0.01 - min_margin_rad: 0.01 - } - avoidance { - enable: false - gain: 0.2 - margin_ratio: 0.15 - max_push: 0.25 - weight: 0.05 - } - } - } - } - - motion { - move_j { - toppra_joint_motion_planner { - path_type: TOPPRA_PATH_TYPE_QUINTIC - sample_period_s: 0.001 - grid_size: 150 - high_grid_size: 300 - } - } - - move_l { - pinocchio_cartesian_motion_planner { - sample_period_s: 0.001 - position_gain: 4.0 - rotation_gain: 4.0 - line_deviation_check { - enable: true - line_deviation_warn_m: 0.01 - line_deviation_stop_m: 0.03 - line_direction_warn_deg: 20.0 - line_direction_stop_deg: 45.0 - line_direction_reset_deg: 10.0 - line_check_min_distance_m: 0.01 - } - joint_continuity_check { - enable: true - max_joint_delta_rad: 0.05 - max_joint_velocity_rad_s: 10.0 - max_joint_acceleration_rad_s2: 5000.0 - } - cartesian_step_feasibility_check { - enable: true - min_linear_speed_ratio: 0.2 - max_linear_direction_deviation_deg: 45.0 - min_angular_speed_ratio: 0.2 - max_angular_direction_deviation_deg: 45.0 - min_desired_linear_speed: 1e-4 - min_desired_angular_speed: 1e-4 - } - } - } - - speed_l { - pinocchio_cartesian_motion_planner { - linear_velocity_max: 0.55 - linear_acceleration_max: 5.0 - linear_jerk_max: 10.0 - angular_velocity_max: 1.0 - angular_acceleration_max: 5.0 - angular_jerk_max: 12.0 - linear_target_replan_threshold: 1e-4 - angular_target_replan_threshold: 1e-4 - linear_reverse_cos_threshold: -0.8660254037844386 - linear_reverse_switch_speed_threshold: 1e-3 - enforce_joint_acceleration_limits: true - line_deviation_check { - enable: true - line_deviation_warn_m: 0.01 - line_deviation_stop_m: 0.03 - line_direction_warn_deg: 20.0 - line_direction_stop_deg: 45.0 - line_direction_reset_deg: 10.0 - line_check_min_distance_m: 0.01 - } - joint_velocity_check { - enable: true - max_joint_velocity_rad_s: 30.0 - max_joint_acceleration_rad_s2: 10000.0 - } - cartesian_velocity_feasibility_check { - enable: true - min_linear_speed_ratio: 0.2 - max_linear_direction_deviation_deg: 5.0 - min_angular_speed_ratio: 0.2 - max_angular_direction_deviation_deg: 5.0 - min_desired_linear_speed: 1e-4 - min_desired_angular_speed: 1e-4 - } - } - - speed_l_controller { - cartesian_velocity_controller { - control_period_s: 0.001 - stop_twist_norm: 1e-9 - stop_command_velocity_norm: 1e-3 - stop_measured_velocity_norm: 1e-2 - stop_acceleration: 10 - } - } - } - } - } -} diff --git a/cmvr-es/config/devices/arm/aubo_arm.pb.txt b/cmvr-es/config/devices/arm/aubo_arm.pb.txt index cd8f3803..551f31df 100644 --- a/cmvr-es/config/devices/arm/aubo_arm.pb.txt +++ b/cmvr-es/config/devices/arm/aubo_arm.pb.txt @@ -17,7 +17,6 @@ arm { tool_frame: "tool0" username: "aubo" password: "123456" - auto_power_on_after_hardware_estop_release: true } } } diff --git a/cmvr-es/config/devices/arm/ume_arms.pb.txt b/cmvr-es/config/devices/arm/ume_arms.pb.txt deleted file mode 100644 index e39f94f4..00000000 --- a/cmvr-es/config/devices/arm/ume_arms.pb.txt +++ /dev/null @@ -1,72 +0,0 @@ -# UME leader-arm device templates. They are deliberately disabled in -# manager/device_manager.pb.txt and hardware_enabled remains false here. -# -# Before real hardware use, independently verify interface bitrate -# (1 Mbit/s arbitration, 5 Mbit/s data, FD+BRS), motor/feedback IDs, -# direction, zero offsets, mechanical joint limits and safe torque limits. -# Each joint must also receive reviewed healthy_feedback_status and raw -# temperature thresholds. They are deliberately absent below, so changing -# hardware_enabled alone is insufficient to arm these placeholder profiles. -arm { - robot_arms { - id: "ume_right" - ume { - can { - dev_id: "can4" - channel_id: 4 - interface_name: "can4" - enable_fd: true - bitrate_switch: true - send_timeout_us: 100 - receive_timeout_us: 100 - receive_own_messages: false - enable_error_frames: true - } - control_frequency_hz: 800 - cycle_deadline_us: 1000 - feedback_watchdog_ms: 20 - shutdown_timeout_ms: 50 - hardware_enabled: false - - joints { joint_name: "RJ1" command_id: 1 feedback_id: 17 reported_motor_id: 1 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } - joints { joint_name: "RJ2" command_id: 2 feedback_id: 18 reported_motor_id: 2 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } - joints { joint_name: "RJ3" command_id: 3 feedback_id: 19 reported_motor_id: 3 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } - joints { joint_name: "RJ4" command_id: 4 feedback_id: 20 reported_motor_id: 4 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } - joints { joint_name: "RJ5" command_id: 5 feedback_id: 21 reported_motor_id: 5 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } - joints { joint_name: "RJ6" command_id: 6 feedback_id: 22 reported_motor_id: 6 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } - joints { joint_name: "RJ7" command_id: 7 feedback_id: 23 reported_motor_id: 7 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } - joints { joint_name: "RJ8" command_id: 8 feedback_id: 24 reported_motor_id: 8 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } - } - } - - robot_arms { - id: "ume_left" - ume { - can { - dev_id: "can5" - channel_id: 5 - interface_name: "can5" - enable_fd: true - bitrate_switch: true - send_timeout_us: 100 - receive_timeout_us: 100 - receive_own_messages: false - enable_error_frames: true - } - control_frequency_hz: 800 - cycle_deadline_us: 1000 - feedback_watchdog_ms: 20 - shutdown_timeout_ms: 50 - hardware_enabled: false - - joints { joint_name: "LJ1" command_id: 1 feedback_id: 17 reported_motor_id: 1 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } - joints { joint_name: "LJ2" command_id: 2 feedback_id: 18 reported_motor_id: 2 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } - joints { joint_name: "LJ3" command_id: 3 feedback_id: 19 reported_motor_id: 3 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } - joints { joint_name: "LJ4" command_id: 4 feedback_id: 20 reported_motor_id: 4 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } - joints { joint_name: "LJ5" command_id: 5 feedback_id: 21 reported_motor_id: 5 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } - joints { joint_name: "LJ6" command_id: 6 feedback_id: 22 reported_motor_id: 6 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } - joints { joint_name: "LJ7" command_id: 7 feedback_id: 23 reported_motor_id: 7 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } - joints { joint_name: "LJ8" command_id: 8 feedback_id: 24 reported_motor_id: 8 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } - } - } -} diff --git a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt index b36237cc..9fda778a 100644 --- a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt +++ b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt @@ -2,7 +2,7 @@ motor { id: "ethercat_motors" motor_groups { - id: "dual_arm_ethercat" + id: "right_arm_ethercat" bus_type: MOTOR_BUS_ETHERCAT vendor: MOTOR_VENDOR_EYOU protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402 @@ -38,20 +38,13 @@ motor { sync_monitor_period_ms: 1000 } - slaves { motor_id: 1 alias: 0 position: 1 } - slaves { motor_id: 2 alias: 0 position: 2 } - slaves { motor_id: 3 alias: 0 position: 3 } - slaves { motor_id: 4 alias: 0 position: 4 } - slaves { motor_id: 5 alias: 0 position: 5 } - slaves { motor_id: 6 alias: 0 position: 6 } - slaves { motor_id: 7 alias: 0 position: 7 } - slaves { motor_id: 8 alias: 0 position: 8 } - slaves { motor_id: 9 alias: 0 position: 9 } - slaves { motor_id: 10 alias: 0 position: 10 } - slaves { motor_id: 11 alias: 0 position: 11 } - slaves { motor_id: 12 alias: 0 position: 12 } - slaves { motor_id: 13 alias: 0 position: 13 } - slaves { motor_id: 14 alias: 0 position: 14 } + slaves { motor_id: 1 alias: 0 position: 0 } + slaves { motor_id: 2 alias: 0 position: 1 } + slaves { motor_id: 3 alias: 0 position: 2 } + slaves { motor_id: 4 alias: 0 position: 3 } + slaves { motor_id: 5 alias: 0 position: 4 } + slaves { motor_id: 6 alias: 0 position: 5 } + slaves { motor_id: 7 alias: 0 position: 6 } } joint_limits { @@ -64,13 +57,6 @@ motor { joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 } joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 } - joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } } motors { @@ -81,13 +67,6 @@ motor { motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } - motors { id: 8 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } - motors { id: 9 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } - motors { id: 10 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } - motors { id: 11 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } - motors { id: 12 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } - motors { id: 13 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } - motors { id: 14 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } } diff --git a/cmvr-es/config/logger/logger.pb.txt b/cmvr-es/config/logger/logger.pb.txt index 54182979..85f8aac0 100644 --- a/cmvr-es/config/logger/logger.pb.txt +++ b/cmvr-es/config/logger/logger.pb.txt @@ -13,17 +13,17 @@ logger { routes { level: LOG_LEVEL_WARNING terminal: true - file: true + file: false } routes { level: LOG_LEVEL_ERROR terminal: true - file: true + file: false } routes { level: LOG_LEVEL_FATAL terminal: true - file: true + file: false } directory: "../log" diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 03ed5db7..f01bc267 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -4,18 +4,6 @@ device_manager { description: "cmvr edge system version 0.1" init_all_motors_when_no_active_joints: true - # The unified safety coordinator observes all decisions while the legacy - # gates remain authoritative during staged hardware migration. - safety { - mode: SHADOW - stop_all_timeout_ms: 15000 - recovery_timeout_ms: 10000 - command_ledger_result_capacity: 4096 - command_ledger_total_id_capacity: 262144 - event_history_capacity: 2048 - fail_startup_on_missing_control_capability: false - } - devices { id: "mujoco_world" type: DEVICE_TYPE_MUJOCO_WORLD @@ -91,7 +79,7 @@ device_manager { id: "ethercat_motors" type: DEVICE_TYPE_MOTOR_SYSTEM config_file: "devices/motor/ethercat_motors.pb.txt" - enable: true + enable: false } devices { @@ -105,21 +93,14 @@ device_manager { id: "eyou_arm" type: DEVICE_TYPE_ROBOT_ARM config_file: "devices/arm/arm.pb.txt" - enable: true - } - - devices { - id: "eyou_left_arm" - type: DEVICE_TYPE_ROBOT_ARM - config_file: "devices/arm/arm_eyou_left.pb.txt" - enable: true + enable: false } devices { id: "aubo_arm" type: DEVICE_TYPE_ROBOT_ARM config_file: "devices/arm/aubo_arm.pb.txt" - enable: true + enable: false } devices { @@ -129,23 +110,6 @@ device_manager { enable: false } - devices { - id: "ume_right" - type: DEVICE_TYPE_ROBOT_ARM - config_file: "devices/arm/ume_arms.pb.txt" - # Two gates must be explicitly changed after the physical safety review: - # this entry and ume.hardware_enabled in the arm config. - enable: false - } - - devices { - id: "ume_left" - type: DEVICE_TYPE_ROBOT_ARM - config_file: "devices/arm/ume_arms.pb.txt" - # Two gates must be explicitly changed after the physical safety review. - enable: false - } - devices { id: "bio_head" type: DEVICE_TYPE_BIO_HEAD_ROBOT @@ -154,11 +118,10 @@ device_manager { } devices { - # 当前部署的控制器型号为 SRC1100；该值是设备实例 ID，不是后端类型名。 id: "src1100" type: DEVICE_TYPE_AGV - config_file: "devices/agv/seer_robokit.pb.txt" - enable: true + config_file: "devices/agv/src1100.pb.txt" + enable: false } devices { diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index 223f9873..f21dd72e 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -30,13 +30,4 @@ task_manager { # Host-development default: no QUIC Gateway or physical media devices. enable: false } - tasks { - id: "ume_teleop" - type: TASK_TYPE_UME_TELEOP - run_mode: TASK_RUN_MODE_BLOCKING_SERVICE - config_file: "tasks/ume_teleop_task/ume_teleop_task.pb.txt" - # Fail-safe default: configure the remote robot endpoint, manifest and - # deployment security policy before enabling this outbound control task. - enable: false - } } diff --git a/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt b/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt index acbe6e6b..9001afb0 100644 --- a/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt +++ b/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt @@ -5,31 +5,4 @@ grpc_server { enable_reflection: true camera_stream_max_pending_frames: 2 camera_stream_max_frame_age_ms: 250 - - # Current small-scope deployment intentionally keeps the existing clients - # certificate-free. Recovery remains unavailable over the network. - security { - transport_mode: INSECURE - authentication_mode: DISABLED - recovery_exposure: RECOVERY_DISABLED - allow_insecure_non_loopback: true - } - - # The RobotArm adapter is implemented, but remains explicitly closed until - # the device itself enables teleop group servo, real hashes are provisioned, - # and group-write timing and independent stop behavior pass hardware review. - arm_teleop_backend { - enable: false - device_id: "right_arm" - # Deliberately empty placeholders are invalid when enable=true. - model_sha256: "" - calibration_sha256: "" - base_frame: "PELVIS_S" - tool_frame: "R_FINGER_TIP_FIXED" - servo_period_s: 0.001 - max_apply_duration_us: 800 - require_powered: true - max_initial_position_step_rad: 0.02 - max_position_step_rad: 0.003 - } } diff --git a/cmvr-es/config/tasks/ume_teleop_task/ume_teleop_task.pb.txt b/cmvr-es/config/tasks/ume_teleop_task/ume_teleop_task.pb.txt deleted file mode 100644 index 00ac7137..00000000 --- a/cmvr-es/config/tasks/ume_teleop_task/ume_teleop_task.pb.txt +++ /dev/null @@ -1,35 +0,0 @@ -ume_teleop { - id: "ume_teleop" - - # Deliberately left empty. The TaskManager entry is disabled by default, and - # init fails closed if it is enabled before a robot endpoint is configured. - server_address: "" - - # M6 implements explicit insecure transport for isolated development only. - # Production deployment must add and configure channel credentials first. - allow_insecure: false - - open_session { - protocol_major: 1 - protocol_minor: 0 - client_instance_id: "ume-controller" - requested_command_rate_hz: 250 - requested_state_rate_hz: 250 - watchdog_timeout_ms: 100 - requested_lease_ms: 500 - - # Replace with the manifest exported by the CMVR-ES robot instance. - expected_robot { - robot_id: "" - position_unit: "rad" - velocity_unit: "rad/s" - effort_unit: "N*m" - } - } - - reconnect { - initial_delay_ms: 100 - maximum_delay_ms: 5000 - multiplier: 2.0 - } -} diff --git a/cmvr-es/devices/README.md b/cmvr-es/devices/README.md index f7175729..aaf43066 100644 --- a/cmvr-es/devices/README.md +++ b/cmvr-es/devices/README.md @@ -36,13 +36,13 @@ config/cmvr_es.pb.txt | 大类 | 抽象接口 | 类别工厂 | 当前可选后端 | | --- | --- | --- | --- | | Camera | [`camera/abstract_camera.h`](camera/abstract_camera.h) | [`camera/camera_factory.h`](camera/camera_factory.h) | UVC、RealSense、Hikvision | -| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SEER Robokit | -| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、[AUBO](arm/aubo_arm/README.md)、Huayan、UME | +| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SRC1100 | +| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、AUBO、Huayan | | DexHand | [`dexhand/abstract_dexhand.h`](dexhand/abstract_dexhand.h) | [`dexhand/dexhand_factory.h`](dexhand/dexhand_factory.h) | RH56DFTP、PX6AXGen3 | | Microphone | [`microphone/abstract_microphone.h`](microphone/abstract_microphone.h) | [`microphone/microphone_factory.h`](microphone/microphone_factory.h) | FFmpeg | | Speaker | [`speaker/abstract_speaker.h`](speaker/abstract_speaker.h) | [`speaker/speaker_factory.h`](speaker/speaker_factory.h) | FFmpeg | | BioHead | [`biohead/abstract_biohead.h`](biohead/abstract_biohead.h) | DeviceFactory 直接创建 | BioHeadRobot | -| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT | +| MotorSystem | `motor/motor_system/` | DeviceFactory 直接创建 | CAN/MuJoCo motor group | 代码目录存在不等于已经接入配置创建链： @@ -222,7 +222,7 @@ CameraDeviceConfig / AGVDeviceConfig / ... 的外层 id ## 摄像头与麦克风实时流 -设备实现抽象流接口后，由 [`../manager/media_source_manager/`](../manager/media_source_manager/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。 +设备实现抽象流接口后，由 [`../manager/media_source_hub/`](../manager/media_source_hub/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。 当前 Hub 轨道： @@ -312,7 +312,7 @@ adapter 检测到描述变化后创建新 descriptor，设备后端不要自行 - 满队列覆盖旧数据是实时媒体的预期行为； - `waitEncodedFrame()` 必须有有限 timeout，不能永久阻塞。 -MediaSourceManager Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。 +MediaSourceHub Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。 发布后的 `MediaFrame`、`TrackDescriptor` 和 payload 不可再修改。 @@ -334,19 +334,19 @@ MediaSourceManager Subscription 同样是单消费者对象，不同协议或客 无硬件参考测试： - [`camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp`](camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp) -- [`../manager/media_source_manager/tests/media_source_manager_test.cpp`](../manager/media_source_manager/tests/media_source_manager_test.cpp) +- [`../manager/media_source_hub/tests/media_source_hub_test.cpp`](../manager/media_source_hub/tests/media_source_hub_test.cpp) ```bash cmake -S . -B build \ -DCMVR_ARCH=x86 \ -DBUILD_TESTING=ON \ - -DCMVR_MEDIA_SOURCE_MANAGER_BUILD_TESTS=ON + -DCMVR_MEDIA_SOURCE_HUB_BUILD_TESTS=ON cmake --build build -j"$(nproc)" ctest \ --test-dir build \ - -R 'hikvision_camera_callback_test|media_source_manager_test' \ + -R 'hikvision_camera_callback_test|media_source_hub_test' \ --output-on-failure ``` diff --git a/cmvr-es/devices/agv/CMakeLists.txt b/cmvr-es/devices/agv/CMakeLists.txt index fe998a13..a0ca27e4 100644 --- a/cmvr-es/devices/agv/CMakeLists.txt +++ b/cmvr-es/devices/agv/CMakeLists.txt @@ -1,5 +1,5 @@ add_subdirectory(my_agv) -add_subdirectory(seer_robokit) +add_subdirectory(src1100) add_library(agv INTERFACE) @@ -8,7 +8,7 @@ target_include_directories(agv INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(agv INTERFACE cmvr_es::device::my_agv - cmvr_es::device::seer_robokit_agv + cmvr_es::device::src1100_agv cmvr_es::proto ) diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 8f436a21..b8ad4a61 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -15,12 +15,6 @@ namespace cmvr::device { -enum class AgvActionKind { - NavigateToPose, - NavigateToStation, - FollowPath, -}; - /** * @brief AGV/移动底盘设备抽象基类。 * @@ -34,16 +28,6 @@ public: DeviceKind kind() const noexcept override { return DeviceKind::AGV; } - /** - * @brief Whether this backend provides terminal-state and stopped-motion - * confirmation plus bounded cancellation suitable for synchronous - * Action execution. - */ - virtual bool supportsSynchronousAction(AgvActionKind) const noexcept - { - return false; - } - /** * @brief 获取 AGV 运行状态快照。 */ @@ -101,40 +85,11 @@ public: /** * @brief 发起显式站点到站点路径导航任务。 */ - virtual AgvResult followPath( - const std::vector& path) + virtual AgvResult followPath(const std::vector& path) { (void)path; return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented"); } - - - - /** - * @brief 按指定速度执行固定距离平移。 - * - * 返回成功表示控制器已经接受命令，不表示运动已经完成。 - */ - virtual AgvResult translate(const AgvTranslation& translation) - { - (void)translation; - return AgvResult::failure( - AgvErrorCode::UnsupportedCommand, - "translate not implemented"); - } - - /** - * @brief 发起显式站点到站点路径导航任务，并指定同步/异步选项。 - * - * 保留单参数虚函数以兼容已有派生类；旧实现会由本重载转发。 - */ - virtual AgvResult followPath( - const std::vector& path, - const AgvMotionOptions& options) - { - (void)options; - return followPath(path); - } /** * @brief 暂停当前导航任务，如果设备支持。 @@ -183,19 +138,6 @@ public: return setVelocity(AgvVelocity{}); } - /** - * @brief 确认 AGV 已进入可安全释放控制权的停止状态。 - * - * 该接口只在导航任务已终止且底盘速度经过连续采样确认为零后返回 - * 成功；仅收到取消、停止或零速度命令的应答不构成成功。 - */ - virtual AgvResult confirmMotionStopped() - { - return AgvResult::failure( - AgvErrorCode::UnsupportedCommand, - "confirmMotionStopped not implemented"); - } - /** * @brief 查询 AGV 可用地图名称列表。 */ diff --git a/cmvr-es/devices/agv/agv_factory.h b/cmvr-es/devices/agv/agv_factory.h index 00e46415..79b2f0a3 100644 --- a/cmvr-es/devices/agv/agv_factory.h +++ b/cmvr-es/devices/agv/agv_factory.h @@ -7,7 +7,7 @@ #include "common/base/logging/logger.h" #include "devices/agv/abstract_agv.h" #include "devices/agv/my_agv/include/my_agv.h" -#include "seer_robokit_agv.h" +#include "devices/agv/src1100/include/src1100_agv.h" namespace cmvr::device { @@ -31,15 +31,15 @@ public: backend.set_id(cfg.id()); return std::make_shared(backend); } - case config::AGVDeviceConfig::kSeerRobokitAgv: + case config::AGVDeviceConfig::kSrc1100Agv: { - if (!cfg.seer_robokit_agv().id().empty() && cfg.seer_robokit_agv().id() != cfg.id()) { + if (!cfg.src1100_agv().id().empty() && cfg.src1100_agv().id() != cfg.id()) { CMVR_LOG(ERROR) << "[AGVFactory]: AGV id does not match backend id: " << cfg.id(); return nullptr; } - auto backend = cfg.seer_robokit_agv(); + auto backend = cfg.src1100_agv(); backend.set_id(cfg.id()); - return std::make_shared(backend); + return std::make_shared(backend); } case config::AGVDeviceConfig::BACKEND_NOT_SET: diff --git a/cmvr-es/devices/agv/seer_robokit/CMakeLists.txt b/cmvr-es/devices/agv/seer_robokit/CMakeLists.txt deleted file mode 100644 index 35ce8bad..00000000 --- a/cmvr-es/devices/agv/seer_robokit/CMakeLists.txt +++ /dev/null @@ -1,57 +0,0 @@ -add_library(seer_robokit_agv SHARED - src/seer_robokit_agv.cpp - src/seer_robokit_transport.cpp - src/seer_robokit_control.cpp - src/seer_robokit_status.cpp - src/seer_robokit_navigation.cpp - src/seer_robokit_navigation_wait.cpp - src/seer_robokit_map.cpp - include/seer_robokit_agv.h - include/seer_robokit_protocol.h - include/seer_robokit_utils.h - include/seer_robokit_navigation_utils.h - include/seer_robokit_pgv_utils.h -) - -target_include_directories(seer_robokit_agv - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${PROJECT_SOURCE_DIR}/cmvr-es -) - -target_link_libraries(seer_robokit_agv - PUBLIC - cmvr_es::proto - jsoncpp -) - -add_library(cmvr_es::device::seer_robokit_agv ALIAS seer_robokit_agv) -install(TARGETS seer_robokit_agv LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - add_executable(seer_robokit_control_authority_test - tests/seer_robokit_control_authority_test.cpp - ) - target_link_libraries(seer_robokit_control_authority_test - PRIVATE - cmvr_es::device::seer_robokit_agv - gtest - gtest_main - pthread - ) - add_test( - NAME seer_robokit_control_authority_test - COMMAND seer_robokit_control_authority_test - ) - set(_seer_robokit_control_authority_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _seer_robokit_control_authority_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(seer_robokit_control_authority_test PROPERTIES - TIMEOUT 180 - ENVIRONMENT "${_seer_robokit_control_authority_test_environment}" - ) -endif() diff --git a/cmvr-es/devices/agv/seer_robokit/README.md b/cmvr-es/devices/agv/seer_robokit/README.md deleted file mode 100644 index b2ee3d00..00000000 --- a/cmvr-es/devices/agv/seer_robokit/README.md +++ /dev/null @@ -1,395 +0,0 @@ -# 仙工 SEER Robokit AGV 适配器 - -`SeerRobokitAgv` 将仙工 SEER Robokit TCP/IP API 适配为 CMVR 的通用 -`AbstractAGV`/`cmvr.api.AgvService`。厂商命令号、端口、抢占控制权、状态轮询、 -地图格式转换和错误码解析都封装在本目录内。 - -本项目现场使用的控制器型号仍是 SRC1100，所以设备实例 ID 保持为 -`src1100`；它只用于配置关联和 gRPC 路由，不再作为驱动实现名称。后端配置字段 -使用 `seer_robokit_agv`，目录、类、库和测试统一使用 `seer_robokit` / -`SeerRobokitAgv` 命名。 - -从旧版本升级时，外部部署配置必须同步使用 `seer_robokit_agv { ... }`，并把 -配置路径更新为 `devices/agv/seer_robokit.pb.txt`；设备实例 ID 保持不变。程序、 -外部配置和部署脚本需要原子升级，不能把旧字段或旧路径与新二进制混用。 - -返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。 - -## 代码与配置 - -所有驱动头文件统一放在 `include/`，实现文件统一放在 `src/`；测试源码独立放在 -`tests/`。除 `seer_robokit_agv.h` 外，其余头文件均为驱动内部实现细节。 - -- 公共类声明：[`include/seer_robokit_agv.h`](include/seer_robokit_agv.h) -- 导航轮询工具： - [`include/seer_robokit_navigation_utils.h`](include/seer_robokit_navigation_utils.h) -- PGV 参数转换： - [`include/seer_robokit_pgv_utils.h`](include/seer_robokit_pgv_utils.h) -- 协议常量：[`include/seer_robokit_protocol.h`](include/seer_robokit_protocol.h) -- 通用解析工具：[`include/seer_robokit_utils.h`](include/seer_robokit_utils.h) -- 生命周期和连接：[`src/seer_robokit_agv.cpp`](src/seer_robokit_agv.cpp) -- TCP 帧与收发：[`src/seer_robokit_transport.cpp`](src/seer_robokit_transport.cpp) -- 控制权与受控命令：[`src/seer_robokit_control.cpp`](src/seer_robokit_control.cpp) -- 状态与推送缓存：[`src/seer_robokit_status.cpp`](src/seer_robokit_status.cpp) -- 导航命令：[`src/seer_robokit_navigation.cpp`](src/seer_robokit_navigation.cpp) -- 阻塞等待与停车确认： - [`src/seer_robokit_navigation_wait.cpp`](src/seer_robokit_navigation_wait.cpp) -- 地图和建图：[`src/seer_robokit_map.cpp`](src/seer_robokit_map.cpp) -- 假控制器测试： - [`tests/seer_robokit_control_authority_test.cpp`](tests/seer_robokit_control_authority_test.cpp) -- 设备配置： - [`../../../config/devices/agv/seer_robokit.pb.txt`](../../../config/devices/agv/seer_robokit.pb.txt) -- DeviceManager 配置： - [`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt) -- gRPC API： - [`../../../../protos/cmvr/api/agv_service.proto`](../../../../protos/cmvr/api/agv_service.proto)、 - [`../../../../protos/cmvr/api/agv_command.proto`](../../../../protos/cmvr/api/agv_command.proto)、 - [`../../../../protos/cmvr/api/agv_utils.proto`](../../../../protos/cmvr/api/agv_utils.proto) - -## 配置和启动 - -现场配置至少需要修改控制器 IP；端口通常保持仙工默认值： - -```textproto -agv { - agvs { - id: "src1100" - seer_robokit_agv { - ip: "192.168.192.5" - port_status: 19204 - port_control: 19205 - port_nav: 19206 - port_config: 19207 - port_other: 19210 - port_push: 19301 - recv_timeout_ms: 1000 - control_nick_name: "cmvr-es" - enable_state_push: true - state_push_interval_ms: 200 - enable_map_update: true - map_update_interval_ms: 1000 - map_update_history_size: 8 - } - } -} -``` - -还要在 `device_manager.pb.txt` 中确认同一个设备 id，并在完成现场安全检查后把 -`enable` 改为 `true`。源码默认配置故意保持关闭。 - -```textproto -devices { - id: "src1100" - type: DEVICE_TYPE_AGV - config_file: "devices/agv/seer_robokit.pb.txt" - enable: true -} -``` - -构建、安装并启动： - -```bash -cmake -S . -B build -DCMAKE_BUILD_TYPE=Release -cmake --build build -j2 -cmake --install build -./output/bin/cmvr_es -``` - -`output/bin/cmvr_es` 默认读取 `output/bin/config/`。修改源码配置后需要重新安装， -或通过程序支持的外部配置入口启动，不能只修改源码文件后继续使用旧的 -`output/` 配置。 - -## 控制器端口和命令 - -| 端口 | 主要用途 | 当前使用的命令 | -| --- | --- | --- | -| `19204` | 状态、站点、地图和建图文件 | `1004`、`1007`、`1020`、`1101`、`1110`、`1300`、`1301`、`1780`、`1800` | -| `19205` | 底盘控制 | `2000`、`2010`、`2022` | -| `19206` | 导航任务 | `3001`、`3002`、`3003`、`3051`、`3066`、`3067` | -| `19207` | 控制权、地图上传下载 | `4005`、`4010`、`4011` | -| `19210` | 开始/停止建图 | `6100`、`6101` | -| `19301` | 机器人状态推送 | `9300`/`19300` 配置，`19301` 推送 | - -所有会改变机器人或控制器状态的调用都在 SEER Robokit 子类内部先通过 `4005` -抢权，负载为稳定的 `nick_name`，成功后才发送实际命令。普通命令集中走 -`sendControlledCommand_`；`emergencyStop` 为保证 `2000` 和导航取消之间不被 -插入其他命令，会在同一个控制序列锁内只抢一次权。只读查询不抢权。不要在 -gRPC 客户端另做一套租约逻辑。 - -## gRPC 接口概览 - -默认示例端点为 `127.0.0.1:50052`；远程部署时替换为 CMVR 服务所在主机， -不是 SEER Robokit 原生 TCP 端口。 - -| gRPC 方法 | SEER Robokit 行为 | 说明 | -| --- | --- | --- | -| `getRuntimeState` | 推送缓存，缺失时查询 `1004/1007/1300` | 只读 | -| `getNavigationStatus` | 跟踪任务查询 `1110`，无精确上下文时回退 `1020` | 只读；同步等待另用 `1101` 确认停车 | -| `emergencyStop` | `2000`，再执行 `3003` 或 `3067` | 软件停止，不替代硬件急停 | -| `clearFault` | 未实现 | 返回 `UnsupportedCommand` | -| `navigateToPose` | `3051` + `freeGo` | 地图绝对位姿，仅双轮差速底盘 | -| `navigateToStation` | `3051` | 站点路径导航；PGV 二次定位也使用此方法 | -| `followPath` | `3066` | 仙工“指定路径导航”，与 `3051` 不同 | -| `pauseNavigation` / `resumeNavigation` | `3001` / `3002` | 导航控制 | -| `cancelNavigation` | `3003`，路径队列使用 `3067` | 取消当前跟踪任务 | -| `setVelocity` / `stopVelocityControl` | `2010` | 车体速度；停止时发送全零速度 | -| `listMaps` / `listStations` | `1300` / `1301` | 只读 | -| `switchMap` | `2022` | 会改变定位所用地图 | -| `uploadMap` / `downloadMap` | `4010` / `4011` | 上传会抢权，下载只读 | -| `startMapping` / `stopMapping` | `6100` / `6101` | 建图控制 | -| `streamMap` | `1780/1800` 加内部解析和缓存 | 对外发送统一 2D/3D 地图，不暴露 `.smap` 原始格式 | - -查询运行状态： - -```bash -grpcurl -plaintext \ - -d '{"header":{"deviceId":"src1100"}}' \ - 127.0.0.1:50052 \ - cmvr.api.AgvService/getRuntimeState -``` - -查询导航状态： - -```bash -grpcurl -plaintext \ - -d '{"header":{"deviceId":"src1100"}}' \ - 127.0.0.1:50052 \ - cmvr.api.AgvService/getNavigationStatus -``` - -列出地图和当前地图站点： - -```bash -grpcurl -plaintext \ - -d '{"header":{"deviceId":"src1100"}}' \ - 127.0.0.1:50052 \ - cmvr.api.AgvService/listMaps - -grpcurl -plaintext \ - -d '{"header":{"deviceId":"src1100"}}' \ - 127.0.0.1:50052 \ - cmvr.api.AgvService/listStations -``` - -## 导航的同步语义 - -`navigateToPose`、`navigateToStation` 和 `followPath` 默认同步阻塞。控制器接受 -命令后，适配器继续轮询精确任务状态，并结合 `1101` 状态确认底盘已经停车； -到达、失败、取消、遇障停止或超时后才返回。`waitTimeoutMs` 为 `0` 时使用 -适配器默认值，当前为 10 分钟；`pollIntervalMs` 为 `0` 时当前使用 200 ms。 - -连续观察到障碍阻挡且底盘已经停止后，适配器会主动取消该导航；清理结果不明确 -时还可能发送软件停止。任务不会在障碍消失后由本次调用自动恢复。等待超时、 -RPC cancel 和 deadline 到期也会进入安全取消及停车确认，因此函数返回时间可能 -晚于最初发现障碍或取消请求的时刻。 - -调用方的 gRPC deadline 必须大于预计行程时间和 `waitTimeoutMs`。RPC 被取消或 -deadline 到期时，适配器会进入安全取消/停车确认流程。显式设置 -`"asynchronous":true` 后不会等待任务终态：站点导航和指定路径导航在控制器 -接受后返回；自由导航仍会做最长约 1.5 秒的启动确认。异步成功不代表已经到点。 - -`AgvMotionOptions` 中，SEER Robokit 的 `3051` 导航当前支持： - -| gRPC 字段 | 控制器字段 | 单位 | -| --- | --- | --- | -| `maxSpeed` | `max_speed` | m/s | -| `maxAngularSpeed` | `max_wspeed` | rad/s | -| `maxAcceleration` | `max_acc` | m/s² | -| `maxAngularAcceleration` | `max_wacc` | rad/s² | -| `reachDistance` | `reach_dist` | m | -| `reachAngle` | `reach_angle` | rad | - -`asynchronous`、`waitTimeoutMs` 和 `pollIntervalMs` 由适配器本地执行。 -`speedRatio` 当前没有对应的 SEER Robokit 序列化字段。`followPath` 的运动选项当前只 -控制同步/异步等待、超时和轮询；在没有确认 `3066` 的速度字段前，不会猜测性地 -写入每个路径段。 - -## 固定路径导航的 PGV 二次定位 - -仙工文档 [“路径导航 / 2. 固定路径导航 PGV 二次定位调整”](https://seer-group.feishu.cn/wiki/Q26SwaNoGisuLWk2vCxcPfVWn2e) -说明 PGV 参数是 `3051 / robot_task_gotarget_req` 的顶层可选字段。因此在 CMVR -中应调用 `navigateToStation`，不是 `followPath`。后者对应另一条 -`3066 / 指定路径导航` 协议，现有仙工资料和仓库历史都没有证明 `3066` 支持 -PGV 字段。 - -PGV 参数通过 `adapterParams.values` 传入。protobuf map 的值是字符串， -SEER Robokit 适配器会在任何状态查询、抢权和运动命令之前完成校验，再转换为控制器 -要求的 JSON `bool`/`number`： - -| `adapterParams.values` 键 | 输出 JSON 类型 | 含义 | -| --- | --- | --- | -| `use_pgv` | `bool` | 使用上视 PGV | -| `use_down_pgv` | `bool` | 使用下视 PGV | -| `pgv_adjust_dist` | `number` | 最大调整半径，必须为有限非负数；用于仙工第 3/4 种调整方式 | -| `pgv_adjust_cx` | `number` | 调整范围圆心在二维码坐标系下的 X 偏移；用于第 4 种方式 | -| `pgv_adjust_cy` | `number` | 调整范围圆心在二维码坐标系下的 Y 偏移；用于第 4 种方式 | -| `pgv_x_adjust` | `number` | 仅调整小车 X 方向误差；用于第 2 种方式 | - -所有数字都必须是完整、有限的数字字符串；偏移量允许正负。适配器不臆造 -调整半径上限，也不假定上视和下视一定互斥，这些约束应由实际 PGV 安装、标定和 -当前控制器版本确定。显式的 `"false"` 和 `"0"` 仍会作为原生布尔值和数值 -发给控制器；没有给出的字段不会发送。第 2/3/4 种方式由控制器和站点配置决定， -本接口只传递与所选方式匹配的调整参数。 - -一旦请求中出现任意 PGV 键，适配器只允许同时出现 `source_id`、`task_id` 和 -上述 PGV 字段；`operation`、`jack_height`、脚本名或未知扩展字段都会在状态 -查询和抢权前被拒绝，避免一次 PGV 导航意外夹带顶升、货叉、IO 或脚本动作。 -没有 PGV 键的既有站点导航扩展语义保持不变。 - -上视 PGV 示例。该命令会让机器人导航到 `AP1`，只能在确认地图、站点、PGV -标定、行驶区域和急停人员后执行： - -```bash -grpcurl -plaintext \ - -d '{ - "header":{"deviceId":"src1100"}, - "stationId":"AP1", - "options":{ - "maxSpeed":0.15, - "maxAcceleration":0.15, - "asynchronous":false, - "waitTimeoutMs":300000, - "pollIntervalMs":200 - }, - "adapterParams":{"values":{ - "use_pgv":"true", - "pgv_adjust_dist":"0.3", - "pgv_adjust_cx":"-0.3", - "pgv_adjust_cy":"0" - }} - }' \ - 127.0.0.1:50052 \ - cmvr.api.AgvService/navigateToStation -``` - -下视 PGV 使用同一接口，把 `use_down_pgv` 设为字符串 `"true"`；其他调整 -字段是否需要传入取决于现场定位方案。如果控制器版本要求明确起点，可在同一个 -map 中增加 `"source_id":"实际起点站点"`；默认起点为 `SELF_POSITION`。 - -仙工在线文档当前有两处拼写不一致： - -- 代码块出现了损坏字段 `pgv_adjustuse_pgv_dist`；适配器会拒绝它，正确字段是 - `pgv_adjust_dist`； -- 表格写成 `pgv_ajdust_cy`，而示例和仓库旧版序列化代码使用 - `pgv_adjust_cy`。适配器兼容接收前者，但只向控制器输出规范字段 - `pgv_adjust_cy`；两个拼写同时出现会因歧义被拒绝。 - -C++ 调用同样复用通用扩展参数： - -```cpp -cmvr::device::AgvMotionOptions options; -options.max_speed = 0.15; -options.max_acceleration = 0.15; - -cmvr::device::AgvAdapterParams adapter; -adapter.values["use_pgv"] = "true"; -adapter.values["pgv_adjust_dist"] = "0.3"; -adapter.values["pgv_adjust_cx"] = "-0.3"; -adapter.values["pgv_adjust_cy"] = "0"; - -const auto result = agv.navigateToStation("AP1", options, adapter); -``` - -## 其他导航和控制示例 - -自由导航使用地图绝对坐标，不是“相对当前位置移动多少米”。示例只展示请求 -结构，发送前必须读取当前位姿并确认目标在同一地图的安全区域： - -```bash -grpcurl -plaintext \ - -d '{ - "header":{"deviceId":"src1100"}, - "pose":{"x":1.0,"y":0.0,"theta":0.0}, - "options":{"maxSpeed":0.15,"maxAcceleration":0.15} - }' \ - 127.0.0.1:50052 \ - cmvr.api.AgvService/navigateToPose -``` - -显式站点路径使用 `3066`： - -```bash -grpcurl -plaintext \ - -d '{ - "header":{"deviceId":"src1100"}, - "path":[ - {"sourceStation":"LM1","targetStation":"LM2"}, - {"sourceStation":"LM2","targetStation":"AP1"} - ] - }' \ - 127.0.0.1:50052 \ - cmvr.api.AgvService/followPath -``` - -暂停、继续和取消的请求体直接是 `CommandHeader.Request`，没有外层 `header`： - -```bash -grpcurl -plaintext -d '{"deviceId":"src1100"}' \ - 127.0.0.1:50052 cmvr.api.AgvService/pauseNavigation - -grpcurl -plaintext -d '{"deviceId":"src1100"}' \ - 127.0.0.1:50052 cmvr.api.AgvService/resumeNavigation - -grpcurl -plaintext -d '{"deviceId":"src1100"}' \ - 127.0.0.1:50052 cmvr.api.AgvService/cancelNavigation -``` - -差速底盘的 `vy` 应保持 `0`。低层速度控制不等价于导航，并可能与已有任务 -冲突；只应在专门的速度控制测试流程中使用： - -```bash -grpcurl -plaintext \ - -d '{"header":{"deviceId":"src1100"},"velocity":{"vx":0.05,"vy":0,"wz":0}}' \ - 127.0.0.1:50052 \ - cmvr.api.AgvService/setVelocity - -grpcurl -plaintext -d '{"deviceId":"src1100"}' \ - 127.0.0.1:50052 cmvr.api.AgvService/stopVelocityControl -``` - -## 错误返回 - -控制器响应中的非零 `ret_code` 和 `err_msg` 会保留在 `AgvResult.message`，并由 -gRPC 同时写入 transport status message 和反馈头的 `errorMessage`。非 OK RPC -下，标准客户端通常不会交付响应体，因此跨客户端应以 transport status message -为准，不要依赖反馈头仍然可见。例如： - -```text -SEER Robokit command failed: ret_code=43051, err_msg=planner_rejected_pose -``` - -控制器仅返回“已接收”不等于导航完成；同步接口仍要等待精确任务终态和停车 -确认。若发送后连接中断且控制器是否执行已无法确定，错误会明确提示 outcome -unknown，调用方不能自动重发运动命令，应先查询状态并取消或停止。 - -## 安全边界 - -- 仙工文档明确把 `3051` 定位为任务链或验证测试等单车场景接口；不要把它当作 - 多车调度接口，否则可能出现路径/速度不连续等危险行为。 -- `emergencyStop` 是控制器软件停止，不是功能安全急停；真实系统必须保留可达的 - 硬件急停、安全激光、碰撞条和独立安全链。 -- 首次 PGV 测试应在低速、空载、隔离区域进行，并先核对二维码坐标系、传感器 - 上/下视方向、调整半径和中心偏移的标定值。 -- PGV 同步成功目前能证明精确 `3051` 任务进入终态，并连续确认两次零速度； - 仙工文档没有明确 `Completed` 是否一定覆盖 PGV 二次调整的全部阶段，仍需实机 - 验证后才能据此联动机械臂。异步成功更不代表 PGV 调整完成。 -- 地图切换、地图上传和开始建图会改变控制器状态，也会先抢占控制权；不要和 - 现场调度系统并行操作。 -- 本目录的假控制器测试验证软件协议、错误路径和并发逻辑，不代表真实 SEER Robokit、 - 底盘、PGV、地图或安全链已经验收。 - -## 测试 - -```bash -cmake --build build \ - --target seer_robokit_control_authority_test grpc_agv_service_test \ - -j2 - -ctest --test-dir build \ - -R '^(seer_robokit_control_authority_test|grpc_agv_service_test)$' \ - --output-on-failure -``` - -`seer_robokit_control_authority_test` 使用本机回环 TCP 假控制器，需要允许本地 -bind/listen。受限沙箱若禁止创建 socket，只能证明编译通过，不能把未执行的 -fake-controller 场景报告为测试通过。测试过程不会连接真实 AGV。 diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h deleted file mode 100644 index f06510f2..00000000 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h +++ /dev/null @@ -1,354 +0,0 @@ -#ifndef CMVR_ES_SEER_ROBOKIT_AGV_H -#define CMVR_ES_SEER_ROBOKIT_AGV_H - -#include -#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 SeerRobokitAgvTestPeer; - -class SeerRobokitAgv final : public AbstractAGV { -public: - explicit SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg); - ~SeerRobokitAgv() override; - - std::string typeName() const override { return "SeerRobokitAgv"; } - - bool supportsSynchronousAction(AgvActionKind kind) const noexcept override - { - switch (kind) { - case AgvActionKind::NavigateToPose: - case AgvActionKind::NavigateToStation: - case AgvActionKind::FollowPath: - return true; - } - return false; - } - - 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 followPath( - const std::vector& path, - const AgvMotionOptions& options) override; - AgvResult translate(const AgvTranslation& translation) override; - AgvResult pauseNavigation() override; - AgvResult resumeNavigation() override; - AgvResult cancelNavigation() override; - - AgvResult setVelocity(const AgvVelocity& velocity) override; - AgvResult confirmMotionStopped() 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: - friend class SeerRobokitAgvTestPeer; - - struct Ports { - int status{19204}; - int control{19205}; - int navigation{19206}; - int config{19207}; - int other{19210}; - int push{19301}; - }; - - struct PoseTaskStatus { - bool found{false}; - int state{0}; - int type{0}; - bool type_present{false}; - double progress{0.0}; - std::string detail; - }; - - struct NavigationSnapshot { - int task_status{0}; - int task_type{0}; - bool task_status_present{false}; - bool task_type_present{false}; - bool blocked{false}; - bool blocked_present{false}; - int block_reason{-1}; - std::string block_reason_raw; - bool velocity_present{false}; - double vx{0.0}; - double vy{0.0}; - double w{0.0}; - bool emergency{false}; - std::string target_id; - std::string active_faults; - std::string detail; - }; - - enum class CommandTransmissionState { - NotSent, - PossiblySent, - }; - - struct TrackedNavigationContext { - std::string token; - std::vector task_ids; - AgvTaskType type{AgvTaskType::None}; - std::string target_id; - std::vector target_ids; - std::uint64_t navigation_generation{0}; - std::chrono::steady_clock::time_point accepted_at{}; - bool synchronous_wait{false}; - }; - - struct PoseTaskContext { - std::string task_id; - math::Pose2d target{}; - double reach_distance{0.0}; - double reach_angle{0.0}; - std::uint64_t navigation_generation{0}; - std::uint64_t controller_fault_sequence_at_start{0}; - std::uint64_t control_attempt_sequence_at_start{0}; - std::uint64_t controller_fault_channel_epoch_at_start{0}; - }; - - AgvResult connect_(); - AgvResult disconnect_(); - AgvResult emergencyStopTrackedNavigation_( - const TrackedNavigationContext* expected_navigation); - AgvResult connectSocket_(int& sock, int port); - AgvResult ensureOtherSocket_(); - void closeSocket_(int& sock) const; - bool connected_() const; - - AgvResult acquireControl_() const; - AgvResult confirmPoseNavigationStarted_( - const PoseTaskContext& context, - bool accept_paused, - const AgvMotionOptions& options) const; - AgvResult waitForPoseNavigationTerminal_( - const PoseTaskContext& pose_context, - const TrackedNavigationContext& navigation_context, - const AgvMotionOptions& options); - AgvResult waitForTrackedNavigationTerminal_( - const TrackedNavigationContext& context, - const AgvMotionOptions& options); - AgvResult queryNavigationSnapshot_(NavigationSnapshot& snapshot) const; - AgvResult cancelTrackedNavigation_( - const TrackedNavigationContext& context, - std::uint64_t& accepted_generation); - AgvResult waitForCanceledTaskToStop_( - const TrackedNavigationContext& context, - const AgvMotionOptions& options, - const std::string& reason, - bool require_global_stopped = false); - AgvResult failAndCancelTrackedNavigation_( - const TrackedNavigationContext& context, - const AgvMotionOptions& options, - AgvErrorCode error_code, - const std::string& reason); - AgvResult queryPoseTaskStatus_( - const std::string& task_id, - PoseTaskStatus& status) const; - AgvResult queryTaskStatuses_( - const std::vector& task_ids, - std::vector& statuses) const; - bool poseTargetReached_( - const PoseTaskContext& context, - std::string& detail) const; - std::string cachedControllerFaultDetail_( - std::uint64_t after_sequence = 0, - int wait_ms = 0, - std::uint64_t* associated_control_attempt = nullptr) const; - int controllerFaultCaptureGraceMs_() const; - int controllerFaultStateMaxAgeMs_() const; - std::string freeNavigationFaultStateUnavailableDetail_() const; - void rememberPoseTask_(const PoseTaskContext& context) const; - void advancePoseTaskGeneration_( - std::uint64_t navigation_generation, - std::uint64_t control_attempt_sequence) const; - void advancePoseTaskControlAttempt_( - std::uint64_t control_attempt_sequence) const; - void clearPoseTask_(std::uint64_t navigation_generation) const; - void clearPoseTaskIfTaskId_(const std::string& task_id) const; - bool currentPoseTask_(PoseTaskContext& context) const; - void rememberTrackedNavigation_( - const TrackedNavigationContext& context) const; - void advanceTrackedNavigationGeneration_( - std::uint64_t navigation_generation) const; - void clearTrackedNavigation_(std::uint64_t navigation_generation) const; - void clearTrackedNavigationIfToken_(const std::string& token) const; - bool currentTrackedNavigation_( - TrackedNavigationContext& context) const; - AgvResult sendControlledCommand_(int sock, - std::uint16_t command, - const Json::Value& payload, - Json::Value* response, - std::uint64_t* accepted_navigation_generation = nullptr, - std::uint64_t* controller_fault_sequence_at_attempt = nullptr, - std::uint64_t* control_attempt_sequence = nullptr, - PoseTaskContext* pose_context_to_publish = nullptr, - bool reject_if_active_controller_fault = false, - TrackedNavigationContext* navigation_context_to_publish = nullptr, - bool preserve_tracked_navigation = false, - const std::string* expected_navigation_token = nullptr, - const std::function* cancellation_requested = nullptr, - const TrackedNavigationContext* expected_active_navigation = nullptr) const; - AgvResult sendCommand_(int sock, - std::uint16_t command, - const Json::Value& payload, - Json::Value* response, - CommandTransmissionState* transmission_state = nullptr) const; - AgvResult sendCommandRaw_(int sock, - std::uint16_t command, - const Json::Value& payload, - std::string* response_payload, - CommandTransmissionState* transmission_state = nullptr) const; - AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const; - AgvResult configurePush_(); - void startPushThread_(); - void stopPushThread_(); - void pushLoop_(); - void invalidateControllerFaultState_(); - 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 parseSeerRobokitMapArchive_( - const std::string& file_name, - const std::string& content, - const AgvMapStreamOptions& options, - std::vector& updates) const; - AgvResult parseSeerRobokitMap2D_( - const std::string& file_name, - const std::string& content, - const AgvMapStreamOptions& options, - AgvUnifiedMapUpdate& update) const; - AgvResult parseSeerRobokitMap3D_( - 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 void applyMotionOptions_( - Json::Value& payload, - const AgvMotionOptions& options, - bool include_reach_options = true); - static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params); - static AgvResult resultFromResponse_(const Json::Value& response); - - config::SeerRobokitAgvConfig config_; - std::string ip_; - std::string control_nick_name_; - 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_; - mutable std::mutex status_io_mutex_; - mutable std::mutex control_sequence_mutex_; - mutable std::atomic navigation_generation_{0}; - mutable std::atomic pose_task_sequence_{0}; - mutable std::atomic control_attempt_sequence_{0}; - mutable std::atomic controller_fault_channel_epoch_{0}; - mutable std::mutex pose_task_mutex_; - mutable PoseTaskContext pose_task_context_; - mutable std::mutex tracked_navigation_mutex_; - mutable TrackedNavigationContext tracked_navigation_context_; - mutable int sock_status_{-1}; - mutable int sock_control_{-1}; - mutable int sock_navigation_{-1}; - mutable int sock_config_{-1}; - mutable int sock_other_{-1}; - mutable int sock_push_{-1}; - std::string last_error_; - - std::atomic push_running_{false}; - std::thread push_thread_; - mutable std::mutex runtime_state_mutex_; - mutable std::condition_variable runtime_state_cv_; - AgvRuntimeState cached_runtime_state_; - bool cached_runtime_state_valid_{false}; - std::uint64_t controller_fault_sequence_{0}; - bool controller_fault_state_observed_{false}; - std::chrono::steady_clock::time_point controller_fault_state_observed_at_{}; - std::string active_controller_fault_detail_; - double last_controller_fault_timestamp_{0.0}; - std::string last_controller_fault_detail_; - std::uint64_t last_controller_fault_control_attempt_{0}; - - 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_SEER_ROBOKIT_AGV_H diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h deleted file mode 100644 index 04a08780..00000000 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h +++ /dev/null @@ -1,257 +0,0 @@ -#ifndef CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H -#define CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H - -#include -#include -#include -#include -#include -#include -#include - -#include "devices/agv/abstract_agv.h" - -namespace cmvr::device::seer_robokit::navigation { - -constexpr auto kPoseNavigationStartTimeout = std::chrono::milliseconds(1500); -constexpr auto kPoseNavigationPollInterval = std::chrono::milliseconds(50); -constexpr int kPoseNavigationRequiredRunningSamples = 2; -constexpr auto kDefaultNavigationWaitTimeout = - std::chrono::milliseconds(600000); -constexpr auto kDefaultNavigationPollInterval = - std::chrono::milliseconds(200); -constexpr auto kMaximumNavigationPollInterval = - std::chrono::milliseconds(5000); -constexpr auto kNavigationCancellationCheckInterval = - std::chrono::milliseconds(50); -constexpr auto kNavigationCancelPollInterval = - std::chrono::milliseconds(100); -constexpr auto kNavigationCancelConfirmationTimeout = - std::chrono::milliseconds(3000); -constexpr int kRequiredBlockedStopSamples = 2; -constexpr int kRequiredCompletedStopSamples = 2; -constexpr double kNavigationStopVelocityTolerance = 0.005; -constexpr int kMinimumControllerFaultCaptureGraceMs = 250; -constexpr int kMaximumControllerFaultCaptureGraceMs = 5000; -constexpr int kDefaultControllerFaultPushIntervalMs = 1000; -constexpr int kControllerFaultPushJitterMs = 100; -constexpr int kMinimumControllerFaultStateMaxAgeMs = 2000; -constexpr int kControllerFaultStateMaxAgeIntervals = 5; -constexpr double kDefaultPoseReachDistance = 0.05; -constexpr double kDefaultPoseReachAngle = 0.10; -constexpr double kTwoPi = 6.28318530717958647692; - -static inline bool exactTaskStateIsActive(const int state) -{ - return state >= 1 && state <= 3; -} - -static inline bool exactTaskStateIsKnownTerminal(const int state) -{ - return state >= 4 && state <= 7; -} - -static inline bool globalTaskStateIsKnownTerminal(const int state) -{ - return state == 0 || exactTaskStateIsKnownTerminal(state); -} - -static inline double angleDistance(const double lhs, const double rhs) -{ - return std::abs(std::remainder(lhs - rhs, kTwoPi)); -} - -static inline AgvResult withUnknownControllerOutcome(AgvResult result) -{ - const auto code = result.ok() ? AgvErrorCode::CommandFailed : result.code; - std::string detail = result.message.empty() ? "unknown transport or protocol error" : result.message; - detail += - "; SEER Robokit controller outcome is unknown after the command attempt; " - "the command may already have taken effect; do not issue another motion " - "command automatically; query status and cancel or stop first"; - return AgvResult::failure(code, detail); -} - -static inline std::string makePoseTaskId( - const std::string& device_id, - const std::uint64_t task_sequence) -{ - const auto timestamp = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); - const std::string prefix = device_id.empty() ? "cmvr-es" : device_id; - return prefix + "_pose_" + std::to_string(timestamp) - + "_" + std::to_string(task_sequence); -} - -static inline std::string makeNavigationTaskId( - const std::string& device_id, - const char* kind, - const std::uint64_t task_sequence) -{ - const auto timestamp = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); - const std::string prefix = device_id.empty() ? "cmvr-es" : device_id; - return prefix + "_" + kind + "_" + std::to_string(timestamp) - + "_" + std::to_string(task_sequence); -} - -static inline const char* blockReasonName(const int reason) -{ - switch (reason) { - case 0: - return "ultrasonic"; - case 1: - return "laser"; - case 2: - return "fallingdown"; - case 3: - return "collision"; - case 4: - return "infrared"; - case 5: - return "locked"; - default: - return "unknown"; - } -} - -static inline std::string invalidMotionOption(const AgvMotionOptions& options) -{ - const auto non_negative_error = [](const double value, const char* field) { - if (!std::isfinite(value)) { - return std::string(field) + " must be finite"; - } - if (value < 0.0) { - return std::string(field) + " must be non-negative"; - } - return std::string{}; - }; - - if (auto error = non_negative_error(options.max_speed, "max_speed"); - !error.empty()) return error; - if (auto error = non_negative_error( - options.max_angular_speed, - "max_angular_speed"); - !error.empty()) return error; - if (auto error = non_negative_error( - options.max_acceleration, - "max_acceleration"); - !error.empty()) return error; - if (auto error = non_negative_error( - options.max_angular_acceleration, - "max_angular_acceleration"); - !error.empty()) return error; - if (auto error = non_negative_error( - options.reach_distance, - "reach_distance"); - !error.empty()) return error; - if (auto error = non_negative_error(options.reach_angle, "reach_angle"); - !error.empty()) return error; - if (auto error = non_negative_error(options.speed_ratio, "speed_ratio"); - !error.empty()) return error; - if (options.wait_timeout_ms < 0) { - return "wait_timeout_ms must be non-negative"; - } - if (options.poll_interval_ms < 0) { - return "poll_interval_ms must be non-negative"; - } - if (options.poll_interval_ms - > kMaximumNavigationPollInterval.count()) { - return "poll_interval_ms must not exceed " - + std::to_string(kMaximumNavigationPollInterval.count()); - } - if (options.wait_timeout_ms > 0 - && options.poll_interval_ms > options.wait_timeout_ms) { - return "poll_interval_ms must not exceed wait_timeout_ms"; - } - return {}; -} - -static inline std::chrono::milliseconds navigationWaitTimeout( - const AgvMotionOptions& options) -{ - return options.wait_timeout_ms > 0 - ? std::chrono::milliseconds(options.wait_timeout_ms) - : kDefaultNavigationWaitTimeout; -} - -static inline std::chrono::milliseconds navigationPollInterval( - const AgvMotionOptions& options) -{ - if (options.poll_interval_ms <= 0) { - return kDefaultNavigationPollInterval; - } - return std::chrono::milliseconds( - std::max(options.poll_interval_ms, 20)); -} - -static inline bool navigationCancellationRequested(const AgvMotionOptions& options) -{ - return options.cancellation_requested - && options.cancellation_requested(); -} - -static inline void sleepForNavigationPoll( - const std::chrono::milliseconds poll_interval, - const std::chrono::steady_clock::time_point overall_deadline, - const AgvMotionOptions& options) -{ - const auto poll_deadline = std::min( - overall_deadline, - std::chrono::steady_clock::now() + poll_interval); - while (std::chrono::steady_clock::now() < poll_deadline - && !navigationCancellationRequested(options)) { - const auto remaining = std::chrono::duration_cast( - poll_deadline - std::chrono::steady_clock::now()); - if (remaining <= std::chrono::milliseconds::zero()) { - break; - } - std::this_thread::sleep_for(std::min( - kNavigationCancellationCheckInterval, - remaining)); - } -} - -template -static inline bool navigationStopped(const Snapshot& snapshot) -{ - return snapshot.velocity_present - && std::abs(snapshot.vx) <= kNavigationStopVelocityTolerance - && std::abs(snapshot.vy) <= kNavigationStopVelocityTolerance - && std::abs(snapshot.w) <= kNavigationStopVelocityTolerance; -} - -static inline AgvResult reconciledNavigationResult( - const AgvResult& command_result, - AgvResult terminal_result) -{ - if (command_result.ok()) { - return terminal_result; - } - if (terminal_result.ok()) { - terminal_result.message = - "SEER Robokit navigation completed after an indeterminate command " - "acknowledgment; initial_detail=" + command_result.message; - return terminal_result; - } - terminal_result.message = - "SEER Robokit navigation command acknowledgment was indeterminate: " - + command_result.message + "; status reconciliation: " - + terminal_result.message; - return terminal_result; -} - -static inline bool parseFiniteDouble(const std::string& value, double& parsed) -{ - std::size_t consumed = 0; - try { - parsed = std::stod(value, &consumed); - } catch (...) { - return false; - } - return consumed == value.size() && std::isfinite(parsed); -} - -} // namespace cmvr::device::seer_robokit::navigation - -#endif // CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_pgv_utils.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_pgv_utils.h deleted file mode 100644 index 37dbbec7..00000000 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_pgv_utils.h +++ /dev/null @@ -1,141 +0,0 @@ -#ifndef CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H -#define CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H - -#include - -#include - -#include "devices/agv/abstract_agv.h" -#include "seer_robokit_navigation_utils.h" -#include "seer_robokit_utils.h" - -namespace cmvr::device::seer_robokit::pgv { - -constexpr char kUsePgv[] = "use_pgv"; -constexpr char kPgvAdjustDist[] = "pgv_adjust_dist"; -constexpr char kPgvAdjustCx[] = "pgv_adjust_cx"; -constexpr char kPgvAdjustCy[] = "pgv_adjust_cy"; -constexpr char kPgvXAdjust[] = "pgv_x_adjust"; -constexpr char kUseDownPgv[] = "use_down_pgv"; - -// These spellings currently appear in the vendor document, but conflict with -// its own field table/example and the repository's older working serializer. -constexpr char kMalformedAdjustDist[] = "pgv_adjustuse_pgv_dist"; -constexpr char kAdjustCyDocumentAlias[] = "pgv_ajdust_cy"; - -static inline bool isPgvAdjustmentKey(const std::string& key) -{ - return key == kUsePgv - || key == kPgvAdjustDist - || key == kPgvAdjustCx - || key == kPgvAdjustCy - || key == kPgvXAdjust - || key == kUseDownPgv - || key == kMalformedAdjustDist - || key == kAdjustCyDocumentAlias; -} - -static inline bool hasPgvAdjustmentParams(const AgvAdapterParams& params) -{ - for (const auto& [key, value] : params.values) { - (void)value; - if (isPgvAdjustmentKey(key)) { - return true; - } - } - return false; -} - -/** - * Parse the string-valued generic adapter parameters into the native JSON - * types required by SEER Robokit API 3051. Returns an error string without - * modifying controller state; an empty string means success. - */ -static inline std::string applyPgvAdjustmentParams( - Json::Value& payload, - const AgvAdapterParams& params) -{ - if (params.getString(kMalformedAdjustDist)) { - return std::string(kMalformedAdjustDist) - + " is a vendor-document typo; use " + kPgvAdjustDist; - } - if (hasPgvAdjustmentParams(params)) { - for (const auto& [key, value] : params.values) { - (void)value; - if (!isPgvAdjustmentKey(key) - && key != "source_id" - && key != "task_id") { - return "PGV adjustment must not be combined with adapter " - "field " + key; - } - } - } - const auto adjust_cy = params.getString(kPgvAdjustCy); - const auto adjust_cy_alias = params.getString(kAdjustCyDocumentAlias); - if (adjust_cy && adjust_cy_alias) { - return std::string(kPgvAdjustCy) + " and its vendor-document alias " - + kAdjustCyDocumentAlias + " must not both be set"; - } - - const auto apply_bool = [&payload, ¶ms](const char* key) { - if (!params.getString(key)) { - return std::string{}; - } - const auto parsed = params.getBool(key); - if (!parsed) { - return std::string(key) - + " must be a boolean string such as true or false"; - } - detail::jsonMember(payload, key) = *parsed; - return std::string{}; - }; - if (auto error = apply_bool(kUsePgv); !error.empty()) { - return error; - } - if (auto error = apply_bool(kUseDownPgv); !error.empty()) { - return error; - } - - const auto apply_number = [&payload, ¶ms]( - const char* key, - const bool non_negative) { - const auto raw = params.getString(key); - if (!raw) { - return std::string{}; - } - double parsed = 0.0; - if (!navigation::parseFiniteDouble(*raw, parsed)) { - return std::string(key) + " must be a complete finite number"; - } - if (non_negative && parsed < 0.0) { - return std::string(key) + " must be non-negative"; - } - detail::jsonMember(payload, key) = parsed; - return std::string{}; - }; - if (auto error = apply_number(kPgvAdjustDist, true); !error.empty()) { - return error; - } - if (auto error = apply_number(kPgvAdjustCx, false); !error.empty()) { - return error; - } - if (adjust_cy_alias) { - double parsed = 0.0; - if (!navigation::parseFiniteDouble(*adjust_cy_alias, parsed)) { - return std::string(kAdjustCyDocumentAlias) - + " must be a complete finite number"; - } - detail::jsonMember(payload, kPgvAdjustCy) = parsed; - } else if (auto error = apply_number(kPgvAdjustCy, false); - !error.empty()) { - return error; - } - if (auto error = apply_number(kPgvXAdjust, false); !error.empty()) { - return error; - } - return {}; -} - -} // namespace cmvr::device::seer_robokit::pgv - -#endif // CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h deleted file mode 100644 index 89df8282..00000000 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef CMVR_ES_SEER_ROBOKIT_PROTOCOL_H -#define CMVR_ES_SEER_ROBOKIT_PROTOCOL_H - -#include - -namespace cmvr::device::seer_robokit::protocol { - -constexpr std::uint16_t kRobotStatusLoc = 1004; -constexpr std::uint16_t kRobotStatusBattery = 1007; -constexpr std::uint16_t kRobotStatusAll2 = 1101; -constexpr std::uint16_t kRobotStatusTask = 1020; -constexpr std::uint16_t kRobotStatusTaskPackage = 1110; -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 kRobotTaskTranslate = 3055; -constexpr std::uint16_t kRobotTaskGoTargetList = 3066; -constexpr std::uint16_t kRobotTaskClearTargetList = 3067; -constexpr std::uint16_t kRobotConfigLock = 4005; -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; - -} // namespace cmvr::device::seer_robokit::protocol - -#endif // CMVR_ES_SEER_ROBOKIT_PROTOCOL_H diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_utils.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_utils.h deleted file mode 100644 index 12095921..00000000 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_utils.h +++ /dev/null @@ -1,92 +0,0 @@ -#ifndef CMVR_ES_SEER_ROBOKIT_UTILS_H -#define CMVR_ES_SEER_ROBOKIT_UTILS_H - -#include -#include -#include -#include - -#include - -namespace cmvr::device::seer_robokit::detail { - -static inline std::string systemError() -{ - return std::strerror(errno); -} - -static inline Json::Value& jsonMember( - Json::Value& value, - const char* key) -{ - return *value.demand(key, key + std::strlen(key)); -} - -static inline Json::Value& jsonMember( - Json::Value& value, - const std::string& key) -{ - return *value.demand(key.data(), key.data() + key.size()); -} - -static inline const Json::Value* jsonFind( - const Json::Value& value, - const char* key) -{ - return value.find(key, key + std::strlen(key)); -} - -static inline Json::Value jsonGet( - const Json::Value& value, - const char* key, - const Json::Value& fallback) -{ - const auto* found = jsonFind(value, key); - return found ? *found : fallback; -} - -static inline double nowSeconds() -{ - const auto now = std::chrono::system_clock::now().time_since_epoch(); - return std::chrono::duration(now).count(); -} - -static inline bool jsonHas( - const Json::Value& value, - const char* key) -{ - return jsonFind(value, key) != nullptr; -} - -static inline bool hasNumericControllerRetCode( - const Json::Value& response) -{ - const auto* ret_code = jsonFind(response, "ret_code"); - return ret_code - && (ret_code->isInt() - || ret_code->isUInt() - || ret_code->isInt64() - || ret_code->isUInt64()); -} - -static inline 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); -} - -} // namespace cmvr::device::seer_robokit::detail - -#endif // CMVR_ES_SEER_ROBOKIT_UTILS_H diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp deleted file mode 100644 index 03457dd3..00000000 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp +++ /dev/null @@ -1,175 +0,0 @@ -#include "seer_robokit_agv.h" - -#include -#include -#include - -#include "common/base/logging/logger.h" - -namespace cmvr::device { - -namespace { - -constexpr int kDefaultMapUpdateIntervalMs = 1000; -constexpr std::size_t kDefaultMapUpdateHistorySize = 8; - -} // namespace - -SeerRobokitAgv::SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg) - : config_(cfg), - ip_(cfg.ip()), - control_nick_name_( - cfg.control_nick_name().empty() - ? (cfg.id().empty() ? "cmvr-es" : "cmvr-es:" + cfg.id()) - : cfg.control_nick_name()), - 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) << "[SeerRobokitAgv] Auto connect failed" - << ", id=" << id_ - << ", ip=" << ip_ - << ", error=" << result.message; - } -} - -SeerRobokitAgv::~SeerRobokitAgv() -{ - (void)disconnect_(); -} - -bool SeerRobokitAgv::init() -{ - return !id_.empty() && !ip_.empty(); -} - -bool SeerRobokitAgv::start() -{ - return true; -} - -bool SeerRobokitAgv::stop() -{ - return true; -} - -bool SeerRobokitAgv::update() -{ - return true; -} - -AgvResult SeerRobokitAgv::connect_() -{ - const auto lifecycle_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; - clearPoseTask_(lifecycle_generation); - clearTrackedNavigation_(lifecycle_generation); - stopPushThread_(); - stopMapUpdateThread_(); - - { - // Status requests may wait for a controller receive timeout without - // holding mutex_. Serialize lifecycle changes with that channel before - // replacing or closing its descriptor. - std::lock_guard status_io_lock(status_io_mutex_); - 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, "SEER Robokit 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) << "[SeerRobokitAgv] 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) << "[SeerRobokitAgv] 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 SeerRobokitAgv::disconnect_() -{ - const auto lifecycle_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; - clearPoseTask_(lifecycle_generation); - clearTrackedNavigation_(lifecycle_generation); - stopMapUpdateThread_(); - stopPushThread_(); - std::lock_guard status_io_lock(status_io_mutex_); - 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(); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp deleted file mode 100644 index 9f3f0b21..00000000 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp +++ /dev/null @@ -1,378 +0,0 @@ -#include "seer_robokit_agv.h" -#include "seer_robokit_navigation_utils.h" -#include "seer_robokit_protocol.h" -#include "seer_robokit_utils.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::device { - -using namespace seer_robokit::navigation; -using namespace seer_robokit::protocol; -using namespace seer_robokit::detail; - -AgvResult SeerRobokitAgv::acquireControl_() const -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "nick_name") = control_nick_name_; - - Json::Value response; - auto result = sendCommand_(sock_config_, kRobotConfigLock, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult SeerRobokitAgv::sendControlledCommand_( - const int sock, - const std::uint16_t command, - const Json::Value& payload, - Json::Value* response, - std::uint64_t* accepted_navigation_generation, - std::uint64_t* controller_fault_sequence_at_attempt, - std::uint64_t* control_attempt_sequence, - PoseTaskContext* pose_context_to_publish, - const bool reject_if_active_controller_fault, - TrackedNavigationContext* navigation_context_to_publish, - const bool preserve_tracked_navigation, - const std::string* expected_navigation_token, - const std::function* cancellation_requested, - const TrackedNavigationContext* expected_active_navigation) const -{ - const auto canceled_before_send = [cancellation_requested]() { - return cancellation_requested - && *cancellation_requested - && (*cancellation_requested)(); - }; - if (canceled_before_send()) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit command was not sent because the caller canceled the " - "operation before control authority was acquired"); - } - - const auto expected_context_is_current = - [this, expected_active_navigation]() { - if (!expected_active_navigation) { - return true; - } - TrackedNavigationContext active_context; - return currentTrackedNavigation_(active_context) - && active_context.token - == expected_active_navigation->token - && active_context.navigation_generation - == expected_active_navigation->navigation_generation - && active_context.type - == expected_active_navigation->type - && navigation_generation_.load(std::memory_order_relaxed) - == expected_active_navigation->navigation_generation; - }; - - // Conditional cancel ownership checks are deliberately performed without - // the control sequencing mutex. A slow 1110/1101 response must never - // prevent emergencyStop() from acquiring authority and sending 2000. - // The exact local token/generation/type is revalidated under the control - // lock both before and after authority acquisition below. - if (expected_active_navigation) { - if (!expected_context_is_current()) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit did not start conditional navigation cancel " - "preflight because the tracked task was already replaced or " - "ended"); - } - - std::vector statuses; - const auto exact_result = queryTaskStatuses_( - expected_active_navigation->task_ids, - statuses); - if (!exact_result.ok()) { - return AgvResult::failure( - exact_result.code, - "SEER Robokit did not send the conditional navigation cancel " - "because exact task ownership preflight failed: " - + exact_result.message); - } - const bool all_exact_tasks_terminal = !statuses.empty() - && std::all_of( - statuses.begin(), - statuses.end(), - [](const PoseTaskStatus& status) { - return status.found - && exactTaskStateIsKnownTerminal(status.state); - }); - if (all_exact_tasks_terminal) { - return AgvResult::success(); - } - - const bool exact_task_still_active = std::any_of( - statuses.begin(), - statuses.end(), - [](const PoseTaskStatus& status) { - return status.found - && exactTaskStateIsActive(status.state); - }); - - NavigationSnapshot snapshot; - const auto snapshot_result = queryNavigationSnapshot_(snapshot); - if (!snapshot_result.ok()) { - return AgvResult::failure( - snapshot_result.code, - "SEER Robokit did not send the conditional navigation cancel " - "because 1101 ownership preflight was unavailable: " - + snapshot_result.message); - } - const int expected_type = expected_active_navigation->type - == AgvTaskType::NavigateToPose - ? 1 - : (expected_active_navigation->type - == AgvTaskType::NavigateToStation - ? 2 - : 3); - const bool global_active = - exactTaskStateIsActive(snapshot.task_status); - const bool target_conflicts = global_active - && !snapshot.target_id.empty() - && !expected_active_navigation->target_ids.empty() - && std::find( - expected_active_navigation->target_ids.begin(), - expected_active_navigation->target_ids.end(), - snapshot.target_id) - == expected_active_navigation->target_ids.end(); - if (global_active - && (snapshot.task_type != expected_type - || target_conflicts)) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit did not send the conditional navigation cancel " - "because 1101 reports another active task: " - + snapshot.detail); - } - - if (!exact_task_still_active) { - const bool clearing_path_queue = - expected_active_navigation->type - == AgvTaskType::FollowPath; - const bool terminal_target_matches = - expected_active_navigation->type - != AgvTaskType::NavigateToStation - || (!snapshot.target_id.empty() - && snapshot.target_id - == expected_active_navigation->target_id); - if (globalTaskStateIsKnownTerminal(snapshot.task_status) - && snapshot.task_status != 0 - && !clearing_path_queue - && snapshot.task_type == expected_type - && terminal_target_matches) { - return AgvResult::success(); - } - } - } - - // Keep the permission acquisition and the following write ordered with - // respect to other control RPCs in this process. Channel I/O serialization - // is separate, so this must remain a distinct lock. - std::lock_guard sequence_lock(control_sequence_mutex_); - if (expected_navigation_token) { - TrackedNavigationContext active_context; - const bool has_active_context = - currentTrackedNavigation_(active_context); - const bool expected_context_matches = expected_active_navigation - ? (has_active_context - && active_context.token - == expected_active_navigation->token - && active_context.navigation_generation - == expected_active_navigation->navigation_generation - && active_context.type - == expected_active_navigation->type - && navigation_generation_.load(std::memory_order_relaxed) - == expected_active_navigation->navigation_generation) - : (has_active_context - && active_context.token == *expected_navigation_token); - if (!expected_context_matches) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit did not send the conditional navigation cancel " - "because the tracked task was already replaced or ended; " - "expected_token=" + *expected_navigation_token - + (active_context.token.empty() - ? std::string(", active_token=") - : ", active_token=" + active_context.token)); - } - } - const auto attempt_sequence = - control_attempt_sequence_.fetch_add( - 1, - std::memory_order_relaxed) + 1; - if (control_attempt_sequence) { - *control_attempt_sequence = attempt_sequence; - } - if (pose_context_to_publish) { - pose_context_to_publish->control_attempt_sequence_at_start = - attempt_sequence; - } - - const auto authority = acquireControl_(); - if (!authority.ok()) { - const std::string detail = authority.message.empty() ? "unknown error" : authority.message; - return AgvResult::failure( - authority.code, - "SEER Robokit acquire control authority failed: " + detail); - } - if (expected_active_navigation && !expected_context_is_current()) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit did not send the conditional navigation cancel because " - "the tracked token, generation, or type changed while control " - "authority was being acquired"); - } - if (canceled_before_send()) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit command was not sent because the caller canceled the " - "operation while control authority was being acquired"); - } - std::string controller_fault_gate_error; - if (controller_fault_sequence_at_attempt - || pose_context_to_publish - || reject_if_active_controller_fault) { - std::lock_guard lock(runtime_state_mutex_); - if (controller_fault_sequence_at_attempt) { - *controller_fault_sequence_at_attempt = - controller_fault_sequence_; - } - if (pose_context_to_publish) { - pose_context_to_publish->controller_fault_sequence_at_start = - controller_fault_sequence_; - pose_context_to_publish - ->controller_fault_channel_epoch_at_start = - controller_fault_channel_epoch_.load( - std::memory_order_relaxed); - } - if (reject_if_active_controller_fault) { - if (!state_push_enabled_) { - controller_fault_gate_error = - "controller fault state is unavailable because state push " - "is disabled"; - } else if (!active_controller_fault_detail_.empty()) { - controller_fault_gate_error = - "the controller reported a fault or invalid fault state: " - + active_controller_fault_detail_; - } else if (!controller_fault_state_observed_) { - controller_fault_gate_error = - "no state push containing fatals/errors has been observed"; - } else { - const auto fault_state_age = - std::chrono::duration_cast( - std::chrono::steady_clock::now() - - controller_fault_state_observed_at_) - .count(); - if (fault_state_age > controllerFaultStateMaxAgeMs_()) { - controller_fault_gate_error = - "the most recent fatals/errors state push is stale " - "(age_ms=" + std::to_string(fault_state_age) - + ", max_age_ms=" - + std::to_string(controllerFaultStateMaxAgeMs_()) - + ")"; - } - } - } - } - if (!controller_fault_gate_error.empty()) { - return AgvResult::failure( - AgvErrorCode::Fault, - "SEER Robokit free-navigation command was not sent because " - + controller_fault_gate_error); - } - if (canceled_before_send()) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit command was not sent because the caller canceled the " - "operation before the controller command write"); - } - if (canceled_before_send()) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit command was not sent because the caller canceled the " - "operation immediately before the controller command write"); - } - const auto publish_navigation_generation = - [this, - accepted_navigation_generation, - pose_context_to_publish, - navigation_context_to_publish, - preserve_tracked_navigation]() { - const auto generation = - navigation_generation_.fetch_add( - 1, - std::memory_order_relaxed) + 1; - *accepted_navigation_generation = generation; - if (pose_context_to_publish) { - pose_context_to_publish->navigation_generation = generation; - rememberPoseTask_(*pose_context_to_publish); - } - if (navigation_context_to_publish) { - navigation_context_to_publish->navigation_generation = - generation; - navigation_context_to_publish->accepted_at = - std::chrono::steady_clock::now(); - rememberTrackedNavigation_( - *navigation_context_to_publish); - } else if (preserve_tracked_navigation) { - advanceTrackedNavigationGeneration_(generation); - } else { - clearTrackedNavigation_(generation); - } - }; - CommandTransmissionState transmission_state = - CommandTransmissionState::NotSent; - auto result = sendCommand_( - sock, - command, - payload, - response, - &transmission_state); - if (!result.ok()) { - if (accepted_navigation_generation - && transmission_state - == CommandTransmissionState::PossiblySent) { - // Once the control write has been attempted, a timeout, disconnect, - // wrong response opcode, or malformed JSON cannot prove rejection: - // the controller may already have executed the command. - publish_navigation_generation(); - return withUnknownControllerOutcome(std::move(result)); - } - return result; - } - if (!accepted_navigation_generation) { - return result; - } - if (!response) { - publish_navigation_generation(); - return withUnknownControllerOutcome(AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit cannot confirm navigation command without a response")); - } - if (!hasNumericControllerRetCode(*response)) { - publish_navigation_generation(); - return withUnknownControllerOutcome(resultFromResponse_(*response)); - } - result = resultFromResponse_(*response); - if (!result.ok()) { - return result; - } - - // Advance only after the controller accepted the command, and do it before - // releasing control_sequence_mutex_. This prevents a failed cancel/pause or - // failed authority acquisition from falsely reporting a pose task canceled, - // while preserving the controller's actual command order under concurrency. - publish_navigation_generation(); - return result; -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp deleted file mode 100644 index d4b5ce2f..00000000 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp +++ /dev/null @@ -1,1047 +0,0 @@ -#include "seer_robokit_agv.h" -#include "seer_robokit_protocol.h" -#include "seer_robokit_utils.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/seer_robokit_map3d.pb.h" - -namespace cmvr::device { - -using namespace seer_robokit::protocol; -using namespace seer_robokit::detail; - -namespace { - -namespace fs = std::filesystem; - -constexpr std::uint64_t kMapSnapshotSequenceStart = 1; - -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_seer_robokit_map_XXXXXX"; - std::string path = pattern.string(); - char* created = ::mkdtemp(path.data()); - if (!created) { - return {}; - } - return fs::path(created); -} - -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)); -} - -} // namespace - -AgvResult SeerRobokitAgv::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 SeerRobokitAgv::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 SeerRobokitAgv::switchMap(const std::string& map_name) -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "map_name") = map_name; - Json::Value response; - std::uint64_t accepted_generation = 0; - auto result = sendControlledCommand_( - sock_control_, - kRobotControlLoadMap, - payload, - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - -AgvResult SeerRobokitAgv::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 = sendControlledCommand_(sock_config_, kRobotConfigUploadMap, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -// AgvResult SeerRobokitAgv::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 SeerRobokitAgv::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; - - // 4011 失败响应包含 ret_code；成功响应本身就是完整地图 JSON。 - if (hasNumericControllerRetCode(response)) { - return resultFromResponse_(response); - } - - content = toJsonString_(response); - if (content.empty()) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit downloaded map content is empty"); - } - - return AgvResult::success(); -} - - -AgvResult SeerRobokitAgv::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; - std::uint64_t accepted_generation = 0; - result = sendControlledCommand_( - sock_other_, - kRobotOtherStartMapping, - payload, - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - 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 SeerRobokitAgv::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 SeerRobokitAgv::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, "SEER Robokit unified map update timeout"); -} - -void SeerRobokitAgv::startMapUpdateThread_() -{ - if (map_update_running_.exchange(true)) { - return; - } - map_update_thread_ = std::thread(&SeerRobokitAgv::mapUpdateLoop_, this); -} - -void SeerRobokitAgv::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 SeerRobokitAgv::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 SeerRobokitAgv::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) << "[SeerRobokitAgv] 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, "SEER Robokit 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, "SEER Robokit 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 SeerRobokitAgv::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, "SEER Robokit map file is empty: " + file_name); - } - - if (contentLooksLikeZip(content)) { - return parseSeerRobokitMapArchive_(file_name, content, options, updates); - } - - if (contentLooksLikeJson(content)) { - if (wants2D(options.dimension)) { - AgvUnifiedMapUpdate update; - const auto result = parseSeerRobokitMap2D_(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 = parseSeerRobokitMap3D_(file_name, content, options, update); - if (!result.ok()) { - return result; - } - updates.push_back(std::move(update)); - return AgvResult::success(); - } - - return AgvResult::success(); -} - -AgvResult SeerRobokitAgv::parseSeerRobokitMapArchive_( - 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 SEER Robokit 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 = parseSeerRobokitMap2D_(file_name, map2d_content, options, update); - if (result.ok()) { - updates.push_back(std::move(update)); - } else { - CMVR_LOG(ERROR) << "[SeerRobokitAgv] 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 = parseSeerRobokitMap3D_(file_name, map3d_content, options, update); - if (result.ok()) { - updates.push_back(std::move(update)); - } else { - CMVR_LOG(ERROR) << "[SeerRobokitAgv] Parse 0.3dsmap failed" - << ", id=" << id_ - << ", file=" << file_name - << ", error=" << result.message; - } - } - } - - fs::remove_all(temp_dir); - return updates.empty() - ? AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit smap archive has no requested map data: " + file_name) - : AgvResult::success(); -} - -AgvResult SeerRobokitAgv::parseSeerRobokitMap2D_( - 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 SEER Robokit 2D map json failed: " + error); - } - if (!root.isObject()) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit 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 SeerRobokitAgv::parseSeerRobokitMap3D_( - 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 SEER Robokit 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 SeerRobokitAgv::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 SeerRobokitAgv::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 SeerRobokitAgv::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 SeerRobokitAgv::stopMapping() -{ - auto result = ensureOtherSocket_(); - if (!result.ok()) return result; - - Json::Value response; - std::uint64_t accepted_generation = 0; - result = sendControlledCommand_( - sock_other_, - kRobotOtherStopMapping, - Json::Value(Json::objectValue), - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp deleted file mode 100644 index edf22ead..00000000 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp +++ /dev/null @@ -1,1440 +0,0 @@ -#include "seer_robokit_agv.h" -#include "seer_robokit_navigation_utils.h" -#include "seer_robokit_pgv_utils.h" -#include "seer_robokit_protocol.h" -#include "seer_robokit_utils.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::device { - -using namespace seer_robokit::navigation; -using namespace seer_robokit::protocol; -using namespace seer_robokit::detail; - -AgvResult SeerRobokitAgv::emergencyStop() -{ - return emergencyStopTrackedNavigation_(nullptr); -} - -AgvResult SeerRobokitAgv::emergencyStopTrackedNavigation_( - const TrackedNavigationContext* expected_navigation) -{ - // This is a controller-level software stop, not a substitute for the - // physical emergency-stop circuit. Keep both stop commands under one - // authority acquisition so no other command from this process can - // interleave between them. - std::lock_guard sequence_lock(control_sequence_mutex_); - TrackedNavigationContext tracked_navigation; - const bool has_tracked_navigation = - currentTrackedNavigation_(tracked_navigation); - if (expected_navigation) { - const bool same_navigation_identity = has_tracked_navigation - && tracked_navigation.token == expected_navigation->token - && tracked_navigation.type == expected_navigation->type - && tracked_navigation.task_ids == expected_navigation->task_ids - && tracked_navigation.target_id == expected_navigation->target_id - && tracked_navigation.target_ids - == expected_navigation->target_ids; - if (!same_navigation_identity) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit did not issue the tracked fail-safe software stop " - "because the navigation identity was replaced before the " - "control lock was acquired"); - } - } - const bool clearing_path_queue = has_tracked_navigation - && tracked_navigation.type == AgvTaskType::FollowPath; - const bool preserve_synchronous_wait = has_tracked_navigation - && tracked_navigation.synchronous_wait; - const auto navigation_stop_command = clearing_path_queue - ? kRobotTaskClearTargetList - : kRobotTaskCancel; - const auto stop_attempt_sequence = - control_attempt_sequence_.fetch_add( - 1, - std::memory_order_relaxed) + 1; - - const auto authority = acquireControl_(); - if (!authority.ok()) { - const std::string detail = authority.message.empty() ? "unknown error" : authority.message; - return AgvResult::failure( - authority.code, - "SEER Robokit acquire control authority failed: " + detail); - } - - struct StopOutcome { - AgvResult result; - bool controller_outcome_unknown{false}; - }; - const auto send_stop = [this](const int sock, const std::uint16_t command) { - Json::Value response; - auto result = sendCommand_( - sock, - command, - Json::Value(Json::objectValue), - &response); - if (!result.ok()) { - return StopOutcome{ - withUnknownControllerOutcome(std::move(result)), - true}; - } - if (!hasNumericControllerRetCode(response)) { - return StopOutcome{ - withUnknownControllerOutcome(resultFromResponse_(response)), - true}; - } - return StopOutcome{resultFromResponse_(response), false}; - }; - - bool generation_advanced = false; - const auto advance_generation_if_needed = [this, &generation_advanced]( - const StopOutcome& outcome) { - if (!generation_advanced - && (outcome.result.ok() || outcome.controller_outcome_unknown)) { - // Publish immediately after the first accepted or indeterminate stop - // outcome. Waiting for the second stop response would leave a window - // in which pose-start confirmation could incorrectly return success. - navigation_generation_.fetch_add(1, std::memory_order_relaxed); - generation_advanced = true; - } - }; - - const auto motion_stop = send_stop(sock_control_, kRobotControlStop); - advance_generation_if_needed(motion_stop); - const auto navigation_cancel = send_stop( - sock_navigation_, - navigation_stop_command); - advance_generation_if_needed(navigation_cancel); - if (generation_advanced) { - const auto stop_generation = - navigation_generation_.load(std::memory_order_relaxed); - if (preserve_synchronous_wait) { - advancePoseTaskGeneration_( - stop_generation, - stop_attempt_sequence); - advanceTrackedNavigationGeneration_(stop_generation); - } else { - clearPoseTask_(stop_generation); - clearTrackedNavigation_(stop_generation); - } - } - - if (!motion_stop.result.ok()) { - const std::string detail = motion_stop.result.message.empty() - ? "unknown error" - : motion_stop.result.message; - if (!navigation_cancel.result.ok()) { - const std::string cancel_detail = navigation_cancel.result.message.empty() - ? "unknown error" - : navigation_cancel.result.message; - return AgvResult::failure( - motion_stop.result.code, - "SEER Robokit software stop failed: control stop: " + detail - + "; cancel navigation: " + cancel_detail); - } - return AgvResult::failure( - motion_stop.result.code, - "SEER Robokit software stop failed: control stop: " + detail); - } - if (!navigation_cancel.result.ok()) { - const std::string detail = navigation_cancel.result.message.empty() - ? "unknown error" - : navigation_cancel.result.message; - return AgvResult::failure( - navigation_cancel.result.code, - "SEER Robokit software stop failed: cancel navigation: " + detail); - } - return AgvResult::success(); -} - -AgvResult SeerRobokitAgv::clearFault() -{ - return AgvResult::failure( - AgvErrorCode::UnsupportedCommand, - "SEER Robokit clearFault command is not implemented"); -} - -AgvResult SeerRobokitAgv::navigateToPose( - const math::Pose2d& pose, - const AgvMotionOptions& options, - const AgvAdapterParams& adapter_params) -{ - if (!std::isfinite(pose.x) - || !std::isfinite(pose.y) - || !std::isfinite(pose.theta)) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit free-navigation pose x, y, and theta must be finite"); - } - if (const std::string error = invalidMotionOption(options); - !error.empty()) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit free-navigation motion option " + error); - } - if (navigationCancellationRequested(options)) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit free-navigation command was not sent because the caller " - "had already canceled the operation"); - } - // This SEER Robokit firmware exposes arbitrary-pose navigation through the - // vendor-specific freeGo extension of API 3051. The empty target id and - // GotoSpecifiedPose skill are part of the controller payload that was - // validated on the differential-drive chassis. - std::string source_id = adapter_params.getString("source_id").value_or("SELF_POSITION"); - if (source_id.empty()) { - source_id = "SELF_POSITION"; - } - if (source_id != "SELF_POSITION") { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit free navigation source_id must be SELF_POSITION"); - } - const std::string requested_target_id = - adapter_params.getString("target_id").value_or(""); - if (!requested_target_id.empty() - && requested_target_id != "SELF_POSITION") { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit free navigation target_id must be empty or SELF_POSITION so a " - "malformed freeGo request cannot fall back to station navigation"); - } - const std::string target_id; - - std::string skill_name = - adapter_params.getString("skill_name").value_or("GotoSpecifiedPose"); - if (skill_name.empty()) { - skill_name = "GotoSpecifiedPose"; - } - if (skill_name != "GotoSpecifiedPose") { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit free navigation skill_name must be GotoSpecifiedPose"); - } - if (!options.asynchronous) { - NavigationSnapshot snapshot; - const auto snapshot_result = queryNavigationSnapshot_(snapshot); - if (!snapshot_result.ok()) { - return AgvResult::failure( - snapshot_result.code, - "SEER Robokit free-navigation command was not sent because the " - "required 1101 safety/status preflight failed: " - + snapshot_result.message); - } - if (snapshot.blocked || snapshot.emergency - || !snapshot.active_faults.empty()) { - return AgvResult::failure( - snapshot.emergency - ? AgvErrorCode::EmergencyStopped - : AgvErrorCode::Fault, - "SEER Robokit free-navigation command was not sent because the " - "controller is not safe to start: " + snapshot.detail); - } - } - - const auto task_sequence = - pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; - std::string task_id_prefix = - adapter_params.getString("task_id").value_or(id_); - if (task_id_prefix.empty()) { - task_id_prefix = id_; - } - const std::string task_id = - makePoseTaskId(task_id_prefix, task_sequence); - - Json::Value payload(Json::objectValue); - jsonMember(payload, "source_id") = source_id; - jsonMember(payload, "id") = target_id; - jsonMember(payload, "task_id") = task_id; - jsonMember(payload, "skill_name") = skill_name; - - auto& free_go = jsonMember(payload, "freeGo"); - jsonMember(free_go, "x") = pose.x; - jsonMember(free_go, "y") = pose.y; - jsonMember(free_go, "theta") = pose.theta; - - // Only strongly typed motion fields and the string whitelist above are - // accepted here. Generic adapter passthrough could inject unrelated 3051 - // operations such as lift, fork, script, or digital-I/O actions. - applyMotionOptions_(payload, options); - - PoseTaskContext context; - context.task_id = task_id; - context.target = pose; - context.reach_distance = options.reach_distance > 0.0 - ? options.reach_distance - : kDefaultPoseReachDistance; - context.reach_angle = options.reach_angle > 0.0 - ? options.reach_angle - : kDefaultPoseReachAngle; - TrackedNavigationContext navigation_context; - navigation_context.token = task_id; - navigation_context.task_ids = {task_id}; - navigation_context.type = AgvTaskType::NavigateToPose; - navigation_context.synchronous_wait = !options.asynchronous; - - Json::Value response; - std::uint64_t navigation_generation = 0; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskGoTarget, - payload, - &response, - &navigation_generation, - nullptr, - nullptr, - &context, - true, - &navigation_context, - false, - nullptr, - &options.cancellation_requested); - const AgvResult command_result = result; - const bool reconcile_indeterminate_command = !result.ok() - && navigation_generation != 0 - && !options.asynchronous; - if (!result.ok() && !reconcile_indeterminate_command) { - return result; - } - result = confirmPoseNavigationStarted_( - context, - !options.asynchronous, - options); - if (!result.ok()) { - TrackedNavigationContext active_navigation; - const bool same_token_still_current = - currentTrackedNavigation_(active_navigation) - && active_navigation.token == navigation_context.token; - if (!same_token_still_current) { - // A pause, resume, explicit cancel, emergency stop, or replacement - // navigation has already ordered the controller state after this - // command. Never let the older confirmation path issue another - // global cancellation against that newer state. - return reconciledNavigationResult( - command_result, - std::move(result)); - } - if (active_navigation.navigation_generation - != navigation_context.navigation_generation - || navigation_generation_.load(std::memory_order_relaxed) - != navigation_context.navigation_generation) { - // A cancel/stop/pause command preserved this exact token but - // advanced its control epoch. A synchronous caller must reconcile - // the exact task and two zero-velocity samples instead of issuing - // a late second cancel. Preserve the established asynchronous - // contract: start confirmation returns superseded immediately. - if (options.asynchronous) { - return reconciledNavigationResult( - command_result, - std::move(result)); - } - return reconciledNavigationResult( - command_result, - waitForPoseNavigationTerminal_( - context, - active_navigation, - options)); - } - auto failure = failAndCancelTrackedNavigation_( - navigation_context, - options, - result.code, - "SEER Robokit free-navigation start confirmation failed after the " - "controller accepted the command: " - + result.message); - return reconciledNavigationResult(command_result, std::move(failure)); - } - if (options.asynchronous) { - return result; - } - TrackedNavigationContext terminal_navigation_context = - navigation_context; - TrackedNavigationContext latest_navigation_context; - if (currentTrackedNavigation_(latest_navigation_context) - && latest_navigation_context.token == navigation_context.token) { - terminal_navigation_context = latest_navigation_context; - } - return reconciledNavigationResult( - command_result, - waitForPoseNavigationTerminal_( - context, - terminal_navigation_context, - options)); -} - -AgvResult SeerRobokitAgv::navigateToStation( - const std::string& station_id, - const AgvMotionOptions& options, - const AgvAdapterParams& adapter_params) -{ - if (station_id.empty()) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit station-navigation station_id must not be empty"); - } - if (const std::string error = invalidMotionOption(options); - !error.empty()) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit station-navigation motion option " + error); - } - if (navigationCancellationRequested(options)) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit station-navigation command was not sent because the " - "caller had already canceled the operation"); - } - if (const auto jack_height = adapter_params.getString("jack_height")) { - double parsed_jack_height = 0.0; - if (!parseFiniteDouble(*jack_height, parsed_jack_height)) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit station-navigation adapter jack_height must be a " - "complete finite number"); - } - } - - Json::Value payload(Json::objectValue); - if (const auto error = seer_robokit::pgv::applyPgvAdjustmentParams( - payload, - adapter_params); - !error.empty()) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit station-navigation PGV adapter parameter " + error); - } - if (!options.asynchronous) { - NavigationSnapshot snapshot; - const auto snapshot_result = queryNavigationSnapshot_(snapshot); - if (!snapshot_result.ok()) { - return AgvResult::failure( - snapshot_result.code, - "SEER Robokit station-navigation command was not sent because " - "the required 1101 safety/status preflight failed: " - + snapshot_result.message); - } - if (snapshot.blocked || snapshot.emergency - || !snapshot.active_faults.empty()) { - return AgvResult::failure( - snapshot.emergency - ? AgvErrorCode::EmergencyStopped - : AgvErrorCode::Fault, - "SEER Robokit station-navigation command was not sent because " - "the controller is not safe to start: " + snapshot.detail); - } - } - - jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); - jsonMember(payload, "id") = station_id; - applyAdapterParams_(payload, adapter_params); - const auto task_sequence = - pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; - std::string task_id_prefix = - adapter_params.getString("task_id").value_or(id_); - if (task_id_prefix.empty()) { - task_id_prefix = id_; - } - const std::string task_id = makeNavigationTaskId( - task_id_prefix, - "station", - task_sequence); - // Always replace a caller-supplied reusable id with a unique id derived - // from it so 1110 cannot report a stale completion from an older request. - jsonMember(payload, "task_id") = task_id; - // Canonical typed motion options must win over string-valued adapter - // extensions so the SRC controller receives JSON numbers. - applyMotionOptions_(payload, options); - Json::Value response; - std::uint64_t accepted_generation = 0; - TrackedNavigationContext navigation_context; - navigation_context.token = task_id; - navigation_context.task_ids = {task_id}; - navigation_context.type = AgvTaskType::NavigateToStation; - navigation_context.target_id = station_id; - navigation_context.target_ids = {station_id}; - navigation_context.synchronous_wait = !options.asynchronous; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskGoTarget, - payload, - &response, - &accepted_generation, - nullptr, - nullptr, - nullptr, - false, - &navigation_context, - false, - nullptr, - &options.cancellation_requested); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - const AgvResult command_result = result; - const bool reconcile_indeterminate_command = !result.ok() - && accepted_generation != 0 - && !options.asynchronous; - if ((!result.ok() && !reconcile_indeterminate_command) - || options.asynchronous) { - return result; - } - return reconciledNavigationResult( - command_result, - waitForTrackedNavigationTerminal_( - navigation_context, - options)); -} - -AgvResult SeerRobokitAgv::followPath( - const std::vector& path) -{ - return followPath(path, AgvMotionOptions{}); -} - -AgvResult SeerRobokitAgv::followPath( - const std::vector& path, - const AgvMotionOptions& options) -{ - if (path.empty()) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit path navigation requires at least one segment"); - } - if (const std::string error = invalidMotionOption(options); - !error.empty()) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit path-navigation motion option " + error); - } - if (navigationCancellationRequested(options)) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit path-navigation command was not sent because the caller " - "had already canceled the operation"); - } - for (const auto& segment : path) { - if (segment.source_station.empty() - || segment.target_station.empty()) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit path-navigation source and target station ids " - "must not be empty"); - } - } - if (!options.asynchronous) { - NavigationSnapshot snapshot; - const auto snapshot_result = queryNavigationSnapshot_(snapshot); - if (!snapshot_result.ok()) { - return AgvResult::failure( - snapshot_result.code, - "SEER Robokit path-navigation command was not sent because the " - "required 1101 safety/status preflight failed: " - + snapshot_result.message); - } - if (snapshot.blocked || snapshot.emergency - || !snapshot.active_faults.empty()) { - return AgvResult::failure( - snapshot.emergency - ? AgvErrorCode::EmergencyStopped - : AgvErrorCode::Fault, - "SEER Robokit path-navigation command was not sent because the " - "controller is not safe to start: " + snapshot.detail); - } - } - - const auto task_sequence = - pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; - const std::string batch_id = makeNavigationTaskId( - id_, - "path", - task_sequence); - Json::Value payload(Json::objectValue); - Json::Value tasks(Json::arrayValue); - std::vector task_ids; - task_ids.reserve(path.size()); - std::size_t index = 0; - for (const auto& segment : path) { - Json::Value task(Json::objectValue); - const std::string task_id = - batch_id + "_segment_" + std::to_string(index++); - jsonMember(task, "task_id") = task_id; - jsonMember(task, "source_id") = segment.source_station; - jsonMember(task, "id") = segment.target_station; - tasks.append(task); - task_ids.push_back(task_id); - } - jsonMember(payload, "move_task_list") = tasks; - Json::Value response; - std::uint64_t accepted_generation = 0; - TrackedNavigationContext navigation_context; - navigation_context.token = batch_id; - navigation_context.task_ids = task_ids; - navigation_context.type = AgvTaskType::FollowPath; - navigation_context.target_id = path.back().target_station; - navigation_context.target_ids.reserve(path.size()); - for (const auto& segment : path) { - navigation_context.target_ids.push_back(segment.target_station); - } - navigation_context.synchronous_wait = !options.asynchronous; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskGoTargetList, - payload, - &response, - &accepted_generation, - nullptr, - nullptr, - nullptr, - false, - &navigation_context, - false, - nullptr, - &options.cancellation_requested); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - const AgvResult command_result = result; - const bool reconcile_indeterminate_command = !result.ok() - && accepted_generation != 0 - && !options.asynchronous; - if ((!result.ok() && !reconcile_indeterminate_command) - || options.asynchronous) { - return result; - } - return reconciledNavigationResult( - command_result, - waitForTrackedNavigationTerminal_( - navigation_context, - options)); -} - - -AgvResult SeerRobokitAgv::translate( - const AgvTranslation& translation) -{ - if (!std::isfinite(translation.distance) - || !std::isfinite(translation.vx) - || !std::isfinite(translation.vy)) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit translation distance, vx, and vy must be finite"); - } - if (translation.distance <= 0.0) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit translation distance must be greater than zero"); - } - if (translation.vx == 0.0 && translation.vy == 0.0) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit translation requires a non-zero vx or vy"); - } - - int mode = 0; - switch (translation.mode) { - case AgvTranslationMode::Odometry: - mode = 0; - break; - case AgvTranslationMode::Localization: - mode = 1; - break; - default: - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit translation mode must be odometry or localization"); - } - - Json::Value payload(Json::objectValue); - jsonMember(payload, "dist") = translation.distance; - jsonMember(payload, "vx") = translation.vx; - jsonMember(payload, "vy") = translation.vy; - jsonMember(payload, "mode") = mode; - - Json::Value response; - std::uint64_t accepted_generation = 0; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskTranslate, - payload, - &response, - &accepted_generation); - - // API 3055 does not expose a task id. Do not publish a fake tracked task, - // but invalidate navigation state if the command may have been accepted. - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - - - - -AgvResult SeerRobokitAgv::pauseNavigation() -{ - Json::Value response; - std::uint64_t accepted_generation = 0; - std::uint64_t control_attempt_sequence = 0; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskPause, - Json::Value(Json::objectValue), - &response, - &accepted_generation, - nullptr, - &control_attempt_sequence, - nullptr, - false, - nullptr, - true); - if (accepted_generation != 0) { - advancePoseTaskGeneration_( - accepted_generation, - control_attempt_sequence); - } - return result; -} - -AgvResult SeerRobokitAgv::resumeNavigation() -{ - Json::Value response; - std::uint64_t accepted_generation = 0; - std::uint64_t control_attempt_sequence = 0; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskResume, - Json::Value(Json::objectValue), - &response, - &accepted_generation, - nullptr, - &control_attempt_sequence, - nullptr, - false, - nullptr, - true); - if (accepted_generation != 0) { - advancePoseTaskGeneration_( - accepted_generation, - control_attempt_sequence); - } - return result; -} - -AgvResult SeerRobokitAgv::cancelNavigation() -{ - Json::Value response; - std::uint64_t accepted_generation = 0; - std::uint64_t control_attempt_sequence = 0; - TrackedNavigationContext tracked_navigation; - const bool has_tracked_navigation = - currentTrackedNavigation_(tracked_navigation); - const auto cancel_command = has_tracked_navigation - && tracked_navigation.type == AgvTaskType::FollowPath - ? kRobotTaskClearTargetList - : kRobotTaskCancel; - auto result = sendControlledCommand_( - sock_navigation_, - cancel_command, - Json::Value(Json::objectValue), - &response, - &accepted_generation, - nullptr, - &control_attempt_sequence, - nullptr, - false, - nullptr, - true, - has_tracked_navigation ? &tracked_navigation.token : nullptr, - nullptr, - has_tracked_navigation ? &tracked_navigation : nullptr); - if (accepted_generation != 0) { - advancePoseTaskGeneration_( - accepted_generation, - control_attempt_sequence); - } - return result; -} - -AgvResult SeerRobokitAgv::setVelocity(const AgvVelocity& velocity) -{ - if (!std::isfinite(velocity.vx) - || !std::isfinite(velocity.vy) - || !std::isfinite(velocity.wz)) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit velocity vx, vy, and wz must be finite"); - } - - Json::Value payload(Json::objectValue); - jsonMember(payload, "vx") = velocity.vx; - jsonMember(payload, "vy") = velocity.vy; - jsonMember(payload, "w") = velocity.wz; - Json::Value response; - const bool stop_velocity = - velocity.vx == 0.0 && velocity.vy == 0.0 && velocity.wz == 0.0; - if (stop_velocity) { - std::uint64_t control_attempt_sequence = 0; - auto result = sendControlledCommand_( - sock_control_, - kRobotControlMotion, - payload, - &response, - nullptr, - nullptr, - &control_attempt_sequence); - result = result.ok() ? resultFromResponse_(response) : result; - if (result.ok()) { - advancePoseTaskControlAttempt_(control_attempt_sequence); - } - return result; - } - - std::uint64_t accepted_generation = 0; - auto result = sendControlledCommand_( - sock_control_, - kRobotControlMotion, - payload, - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - -void SeerRobokitAgv::rememberPoseTask_(const PoseTaskContext& context) const -{ - std::lock_guard lock(pose_task_mutex_); - if (context.navigation_generation - < pose_task_context_.navigation_generation) { - return; - } - pose_task_context_ = context; -} - -void SeerRobokitAgv::advancePoseTaskGeneration_( - const std::uint64_t navigation_generation, - const std::uint64_t control_attempt_sequence) const -{ - std::lock_guard lock(pose_task_mutex_); - if (navigation_generation - < pose_task_context_.navigation_generation) { - return; - } - pose_task_context_.navigation_generation = navigation_generation; - pose_task_context_.control_attempt_sequence_at_start = - control_attempt_sequence; -} - -void SeerRobokitAgv::advancePoseTaskControlAttempt_( - const std::uint64_t control_attempt_sequence) const -{ - std::lock_guard lock(pose_task_mutex_); - if (pose_task_context_.task_id.empty() - || control_attempt_sequence - < pose_task_context_.control_attempt_sequence_at_start) { - return; - } - pose_task_context_.control_attempt_sequence_at_start = - control_attempt_sequence; -} - -void SeerRobokitAgv::clearPoseTask_( - const std::uint64_t navigation_generation) const -{ - std::lock_guard lock(pose_task_mutex_); - if (navigation_generation < pose_task_context_.navigation_generation) { - return; - } - pose_task_context_ = PoseTaskContext{}; - pose_task_context_.navigation_generation = navigation_generation; -} - -void SeerRobokitAgv::clearPoseTaskIfTaskId_(const std::string& task_id) const -{ - std::lock_guard lock(pose_task_mutex_); - if (pose_task_context_.task_id != task_id) { - return; - } - const auto generation = pose_task_context_.navigation_generation; - pose_task_context_ = PoseTaskContext{}; - pose_task_context_.navigation_generation = generation; -} - -bool SeerRobokitAgv::currentPoseTask_(PoseTaskContext& context) const -{ - std::lock_guard lock(pose_task_mutex_); - if (pose_task_context_.task_id.empty()) { - return false; - } - context = pose_task_context_; - return true; -} - -void SeerRobokitAgv::rememberTrackedNavigation_( - const TrackedNavigationContext& context) const -{ - std::lock_guard lock(tracked_navigation_mutex_); - if (context.navigation_generation - < tracked_navigation_context_.navigation_generation) { - return; - } - tracked_navigation_context_ = context; -} - -void SeerRobokitAgv::advanceTrackedNavigationGeneration_( - const std::uint64_t navigation_generation) const -{ - std::lock_guard lock(tracked_navigation_mutex_); - if (navigation_generation - < tracked_navigation_context_.navigation_generation) { - return; - } - tracked_navigation_context_.navigation_generation = - navigation_generation; -} - -void SeerRobokitAgv::clearTrackedNavigation_( - const std::uint64_t navigation_generation) const -{ - std::lock_guard lock(tracked_navigation_mutex_); - if (navigation_generation - < tracked_navigation_context_.navigation_generation) { - return; - } - tracked_navigation_context_ = TrackedNavigationContext{}; - tracked_navigation_context_.navigation_generation = - navigation_generation; -} - -void SeerRobokitAgv::clearTrackedNavigationIfToken_( - const std::string& token) const -{ - std::lock_guard lock(tracked_navigation_mutex_); - if (tracked_navigation_context_.token != token) { - return; - } - const auto generation = - tracked_navigation_context_.navigation_generation; - tracked_navigation_context_ = TrackedNavigationContext{}; - tracked_navigation_context_.navigation_generation = generation; -} - -bool SeerRobokitAgv::currentTrackedNavigation_( - TrackedNavigationContext& context) const -{ - std::lock_guard lock(tracked_navigation_mutex_); - if (tracked_navigation_context_.token.empty()) { - return false; - } - context = tracked_navigation_context_; - return true; -} - -std::string SeerRobokitAgv::cachedControllerFaultDetail_( - const std::uint64_t after_sequence, - const int wait_ms, - std::uint64_t* associated_control_attempt) const -{ - std::unique_lock lock(runtime_state_mutex_); - const auto has_matching_fault = [this, after_sequence]() { - return !last_controller_fault_detail_.empty() - && controller_fault_sequence_ > after_sequence; - }; - if (!has_matching_fault() - && wait_ms > 0 - && state_push_enabled_) { - runtime_state_cv_.wait_for( - lock, - std::chrono::milliseconds(wait_ms), - has_matching_fault); - } - if (!has_matching_fault()) { - return {}; - } - if (associated_control_attempt) { - *associated_control_attempt = - last_controller_fault_control_attempt_; - } - return "cached_controller_fault_at=" - + std::to_string(last_controller_fault_timestamp_) - + ", " + last_controller_fault_detail_; -} - -int SeerRobokitAgv::controllerFaultCaptureGraceMs_() const -{ - const int configured_interval = config_.state_push_interval_ms(); - const int effective_interval = configured_interval > 0 - ? configured_interval - : kDefaultControllerFaultPushIntervalMs; - const auto configured_grace = - static_cast(effective_interval) - + kControllerFaultPushJitterMs; - return static_cast(std::min( - std::max( - configured_grace, - static_cast(kMinimumControllerFaultCaptureGraceMs)), - static_cast(kMaximumControllerFaultCaptureGraceMs))); -} - -int SeerRobokitAgv::controllerFaultStateMaxAgeMs_() const -{ - const int configured_interval = config_.state_push_interval_ms(); - const int effective_interval = configured_interval > 0 - ? std::min( - configured_interval, - kMaximumControllerFaultCaptureGraceMs - - kControllerFaultPushJitterMs) - : kDefaultControllerFaultPushIntervalMs; - const auto max_age = - static_cast(effective_interval) - * kControllerFaultStateMaxAgeIntervals - + kControllerFaultPushJitterMs; - return static_cast(std::max( - max_age, - static_cast(kMinimumControllerFaultStateMaxAgeMs))); -} - -std::string SeerRobokitAgv::freeNavigationFaultStateUnavailableDetail_() const -{ - std::lock_guard lock(runtime_state_mutex_); - if (!state_push_enabled_) { - return "controller fault state is unavailable because state push is " - "disabled"; - } - // A newly reported active fault is handled through the sequenced fault - // cache, including its raw fatals/errors payload. Do not replace that - // diagnostic with the less specific "incomplete push" message. - if (!active_controller_fault_detail_.empty()) { - return {}; - } - if (!controller_fault_state_observed_) { - return "no complete state push containing fatals/errors is currently " - "available"; - } - const auto fault_state_age = - std::chrono::duration_cast( - std::chrono::steady_clock::now() - - controller_fault_state_observed_at_) - .count(); - const int max_age_ms = controllerFaultStateMaxAgeMs_(); - if (fault_state_age > max_age_ms) { - return "the most recent fatals/errors state push is stale (age_ms=" - + std::to_string(fault_state_age) - + ", max_age_ms=" + std::to_string(max_age_ms) + ")"; - } - return {}; -} - -AgvResult SeerRobokitAgv::queryPoseTaskStatus_( - const std::string& task_id, - PoseTaskStatus& status) const -{ - std::vector statuses; - const auto result = queryTaskStatuses_({task_id}, statuses); - if (!result.ok()) { - status = PoseTaskStatus{}; - return result; - } - status = std::move(statuses.front()); - return result; -} - -AgvResult SeerRobokitAgv::queryTaskStatuses_( - const std::vector& requested_task_ids, - std::vector& statuses) const -{ - statuses.assign(requested_task_ids.size(), PoseTaskStatus{}); - if (requested_task_ids.empty()) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit 1110 task status query requires at least one task id"); - } - - Json::Value payload(Json::objectValue); - Json::Value task_ids(Json::arrayValue); - for (const auto& task_id : requested_task_ids) { - task_ids.append(task_id); - } - jsonMember(payload, "task_ids") = std::move(task_ids); - - Json::Value response; - auto result = sendCommand_( - sock_status_, - kRobotStatusTaskPackage, - payload, - &response); - if (!result.ok()) { - return result; - } - result = resultFromResponse_(response); - if (!result.ok()) { - return result; - } - - const auto* package = jsonFind(response, "task_status_package"); - const double progress = package - ? jsonGet(*package, "percentage", 0.0).asDouble() - : 0.0; - if (package) { - if (const auto* status_list = jsonFind(*package, "task_status_list"); - status_list && status_list->isArray()) { - for (const auto& item : *status_list) { - const std::string returned_task_id = - jsonGet(item, "task_id", "").asString(); - const auto requested = std::find( - requested_task_ids.begin(), - requested_task_ids.end(), - returned_task_id); - if (requested == requested_task_ids.end()) { - continue; - } - const auto index = static_cast( - std::distance(requested_task_ids.begin(), requested)); - auto& status = statuses[index]; - status.found = true; - status.state = jsonGet(item, "status", 0).asInt(); - if (const auto* type = jsonFind(item, "type"); - type && type->isNumeric()) { - status.type = type->asInt(); - status.type_present = true; - } - } - } - } - - std::ostringstream common_detail; - if (const auto* ret_code = jsonFind(response, "ret_code")) { - common_detail << ", status_query_ret_code=" - << jsonValueToString(*ret_code); - } - const auto append_field = [&common_detail]( - const Json::Value& object, - const char* key, - const char* label) { - const auto* value = jsonFind(object, key); - if (!value || value->isNull()) { - return; - } - const std::string text = jsonValueToString(*value); - if (!text.empty()) { - common_detail << ", " << label << "=" << text; - } - }; - if (package) { - append_field(*package, "info", "info"); - append_field(*package, "closest_target", "closest_target"); - append_field(*package, "source_name", "source_name"); - append_field(*package, "target_name", "target_name"); - append_field(*package, "percentage", "percentage"); - append_field(*package, "distance", "distance"); - } - append_field(response, "create_on", "create_on"); - append_field(response, "err_msg", "status_query_err_msg"); - - for (std::size_t index = 0; index < requested_task_ids.size(); ++index) { - auto& status = statuses[index]; - status.progress = progress; - std::ostringstream detail; - detail << "task_id=" << requested_task_ids[index]; - if (status.found) { - detail << ", task_status=" << status.state; - if (status.type_present) { - detail << ", task_type=" << status.type; - } else { - detail << ", task_type="; - } - } else { - detail << " not present in task_status_package"; - } - detail << common_detail.str(); - status.detail = detail.str(); - } - return AgvResult::success(); -} - -AgvResult SeerRobokitAgv::queryNavigationSnapshot_( - NavigationSnapshot& snapshot) const -{ - snapshot = NavigationSnapshot{}; - - Json::Value response; - auto result = sendCommand_( - sock_status_, - kRobotStatusAll2, - Json::Value(Json::objectValue), - &response); - if (!result.ok()) { - return result; - } - result = resultFromResponse_(response); - if (!result.ok()) { - return result; - } - - const auto* task_status = jsonFind(response, "task_status"); - const auto* task_type = jsonFind(response, "task_type"); - const auto* blocked = jsonFind(response, "blocked"); - const auto* vx = jsonFind(response, "vx"); - const auto* vy = jsonFind(response, "vy"); - const auto* w = jsonFind(response, "w"); - if (!w) { - w = jsonFind(response, "wz"); - } - - if (!task_status || !task_status->isNumeric() - || !task_type || !task_type->isNumeric()) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit 1101 navigation snapshot did not contain numeric " - "task_status/task_type"); - } - if (!blocked || !(blocked->isBool() || blocked->isNumeric())) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit 1101 navigation snapshot did not contain blocked"); - } - if (!vx || !vx->isNumeric() - || !vy || !vy->isNumeric() - || !w || !w->isNumeric()) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit 1101 navigation snapshot did not contain numeric " - "vx/vy/w required to confirm that navigation has stopped"); - } - - snapshot.task_status = task_status->asInt(); - snapshot.task_type = task_type->asInt(); - snapshot.task_status_present = true; - snapshot.task_type_present = true; - snapshot.blocked = blocked->asBool(); - snapshot.blocked_present = true; - snapshot.vx = vx->asDouble(); - snapshot.vy = vy->asDouble(); - snapshot.w = w->asDouble(); - snapshot.velocity_present = - std::isfinite(snapshot.vx) - && std::isfinite(snapshot.vy) - && std::isfinite(snapshot.w); - if (!snapshot.velocity_present) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit 1101 navigation snapshot contained non-finite vx/vy/w"); - } - - if (const auto* reason = jsonFind(response, "block_reason"); - reason && !reason->isNull()) { - snapshot.block_reason_raw = jsonValueToString(*reason); - if (reason->isNumeric()) { - snapshot.block_reason = reason->asInt(); - } - } - if (const auto* target_id = jsonFind(response, "target_id"); - target_id && target_id->isString()) { - snapshot.target_id = target_id->asString(); - } - if (const auto* emergency = jsonFind(response, "emergency"); - emergency && (emergency->isBool() || emergency->isNumeric())) { - snapshot.emergency = emergency->asBool(); - } - - std::ostringstream faults; - const auto append_faults = [&response, &faults](const char* key) { - const auto* value = jsonFind(response, key); - if (!value) { - return; - } - const bool malformed = !value->isArray(); - if (!malformed && value->empty()) { - return; - } - if (faults.tellp() > 0) { - faults << ", "; - } - faults << key << "=" - << (value->isNull() - ? std::string("null") - : jsonValueToString(*value)); - if (malformed) { - faults << "(malformed; expected array)"; - } - }; - append_faults("fatals"); - append_faults("errors"); - snapshot.active_faults = faults.str(); - - std::ostringstream detail; - detail << "1101 task_status=" << snapshot.task_status - << ", task_type=" << snapshot.task_type - << ", blocked=" << (snapshot.blocked ? "true" : "false") - << ", velocity=(" << snapshot.vx << "," << snapshot.vy - << "," << snapshot.w << ")"; - if (!snapshot.target_id.empty()) { - detail << ", target_id=" << snapshot.target_id; - } - if (!snapshot.block_reason_raw.empty()) { - detail << ", block_reason=" << snapshot.block_reason_raw; - if (snapshot.block_reason >= 0) { - detail << "(" << blockReasonName(snapshot.block_reason) << ")"; - } - } - const auto append_detail_field = [&response, &detail](const char* key) { - const auto* value = jsonFind(response, key); - if (!value || value->isNull()) { - return; - } - const std::string text = jsonValueToString(*value); - if (!text.empty()) { - detail << ", " << key << "=" << text; - } - }; - append_detail_field("block_x"); - append_detail_field("block_y"); - append_detail_field("block_di"); - append_detail_field("block_ultrasonic_id"); - append_detail_field("move_status_info"); - append_detail_field("err_msg"); - append_detail_field("warnings"); - if (!snapshot.active_faults.empty()) { - detail << ", " << snapshot.active_faults; - } - if (snapshot.emergency) { - detail << ", emergency=true"; - } - snapshot.detail = detail.str(); - return AgvResult::success(); -} - -bool SeerRobokitAgv::poseTargetReached_( - const PoseTaskContext& context, - std::string& detail) const -{ - math::Pose2d current_pose; - bool current_pose_available = false; - std::string pose_source; - std::string query_error; - - Json::Value response; - auto result = sendCommand_( - sock_status_, - kRobotStatusLoc, - Json::Value(Json::objectValue), - &response); - if (result.ok()) { - result = resultFromResponse_(response); - } - const auto* x = jsonFind(response, "x"); - const auto* y = jsonFind(response, "y"); - const auto* angle = jsonFind(response, "angle"); - if (result.ok() - && x && x->isNumeric() - && y && y->isNumeric() - && angle && angle->isNumeric()) { - current_pose.x = x->asDouble(); - current_pose.y = y->asDouble(); - current_pose.theta = angle->asDouble(); - if (std::isfinite(current_pose.x) - && std::isfinite(current_pose.y) - && std::isfinite(current_pose.theta)) { - current_pose_available = true; - pose_source = "controller_1004"; - } else { - query_error = "SEER Robokit 1004 response contained non-finite x/y/angle"; - } - } else if (!result.ok()) { - query_error = result.message; - } else { - query_error = - "SEER Robokit 1004 response did not contain numeric x/y/angle"; - } - - if (!current_pose_available) { - detail = "target pose could not be verified"; - if (!query_error.empty()) { - detail += ": " + query_error; - } - return false; - } - - const double distance_error = std::hypot( - current_pose.x - context.target.x, - current_pose.y - context.target.y); - const double angle_error = angleDistance( - current_pose.theta, - context.target.theta); - std::ostringstream description; - description << "pose_source=" << pose_source - << ", current_pose=(" << current_pose.x - << "," << current_pose.y - << "," << current_pose.theta - << "), target_pose=(" << context.target.x - << "," << context.target.y - << "," << context.target.theta - << "), distance_error=" << distance_error - << ", distance_tolerance=" << context.reach_distance - << ", angle_error=" << angle_error - << ", angle_tolerance=" << context.reach_angle; - if (!query_error.empty()) { - description << ", 1004_query_error=" << query_error; - } - detail = description.str(); - return distance_error <= context.reach_distance - && angle_error <= context.reach_angle; -} - -void SeerRobokitAgv::applyMotionOptions_( - Json::Value& payload, - const AgvMotionOptions& options, - const bool include_reach_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 (include_reach_options) { - 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 SeerRobokitAgv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params) -{ - for (const auto& [key, value] : params.values) { - if (seer_robokit::pgv::isPgvAdjustmentKey(key) - || key.rfind("port_", 0) == 0 - || key == "target_id" - || key == "id" - || key == "x" - || key == "y" - || key == "angle" - || key == "freeGo" - || key == "max_speed" - || key == "max_wspeed" - || key == "max_acc" - || key == "max_wacc" - || key == "reach_dist" - || key == "reach_angle" - || key == "jack_height") { - continue; - } - jsonMember(payload, key) = value; - } - if (const auto jack_height = params.getDouble("jack_height")) { - jsonMember(payload, "jack_height") = *jack_height; - } -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp deleted file mode 100644 index 62aaf35c..00000000 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp +++ /dev/null @@ -1,1619 +0,0 @@ -#include "seer_robokit_agv.h" -#include "seer_robokit_navigation_utils.h" -#include "seer_robokit_protocol.h" -#include "seer_robokit_utils.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::device { - -using namespace seer_robokit::navigation; -using namespace seer_robokit::protocol; -using namespace seer_robokit::detail; - -AgvResult SeerRobokitAgv::confirmPoseNavigationStarted_( - const PoseTaskContext& context, - const bool accept_paused, - const AgvMotionOptions& options) const -{ - const auto deadline = std::chrono::steady_clock::now() + kPoseNavigationStartTimeout; - std::string last_status = "no task status received"; - int consecutive_running_samples = 0; - bool matching_task_observed = false; - int last_matching_state = 0; - bool last_poll_matched = false; - bool running_stability_window_active = false; - std::chrono::steady_clock::time_point running_stable_at{}; - const auto running_stability_window = std::chrono::milliseconds( - controllerFaultCaptureGraceMs_()); - const auto hard_deadline = deadline + running_stability_window; - - const auto superseded = [this, &context, accept_paused]() { - if (!accept_paused) { - return navigation_generation_.load(std::memory_order_relaxed) - != context.navigation_generation; - } - TrackedNavigationContext active_context; - return !currentTrackedNavigation_(active_context) - || active_context.token != context.task_id; - }; - const auto superseded_result = []() { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit free-navigation start confirmation was superseded by " - "another accepted navigation, velocity, pause, or stop command; " - "the controller task state is unknown, so do not retry automatically " - "before querying or canceling navigation"); - }; - const auto fault_monitoring_unavailable = - [this, &context]() { - if (controller_fault_channel_epoch_.load( - std::memory_order_relaxed) - != context.controller_fault_channel_epoch_at_start) { - return std::string( - "the controller fault push channel changed or was " - "invalidated after the free-navigation command was " - "accepted"); - } - return freeNavigationFaultStateUnavailableDetail_(); - }; - const auto fault_monitoring_unavailable_result = - [](const std::string& detail) { - return AgvResult::failure( - AgvErrorCode::Fault, - "SEER Robokit accepted the free-navigation command, but controller " - "fault monitoring became unavailable during start " - "confirmation: " + detail - + "; the task state is unsafe to accept, so query the " - "controller and cancel or stop before another motion " - "command"); - }; - const auto fault_attribution_is_ambiguous = - [&context](const std::uint64_t associated_control_attempt) { - return associated_control_attempt != 0 - && associated_control_attempt - != context.control_attempt_sequence_at_start; - }; - const auto ambiguous_fault_result = - [](const std::string& task_detail, const std::string& fault) { - return AgvResult::failure( - AgvErrorCode::Fault, - "SEER Robokit reported a controller fault after another control " - "command attempt had begun; the fault " - "cannot be attributed to the tracked free-navigation task: " - + task_detail + ", " + fault - + "; query navigation status and cancel or stop before " - "another motion command"); - }; - const auto supersededWithFault = [&]() { - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - 0, - &fault_control_attempt); - if (!fault.empty() - && fault_attribution_is_ambiguous(fault_control_attempt)) { - return ambiguous_fault_result(last_status, fault); - } - return superseded_result(); - }; - - while (true) { - if (navigationCancellationRequested(options)) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit free-navigation start confirmation was canceled by " - "the caller"); - } - if (superseded()) { - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - 0, - &fault_control_attempt); - if (!fault.empty() - && fault_attribution_is_ambiguous( - fault_control_attempt)) { - return ambiguous_fault_result(last_status, fault); - } - return supersededWithFault(); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - - PoseTaskStatus task_status; - const auto query_result = queryPoseTaskStatus_( - context.task_id, - task_status); - if (!query_result.ok()) { - const std::string detail = query_result.message.empty() - ? "unknown error" - : query_result.message; - return AgvResult::failure( - query_result.code, - "SEER Robokit accepted the free-navigation command, but task start " - "could not be verified: " + detail - + "; do not retry automatically before checking or canceling navigation"); - } - - if (superseded()) { - return supersededWithFault(); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - - last_status = task_status.detail; - const bool task_present = - task_status.found && task_status.state != 404; - last_poll_matched = task_present; - if (task_present) { - matching_task_observed = true; - last_matching_state = task_status.state; - if (task_status.type_present && task_status.type != 1) { - return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SEER Robokit created an unexpected task type for free navigation: " - + last_status); - } - - if (task_status.state == 2) { - const auto now = std::chrono::steady_clock::now(); - if (!running_stability_window_active) { - running_stability_window_active = true; - running_stable_at = now + running_stability_window; - } - ++consecutive_running_samples; - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - 0, - &fault_control_attempt); - if (!fault.empty()) { - if (superseded()) { - return supersededWithFault(); - } - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - return ambiguous_fault_result(last_status, fault); - } - return AgvResult::failure( - AgvErrorCode::Fault, - "SEER Robokit free-navigation task reached Running state, " - "but the controller reported a new fault during start " - "confirmation: " + last_status + ", " + fault - + "; do not retry automatically; cancel or stop the " - "task before another motion command"); - } - if (consecutive_running_samples - >= kPoseNavigationRequiredRunningSamples - && now >= running_stable_at) { - if (superseded()) { - return supersededWithFault(); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result( - unavailable); - } - return AgvResult::success(); - } - } else { - consecutive_running_samples = 0; - running_stability_window_active = false; - } - - if (task_status.state == 3) { - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - controllerFaultCaptureGraceMs_(), - &fault_control_attempt); - if (superseded()) { - return supersededWithFault(); - } - if (!fault.empty()) { - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - return ambiguous_fault_result(last_status, fault); - } - return AgvResult::failure( - AgvErrorCode::Fault, - "SEER Robokit free-navigation task was established but " - "paused while the controller reported a new fault: " - + last_status + ", " + fault - + "; do not retry automatically before querying or " - "canceling it"); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - if (accept_paused) { - return AgvResult::success(); - } - return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SEER Robokit free-navigation task was established but is paused: " - + last_status - + "; do not retry automatically before querying or canceling it"); - } - if (task_status.state == 4) { - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - controllerFaultCaptureGraceMs_(), - &fault_control_attempt); - if (superseded()) { - return supersededWithFault(); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - if (!fault.empty()) { - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - return ambiguous_fault_result(last_status, fault); - } - return AgvResult::failure( - AgvErrorCode::Fault, - "SEER Robokit free-navigation task reported Completed, but " - "the controller reported a new fault during completion " - "confirmation: " + last_status + ", " + fault - + "; do not retry automatically; cancel or stop " - "the task before another motion command"); - } - if (accept_paused) { - // Synchronous callers perform the authoritative pose - // check only after two zero-velocity 1101 samples. A - // Completed task may still be decelerating here. - return AgvResult::success(); - } - std::string pose_detail; - const bool target_reached = - poseTargetReached_(context, pose_detail); - if (superseded()) { - return supersededWithFault(); - } - std::uint64_t post_pose_fault_control_attempt = 0; - const std::string post_pose_fault = - cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - 0, - &post_pose_fault_control_attempt); - if (!post_pose_fault.empty()) { - if (fault_attribution_is_ambiguous( - post_pose_fault_control_attempt)) { - return ambiguous_fault_result( - last_status, - post_pose_fault); - } - return AgvResult::failure( - AgvErrorCode::Fault, - "SEER Robokit free-navigation task reported Completed, but " - "the controller reported a new fault during target " - "verification: " + last_status + ", " - + post_pose_fault - + "; do not retry automatically; cancel or stop " - "the task before another motion command"); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - if (target_reached) { - return AgvResult::success(); - } - return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SEER Robokit free-navigation task completed before a stable running " - "state, but the requested target was not reached: " - + last_status + ", " + pose_detail - + "; check the freeGo payload and controller alarms before retrying"); - } - if (task_status.state == 5 - || task_status.state == 6 - || task_status.state == 7) { - std::string detail = last_status; - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - controllerFaultCaptureGraceMs_(), - &fault_control_attempt); - if (superseded()) { - return supersededWithFault(); - } - if (!fault.empty()) { - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - detail += - ", controller_fault_attribution=ambiguous because " - "the fault was observed after another control " - "command attempt had begun"; - } - detail += ", " + fault; - } - return AgvResult::failure( - task_status.state == 5 || task_status.state == 7 - ? AgvErrorCode::TaskFailed - : AgvErrorCode::TaskCanceled, - task_status.state == 5 || task_status.state == 7 - ? "SEER Robokit free-navigation task failed: " + detail - : "SEER Robokit free-navigation task was canceled: " + detail); - } - if (task_status.state < 1 || task_status.state > 7) { - return AgvResult::failure( - AgvErrorCode::TaskFailed, - "SEER Robokit free-navigation task reported an unsupported " - "terminal or vendor-specific status: " + last_status); - } - } else { - consecutive_running_samples = 0; - running_stability_window_active = false; - } - - const auto now = std::chrono::steady_clock::now(); - if (now >= hard_deadline - || (now >= deadline && !running_stability_window_active)) { - break; - } - sleepForNavigationPoll( - kPoseNavigationPollInterval, - hard_deadline, - options); - } - - if (matching_task_observed - && last_poll_matched - && last_matching_state == 1) { - // A matching Waiting task has been accepted by the controller and may - // legitimately remain queued. Returning a rejection here would invite - // a duplicate command while the original task can still start later. - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - controllerFaultCaptureGraceMs_(), - &fault_control_attempt); - if (superseded()) { - return supersededWithFault(); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - if (!fault.empty()) { - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - return ambiguous_fault_result(last_status, fault); - } - return AgvResult::failure( - AgvErrorCode::Fault, - "SEER Robokit free-navigation task was accepted and remains active, " - "but the controller reported a new fault: " - + last_status + ", " + fault - + "; the task may still start later, so do not retry " - "automatically; cancel or stop it before another motion command"); - } - return AgvResult::success(); - } - - std::string detail = last_status; - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - 0, - &fault_control_attempt); - if (superseded()) { - return supersededWithFault(); - } - if (!fault.empty()) { - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - detail += - ", controller_fault_attribution=ambiguous because the fault " - "was observed after another control command attempt had begun"; - } - detail += ", " + fault; - } - return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SEER Robokit accepted the free-navigation command, but no stable matching " - "pose task was established within " - + std::to_string(kPoseNavigationStartTimeout.count()) - + " ms; last " + detail - + "; do not retry automatically before checking or canceling navigation"); -} - -AgvResult SeerRobokitAgv::cancelTrackedNavigation_( - const TrackedNavigationContext& context, - std::uint64_t& accepted_generation) -{ - accepted_generation = 0; - Json::Value response; - const auto cancel_command = context.type == AgvTaskType::FollowPath - ? kRobotTaskClearTargetList - : kRobotTaskCancel; - auto result = sendControlledCommand_( - sock_navigation_, - cancel_command, - Json::Value(Json::objectValue), - &response, - &accepted_generation, - nullptr, - nullptr, - nullptr, - false, - nullptr, - true, - &context.token, - nullptr, - &context); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - -AgvResult SeerRobokitAgv::confirmMotionStopped() -{ - TrackedNavigationContext tracked_navigation; - if (currentTrackedNavigation_(tracked_navigation)) { - return waitForCanceledTaskToStop_( - tracked_navigation, - AgvMotionOptions{}, - "explicit motion-stop confirmation", - true); - } - - const auto confirmation_window = - kNavigationCancelConfirmationTimeout; - const auto deadline = - std::chrono::steady_clock::now() + confirmation_window; - int stopped_samples = 0; - std::string last_detail = "no 1101 status received"; - while (std::chrono::steady_clock::now() < deadline) { - NavigationSnapshot snapshot; - const auto snapshot_result = queryNavigationSnapshot_(snapshot); - if (!snapshot_result.ok()) { - return AgvResult::failure( - snapshot_result.code, - "SEER Robokit stopped state could not be confirmed with 1101: " - + snapshot_result.message); - } - - last_detail = snapshot.detail; - const bool no_active_navigation = - snapshot.task_status_present && - globalTaskStateIsKnownTerminal(snapshot.task_status); - if (no_active_navigation && navigationStopped(snapshot)) { - ++stopped_samples; - if (stopped_samples >= kRequiredCompletedStopSamples) { - return { - AgvErrorCode::OK, - "no active navigation and two zero-velocity samples were " - "confirmed with 1101"}; - } - } else { - stopped_samples = 0; - } - - const auto now = std::chrono::steady_clock::now(); - if (now < deadline) { - std::this_thread::sleep_for(std::min( - kNavigationCancelPollInterval, - std::chrono::duration_cast( - deadline - now))); - } - } - - return AgvResult::failure( - AgvErrorCode::Timeout, - "SEER Robokit did not confirm an inactive navigation task and two " - "zero-velocity samples within " - + std::to_string(confirmation_window.count()) - + " ms; last_status=" + last_detail); -} - -AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_( - const TrackedNavigationContext& context, - const AgvMotionOptions& options, - const std::string& reason, - const bool require_global_stopped) -{ - if (context.task_ids.empty()) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit cannot confirm cancellation without a tracked task id"); - } - - (void)options; - const auto confirmation_window = kNavigationCancelConfirmationTimeout; - const auto deadline = - std::chrono::steady_clock::now() + confirmation_window; - int stopped_samples = 0; - std::string last_detail = "no post-cancel status received"; - while (std::chrono::steady_clock::now() < deadline) { - std::vector task_statuses; - const auto task_result = queryTaskStatuses_( - context.task_ids, - task_statuses); - if (!task_result.ok()) { - return AgvResult::failure( - task_result.code, - "SEER Robokit navigation cancel was sent, but exact task " - "termination could not be queried: " + task_result.message); - } - - std::ostringstream exact_detail; - bool all_exact_tasks_terminal = !task_statuses.empty(); - bool any_exact_task_active = false; - for (std::size_t index = 0; index < task_statuses.size(); ++index) { - if (index > 0) { - exact_detail << "; "; - } - const auto& status = task_statuses[index]; - exact_detail << status.detail; - if (!status.found - || !exactTaskStateIsKnownTerminal(status.state)) { - all_exact_tasks_terminal = false; - } - if (status.found && exactTaskStateIsActive(status.state)) { - any_exact_task_active = true; - } - } - last_detail = exact_detail.str(); - - TrackedNavigationContext active_context; - const bool another_local_navigation_started = - currentTrackedNavigation_(active_context) - && active_context.token != context.token; - if (all_exact_tasks_terminal && another_local_navigation_started && - !require_global_stopped) { - // A later navigation is allowed to move after this exact task has - // reached a terminal state. Its velocity must not keep the older - // waiter alive or make it cancel the newer task. - for (const auto& task_id : context.task_ids) { - clearPoseTaskIfTaskId_(task_id); - } - return { - AgvErrorCode::OK, - "old exact task ids are terminal; global stopped state was " - "not inspected because a newer local navigation owns 1101"}; - } - if (another_local_navigation_started) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit conditional navigation cancel could not confirm all " - "exact task ids terminal before a newer local navigation " - "started; the newer task was not inspected or canceled; " - + last_detail); - } - - NavigationSnapshot snapshot; - const auto snapshot_result = queryNavigationSnapshot_(snapshot); - if (!snapshot_result.ok()) { - return AgvResult::failure( - snapshot_result.code, - "SEER Robokit navigation cancel was sent, but stopped state " - "could not be confirmed with 1101: " - + snapshot_result.message); - } - last_detail += ", " + snapshot.detail; - const int expected_global_type = - context.type == AgvTaskType::NavigateToPose - ? 1 - : (context.type == AgvTaskType::NavigateToStation ? 2 : 3); - const bool global_target_matches = snapshot.target_id.empty() - || context.target_ids.empty() - || std::find( - context.target_ids.begin(), - context.target_ids.end(), - snapshot.target_id) - != context.target_ids.end(); - const bool global_terminal = snapshot.task_status_present && - (snapshot.task_status == 0 - || (exactTaskStateIsKnownTerminal(snapshot.task_status) - && snapshot.task_type == expected_global_type - && global_target_matches)); - const bool task_termination_confirmed = require_global_stopped - ? (!any_exact_task_active && global_terminal) - : (all_exact_tasks_terminal - || (!any_exact_task_active && global_terminal)); - - if (task_termination_confirmed && navigationStopped(snapshot)) { - ++stopped_samples; - if (stopped_samples >= kRequiredCompletedStopSamples) { - for (const auto& task_id : context.task_ids) { - clearPoseTaskIfTaskId_(task_id); - } - clearTrackedNavigationIfToken_(context.token); - return { - AgvErrorCode::OK, - "stopped state was confirmed from task termination and " - "two zero-velocity samples"}; - } - } else { - stopped_samples = 0; - } - const auto now = std::chrono::steady_clock::now(); - if (now < deadline) { - std::this_thread::sleep_for(std::min( - kNavigationCancelPollInterval, - std::chrono::duration_cast( - deadline - now))); - } - } - - return AgvResult::failure( - AgvErrorCode::Timeout, - "SEER Robokit accepted the conditional navigation cancel, but task " - "termination and stopped velocity were not confirmed after " - + std::to_string(confirmation_window.count()) + " ms; reason=" - + reason + ", last_status=" + last_detail - + "; the robot state must be checked before another motion command"); -} - -AgvResult SeerRobokitAgv::failAndCancelTrackedNavigation_( - const TrackedNavigationContext& context, - const AgvMotionOptions& options, - const AgvErrorCode error_code, - const std::string& reason) -{ - const auto same_navigation_identity = []( - const TrackedNavigationContext& lhs, - const TrackedNavigationContext& rhs) { - return lhs.token == rhs.token - && lhs.type == rhs.type - && lhs.task_ids == rhs.task_ids - && lhs.target_id == rhs.target_id - && lhs.target_ids == rhs.target_ids; - }; - - TrackedNavigationContext cancel_context = context; - std::uint64_t cancel_generation = 0; - auto cancel_result = cancelTrackedNavigation_( - cancel_context, - cancel_generation); - - // A public cancel, pause/resume, or emergency stop can advance the control - // generation of this exact logical task while its synchronous waiter is - // handling an error. Refresh only an identical token/type/task/target - // identity; a replacement navigation must remain impossible to cancel. - bool same_task_generation_advanced = false; - for (int refresh_attempt = 0; - !cancel_result.ok() - && cancel_generation == 0 - && cancel_result.code == AgvErrorCode::TaskCanceled - && refresh_attempt < 3; - ++refresh_attempt) { - TrackedNavigationContext latest_context; - if (!currentTrackedNavigation_(latest_context) - || !same_navigation_identity(context, latest_context)) { - break; - } - if (latest_context.navigation_generation - <= cancel_context.navigation_generation) { - // sendControlledCommand_ publishes the global generation just - // before updating the tracked context. Yield across that tiny - // window, but keep the retry bounded. - if (navigation_generation_.load(std::memory_order_relaxed) - > cancel_context.navigation_generation) { - same_task_generation_advanced = true; - std::this_thread::yield(); - continue; - } - break; - } - same_task_generation_advanced = true; - cancel_context = std::move(latest_context); - cancel_generation = 0; - cancel_result = cancelTrackedNavigation_( - cancel_context, - cancel_generation); - } - - bool fail_safe_stop_sent = false; - const auto issue_tracked_fail_safe_stop = [this, - &cancel_context, - &fail_safe_stop_sent]() { - const auto stop_result = - emergencyStopTrackedNavigation_(&cancel_context); - fail_safe_stop_sent = stop_result.ok(); - return stop_result; - }; - - // If 1110/1101 ownership preflight is unavailable, a bare global - // 3003/3067 based only on stale local state could cancel another client's - // task. Escalate explicitly to the controller's software-stop sequence - // (2000 plus the matching navigation cancel), protected by a complete - // navigation-identity check under the control mutex. - const bool ownership_status_unavailable = - cancel_result.code == AgvErrorCode::NotConnected - || cancel_result.code == AgvErrorCode::Timeout - || cancel_result.code == AgvErrorCode::CommandFailed; - if (!cancel_result.ok() && cancel_generation == 0 - && (ownership_status_unavailable || same_task_generation_advanced)) { - const auto stop_result = issue_tracked_fail_safe_stop(); - if (!stop_result.ok()) { - return AgvResult::failure( - error_code, - reason + "; conditional cancel ownership could not be " - "confirmed: " + cancel_result.message - + "; tracked fail-safe software stop/cancel failed: " - + stop_result.message - + "; the stopped state is unconfirmed, so do not retry " - "motion automatically"); - } - } - if (!cancel_result.ok() && cancel_generation == 0 - && !fail_safe_stop_sent) { - return AgvResult::failure( - error_code, - reason + "; conditional cancel was not accepted: " - + cancel_result.message - + "; the original navigation task may still be active or may " - "have been replaced, so do not retry motion automatically"); - } - - auto stopped_result = waitForCanceledTaskToStop_( - cancel_context, - options, - reason); - if (!stopped_result.ok() && !fail_safe_stop_sent) { - // Exact task terminal state is not proof that a differential chassis - // has stopped. If 1101 cannot confirm two zero-velocity samples after - // a normal cancel (or after observing an already-terminal task), issue - // a task-identity-protected software stop before returning an error. - const auto stop_result = issue_tracked_fail_safe_stop(); - if (!stop_result.ok()) { - return AgvResult::failure( - error_code, - reason + "; stopped state could not be confirmed: " - + stopped_result.message - + "; tracked fail-safe software stop/cancel failed: " - + stop_result.message - + "; the stopped state remains unconfirmed, so do not " - "retry motion automatically"); - } - stopped_result = waitForCanceledTaskToStop_( - cancel_context, - options, - reason); - } - if (!stopped_result.ok()) { - const std::string cancel_detail = fail_safe_stop_sent - ? "; tracked fail-safe software stop and navigation cancel were issued" - : (cancel_generation != 0 - ? "; controller accepted conditional cancel" - : (cancel_result.ok() - ? "; exact task was already terminal, so global cancel was not sent" - : "; conditional cancel response was indeterminate: " - + cancel_result.message)); - return AgvResult::failure( - error_code, - reason + cancel_detail + "; stopped state remains unconfirmed: " - + stopped_result.message); - } - - const std::string cancel_detail = fail_safe_stop_sent - ? "; a tracked fail-safe software stop and navigation cancel were issued" - : (cancel_generation != 0 - ? "; the tracked navigation task was conditionally canceled" - : (cancel_result.ok() - ? "; the exact task was already terminal and no global cancel was sent" - : "; cancel response was indeterminate, but the exact task subsequently terminated")); - return AgvResult::failure( - error_code, - reason + cancel_detail + "; " + stopped_result.message); -} - -AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_( - const TrackedNavigationContext& context, - const AgvMotionOptions& options) -{ - if (context.task_ids.empty()) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit synchronous navigation has no task id to track"); - } - - const auto poll_interval = navigationPollInterval(options); - const auto deadline = std::chrono::steady_clock::now() - + navigationWaitTimeout(options); - const auto accepted_at = context.accepted_at - == std::chrono::steady_clock::time_point{} - ? std::chrono::steady_clock::now() - : context.accepted_at; - const auto start_deadline = accepted_at + kPoseNavigationStartTimeout; - bool any_task_observed = false; - bool final_completion_observed = false; - AgvErrorCode terminal_error = AgvErrorCode::OK; - std::string terminal_reason; - int blocked_stopped_samples = 0; - int terminal_stopped_samples = 0; - std::string last_detail = "no task status received"; - bool superseded_wait_active = false; - std::chrono::steady_clock::time_point superseded_deadline; - - while (std::chrono::steady_clock::now() < deadline) { - TrackedNavigationContext active_context; - const bool still_current = currentTrackedNavigation_(active_context) - && active_context.token == context.token; - if (navigationCancellationRequested(options)) { - if (!still_current) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit synchronous navigation wait was canceled after " - "the tracked task had already been replaced; the newer " - "task was not canceled"); - } - return failAndCancelTrackedNavigation_( - context, - options, - AgvErrorCode::TaskCanceled, - "SEER Robokit synchronous navigation wait was canceled by the caller"); - } - - std::vector task_statuses; - const auto task_result = queryTaskStatuses_( - context.task_ids, - task_statuses); - if (!task_result.ok()) { - if (!still_current) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit superseded navigation exact task query failed; " - "the newer task was not canceled: " + task_result.message); - } - return failAndCancelTrackedNavigation_( - context, - options, - task_result.code, - "SEER Robokit synchronous navigation exact task query failed: " - + task_result.message); - } - std::ostringstream exact_detail; - bool any_status_found_now = false; - bool any_exact_active = false; - bool all_exact_terminal_now = !task_statuses.empty(); - for (std::size_t index = 0; index < task_statuses.size(); ++index) { - auto& task_status = task_statuses[index]; - if (index > 0) { - exact_detail << "; "; - } - exact_detail << task_status.detail; - if (!task_status.found || task_status.state == 404) { - all_exact_terminal_now = false; - continue; - } - any_status_found_now = true; - any_task_observed = true; - if (task_status.state >= 1 && task_status.state <= 3) { - any_exact_active = true; - } - if (!exactTaskStateIsKnownTerminal(task_status.state)) { - all_exact_terminal_now = false; - } - if (task_status.type_present) { - const bool expected_type = - context.type == AgvTaskType::NavigateToStation - ? task_status.type == 2 - : (task_status.type == 2 || task_status.type == 3); - if (!expected_type) { - return failAndCancelTrackedNavigation_( - context, - options, - AgvErrorCode::TaskRejected, - "SEER Robokit exact task id reported an unexpected task " - "type: " + task_status.detail); - } - } - if (task_status.state == 5 || task_status.state == 7) { - terminal_error = AgvErrorCode::TaskFailed; - terminal_reason = - "SEER Robokit tracked navigation segment failed: " - + task_status.detail; - } else if (task_status.state == 6 - && terminal_error == AgvErrorCode::OK) { - terminal_error = AgvErrorCode::TaskCanceled; - terminal_reason = - "SEER Robokit tracked navigation segment was canceled: " - + task_status.detail; - } else if (task_status.state == 4 - && index + 1 == task_statuses.size()) { - final_completion_observed = true; - } else if (task_status.state < 1 - || task_status.state > 7) { - return failAndCancelTrackedNavigation_( - context, - options, - AgvErrorCode::TaskFailed, - "SEER Robokit tracked navigation reported an unsupported " - "terminal or vendor-specific state: " + task_status.detail); - } - } - last_detail = exact_detail.str(); - - TrackedNavigationContext post_query_context; - const bool still_current_after_query = - currentTrackedNavigation_(post_query_context) - && post_query_context.token == context.token; - - if (!any_task_observed - && std::chrono::steady_clock::now() >= start_deadline) { - if (!still_current_after_query) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit navigation was superseded before any exact task " - "id was observed; the newer task was not canceled; " - + last_detail); - } - return failAndCancelTrackedNavigation_( - context, - options, - AgvErrorCode::TaskRejected, - "SEER Robokit accepted the navigation command, but its exact task " - "id was not established within " - + std::to_string(kPoseNavigationStartTimeout.count()) - + " ms: " + last_detail); - } - - // Once another local control command has replaced this token, never - // consume global 1101 state (it can belong to the newer command) and - // never issue a global cancel. Exact terminal state is sufficient to - // finish the older waiter; otherwise wait briefly for 1110 to settle. - if (!still_current_after_query) { - if (terminal_error != AgvErrorCode::OK) { - return AgvResult::failure(terminal_error, terminal_reason); - } - if (final_completion_observed) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit navigation final task reported Completed, but a " - "newer local task replaced it before stopped state could " - "be confirmed; the newer task was not canceled"); - } - if (any_task_observed && !any_status_found_now) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit navigation was superseded and its exact task " - "status disappeared; the newer task was not canceled; " - + last_detail); - } - if (!superseded_wait_active) { - superseded_wait_active = true; - superseded_deadline = std::chrono::steady_clock::now() - + kNavigationCancelConfirmationTimeout; - } - if (std::chrono::steady_clock::now() >= superseded_deadline) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit superseded navigation exact task remained " - "non-terminal after the replacement grace window; the " - "newer task was not inspected or canceled; " + last_detail); - } - sleepForNavigationPoll( - poll_interval, - std::min(deadline, superseded_deadline), - options); - continue; - } - - const bool failed_path_may_have_hidden_queued_segments = - terminal_error != AgvErrorCode::OK - && context.type == AgvTaskType::FollowPath - && !all_exact_terminal_now; - if (terminal_error != AgvErrorCode::OK - && (any_exact_active - || failed_path_may_have_hidden_queued_segments)) { - return failAndCancelTrackedNavigation_( - context, - options, - terminal_error, - terminal_reason - + "; another exact path segment is still active or remains " - "hidden in the queued path and must be cleared before " - "any global 1101 attribution; " - + last_detail); - } - - NavigationSnapshot snapshot; - const auto snapshot_result = queryNavigationSnapshot_(snapshot); - if (!snapshot_result.ok()) { - if (terminal_error != AgvErrorCode::OK - || final_completion_observed) { - last_detail += ", 1101 detail unavailable: " - + snapshot_result.message; - sleepForNavigationPoll(poll_interval, deadline, options); - continue; - } - return failAndCancelTrackedNavigation_( - context, - options, - snapshot_result.code, - "SEER Robokit synchronous navigation safety snapshot failed: " - + snapshot_result.message); - } - last_detail += ", " + snapshot.detail; - - TrackedNavigationContext post_snapshot_context; - if (!currentTrackedNavigation_(post_snapshot_context) - || post_snapshot_context.token != context.token) { - // A concurrent navigation may have replaced this task while 1101 - // was in flight. Discard that global snapshot because it may - // already describe the replacement. - if (terminal_error != AgvErrorCode::OK) { - return AgvResult::failure(terminal_error, terminal_reason); - } - if (final_completion_observed) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit navigation final task reported Completed, but " - "the 1101 snapshot was superseded before stopped state " - "could be confirmed; the newer task was not canceled"); - } - if (!superseded_wait_active) { - superseded_wait_active = true; - superseded_deadline = std::chrono::steady_clock::now() - + kNavigationCancelConfirmationTimeout; - } - if (std::chrono::steady_clock::now() >= superseded_deadline) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit superseded navigation did not expose an exact " - "terminal state during the replacement grace window; the " - "newer task was not inspected or canceled; " + last_detail); - } - sleepForNavigationPoll( - poll_interval, - std::min(deadline, superseded_deadline), - options); - continue; - } - - const int expected_global_type = - context.type == AgvTaskType::NavigateToStation ? 2 : 3; - const bool global_active = snapshot.task_status >= 1 - && snapshot.task_status <= 3; - // 1110 and 1101 are separate controller publications. During the - // task-establishment window, even a newly visible exact task id can - // be paired with an older global snapshot. Until that window closes, - // use 1101 only for physical safety (faults, emergency, blockage and - // velocity), never for ownership or task-terminal attribution. - const bool global_attribution_ready = any_task_observed - && std::chrono::steady_clock::now() >= start_deadline; - const bool attributed_global_active = global_attribution_ready - && global_active; - const bool global_type_matches = - snapshot.task_type == expected_global_type; - const bool global_target_matches = - context.type == AgvTaskType::NavigateToStation - ? (!snapshot.target_id.empty() - && snapshot.target_id == context.target_id) - : (context.type == AgvTaskType::FollowPath - ? (!snapshot.target_id.empty() - && std::find( - context.target_ids.begin(), - context.target_ids.end(), - snapshot.target_id) - != context.target_ids.end()) - : true); - const bool global_matches_context = global_type_matches - && global_target_matches; - const bool attributed_matching_global_active = - attributed_global_active && global_matches_context; - if (snapshot.emergency || !snapshot.active_faults.empty()) { - return failAndCancelTrackedNavigation_( - context, - options, - snapshot.emergency - ? AgvErrorCode::EmergencyStopped - : AgvErrorCode::Fault, - "SEER Robokit controller reported a navigation safety fault: " - + last_detail); - } - if (terminal_error == AgvErrorCode::OK - && !any_exact_active - && global_attribution_ready - && global_matches_context - && (snapshot.task_status == 5 - || snapshot.task_status == 7)) { - terminal_error = AgvErrorCode::TaskFailed; - terminal_reason = - "SEER Robokit controller reported navigation failure: " - + last_detail; - } else if (terminal_error == AgvErrorCode::OK - && !any_exact_active - && global_attribution_ready - && global_matches_context - && snapshot.task_status == 6) { - terminal_error = AgvErrorCode::TaskCanceled; - terminal_reason = - "SEER Robokit controller reported navigation cancellation: " - + last_detail; - } - if (terminal_error != AgvErrorCode::OK - && (any_exact_active - || attributed_matching_global_active)) { - return failAndCancelTrackedNavigation_( - context, - options, - terminal_error, - terminal_reason - + "; another segment or the global navigation task is " - "still active and must be canceled before returning; " - + last_detail); - } - if (snapshot.blocked && navigationStopped(snapshot)) { - ++blocked_stopped_samples; - } else { - blocked_stopped_samples = 0; - } - if (blocked_stopped_samples >= kRequiredBlockedStopSamples) { - return failAndCancelTrackedNavigation_( - context, - options, - AgvErrorCode::TaskFailed, - "SEER Robokit navigation remained blocked while stopped for " - + std::to_string(blocked_stopped_samples) - + " consecutive 1101 samples: " + last_detail); - } - - bool terminal_candidate = terminal_error != AgvErrorCode::OK; - if (global_attribution_ready - && final_completion_observed - && !any_exact_active) { - const bool expected_completed_snapshot = - snapshot.task_status == 0 - || (snapshot.task_status == 4 - && snapshot.task_type == expected_global_type - && (context.target_id.empty() - || snapshot.target_id.empty() - || snapshot.target_id == context.target_id)); - if (expected_completed_snapshot) { - terminal_candidate = true; - } - } - if (terminal_candidate && navigationStopped(snapshot)) { - ++terminal_stopped_samples; - } else { - terminal_stopped_samples = 0; - } - if (terminal_stopped_samples >= kRequiredCompletedStopSamples) { - clearTrackedNavigationIfToken_(context.token); - if (terminal_error != AgvErrorCode::OK) { - return AgvResult::failure( - terminal_error, - terminal_reason + "; stopped velocity confirmed; " - + last_detail); - } - return AgvResult::success(); - } - - sleepForNavigationPoll(poll_interval, deadline, options); - } - - TrackedNavigationContext active_context; - if (!currentTrackedNavigation_(active_context) - || active_context.token != context.token) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit superseded navigation did not expose an exact terminal " - "state before the wait timeout; the newer task was not canceled; " - "last_status=" + last_detail); - } - if (terminal_error != AgvErrorCode::OK - || final_completion_observed) { - // A terminal task status is not proof that a differential chassis has - // stopped. Keep ownership and enter the same bounded cancellation / - // stop-confirmation path used by all other unsafe exits. If stopped - // state still cannot be established, tracking remains published so a - // later explicit cancel can use the correct 3003/3067 command. - return failAndCancelTrackedNavigation_( - context, - options, - terminal_error != AgvErrorCode::OK - ? terminal_error - : AgvErrorCode::Timeout, - "SEER Robokit navigation reached a terminal task state, but stopped " - "velocity could not be confirmed before timeout; last_status=" - + last_detail); - } - return failAndCancelTrackedNavigation_( - context, - options, - AgvErrorCode::Timeout, - "SEER Robokit synchronous navigation did not reach a terminal stopped " - "state within " + std::to_string(navigationWaitTimeout(options).count()) - + " ms; last_status=" + last_detail); -} - -AgvResult SeerRobokitAgv::waitForPoseNavigationTerminal_( - const PoseTaskContext& pose_context, - const TrackedNavigationContext& navigation_context, - const AgvMotionOptions& options) -{ - const auto poll_interval = navigationPollInterval(options); - const auto deadline = std::chrono::steady_clock::now() - + navigationWaitTimeout(options); - const auto accepted_at = navigation_context.accepted_at - == std::chrono::steady_clock::time_point{} - ? std::chrono::steady_clock::now() - : navigation_context.accepted_at; - const auto global_attribution_deadline = - accepted_at + kPoseNavigationStartTimeout; - bool completion_observed = false; - AgvErrorCode terminal_error = AgvErrorCode::OK; - std::string terminal_reason; - int blocked_stopped_samples = 0; - int terminal_stopped_samples = 0; - std::string last_detail = "free-navigation task start was confirmed"; - bool superseded_wait_active = false; - std::chrono::steady_clock::time_point superseded_deadline; - - while (std::chrono::steady_clock::now() < deadline) { - TrackedNavigationContext active_context; - const bool still_current = currentTrackedNavigation_(active_context) - && active_context.token == navigation_context.token; - if (navigationCancellationRequested(options)) { - if (!still_current) { - return AgvResult::failure( - AgvErrorCode::TaskCanceled, - "SEER Robokit synchronous free-navigation wait was canceled " - "after its task had been replaced; the newer task was not " - "canceled"); - } - return failAndCancelTrackedNavigation_( - navigation_context, - options, - AgvErrorCode::TaskCanceled, - "SEER Robokit synchronous free-navigation wait was canceled by " - "the caller"); - } - - PoseTaskStatus exact_status; - const auto exact_result = queryPoseTaskStatus_( - pose_context.task_id, - exact_status); - if (!exact_result.ok()) { - if (!still_current) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit superseded free-navigation exact task query " - "failed; the newer task was not canceled: " - + exact_result.message); - } - return failAndCancelTrackedNavigation_( - navigation_context, - options, - exact_result.code, - "SEER Robokit synchronous free-navigation exact task query " - "failed: " + exact_result.message); - } - last_detail = exact_status.detail; - if (!exact_status.found || exact_status.state == 404) { - if (!still_current) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit free-navigation task was superseded and its " - "exact status disappeared; the newer task was not " - "canceled: " + last_detail); - } - return failAndCancelTrackedNavigation_( - navigation_context, - options, - AgvErrorCode::CommandFailed, - "SEER Robokit exact free-navigation task disappeared after its " - "start was confirmed: " + last_detail); - } - if (exact_status.type_present && exact_status.type != 1) { - if (!still_current) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit superseded free-navigation task id reported an " - "unexpected type; the newer task was not canceled: " - + last_detail); - } - return failAndCancelTrackedNavigation_( - navigation_context, - options, - AgvErrorCode::TaskRejected, - "SEER Robokit exact free-navigation task reported an unexpected " - "type: " + last_detail); - } - if (exact_status.state == 4 && !completion_observed - && terminal_error == AgvErrorCode::OK) { - // A Completed task can still be decelerating. Defer pose - // acceptance until two stopped 1101 samples have been observed; - // an eager 1004 check here can reject a task that settles inside - // tolerance or accept one that later drifts outside it. - completion_observed = true; - } else if ((exact_status.state == 5 || exact_status.state == 7) - && terminal_error == AgvErrorCode::OK) { - terminal_error = AgvErrorCode::TaskFailed; - terminal_reason = - "SEER Robokit free-navigation task failed: " + last_detail; - } else if (exact_status.state == 6 - && terminal_error == AgvErrorCode::OK) { - terminal_error = AgvErrorCode::TaskCanceled; - terminal_reason = - "SEER Robokit free-navigation task was canceled: " + last_detail; - } else if (exact_status.state < 1 || exact_status.state > 7) { - if (!still_current) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit superseded free-navigation task reported an " - "unsupported state; the newer task was not canceled: " - + last_detail); - } - return failAndCancelTrackedNavigation_( - navigation_context, - options, - AgvErrorCode::TaskFailed, - "SEER Robokit exact free-navigation task reported an unsupported " - "state: " + last_detail); - } - - // Re-read the token after the exact 1110 query. A concurrent - // pose command may have replaced the active context while those I/O - // operations were in flight; global 1101 must never be attributed to - // the older waiter in that case. - TrackedNavigationContext post_query_context; - const bool still_current_after_query = - currentTrackedNavigation_(post_query_context) - && post_query_context.token == navigation_context.token; - if (!still_current_after_query) { - if (terminal_error != AgvErrorCode::OK) { - clearPoseTaskIfTaskId_(pose_context.task_id); - return AgvResult::failure(terminal_error, terminal_reason); - } - if (completion_observed) { - clearPoseTaskIfTaskId_(pose_context.task_id); - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit free-navigation task reported Completed, but a " - "newer local task replaced it before final stopped-pose " - "verification; the newer task was not canceled"); - } - if (!superseded_wait_active) { - superseded_wait_active = true; - superseded_deadline = std::chrono::steady_clock::now() - + kNavigationCancelConfirmationTimeout; - } - if (std::chrono::steady_clock::now() >= superseded_deadline) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit superseded free-navigation task remained " - "non-terminal after the replacement grace window; the " - "newer task was not inspected or canceled; " + last_detail); - } - sleepForNavigationPoll( - poll_interval, - std::min(deadline, superseded_deadline), - options); - continue; - } - - NavigationSnapshot snapshot; - const auto snapshot_result = queryNavigationSnapshot_(snapshot); - if (!snapshot_result.ok()) { - if (terminal_error != AgvErrorCode::OK || completion_observed) { - last_detail += ", 1101 detail unavailable: " - + snapshot_result.message; - sleepForNavigationPoll(poll_interval, deadline, options); - continue; - } - return failAndCancelTrackedNavigation_( - navigation_context, - options, - snapshot_result.code, - "SEER Robokit synchronous free-navigation safety snapshot " - "failed: " + snapshot_result.message); - } - last_detail += ", " + snapshot.detail; - - TrackedNavigationContext post_snapshot_context; - if (!currentTrackedNavigation_(post_snapshot_context) - || post_snapshot_context.token != navigation_context.token) { - // The snapshot may describe the replacement task; discard it. - if (terminal_error != AgvErrorCode::OK) { - clearPoseTaskIfTaskId_(pose_context.task_id); - return AgvResult::failure(terminal_error, terminal_reason); - } - if (completion_observed) { - clearPoseTaskIfTaskId_(pose_context.task_id); - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit free-navigation task reported Completed, but the " - "1101 snapshot was superseded before final stopped-pose " - "verification; the newer task was not canceled"); - } - if (!superseded_wait_active) { - superseded_wait_active = true; - superseded_deadline = std::chrono::steady_clock::now() - + kNavigationCancelConfirmationTimeout; - } - if (std::chrono::steady_clock::now() >= superseded_deadline) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit superseded free navigation did not expose an " - "exact terminal state during the replacement grace " - "window; the newer task was not inspected or canceled; " - + last_detail); - } - sleepForNavigationPoll( - poll_interval, - std::min(deadline, superseded_deadline), - options); - continue; - } - - const bool global_active = exactTaskStateIsActive( - snapshot.task_status); - const bool global_attribution_ready = - std::chrono::steady_clock::now() - >= global_attribution_deadline; - const bool attributed_global_active = global_attribution_ready - && global_active; - const bool attributed_matching_global_active = - attributed_global_active && snapshot.task_type == 1; - - if (snapshot.emergency || !snapshot.active_faults.empty()) { - return failAndCancelTrackedNavigation_( - navigation_context, - options, - snapshot.emergency - ? AgvErrorCode::EmergencyStopped - : AgvErrorCode::Fault, - "SEER Robokit controller reported a free-navigation safety " - "fault: " + last_detail); - } - - if (terminal_error == AgvErrorCode::OK - && !exactTaskStateIsActive(exact_status.state) - && global_attribution_ready - && snapshot.task_type == 1 - && (snapshot.task_status == 5 - || snapshot.task_status == 7)) { - terminal_error = AgvErrorCode::TaskFailed; - terminal_reason = - "SEER Robokit controller reported free-navigation failure: " - + last_detail; - } else if (terminal_error == AgvErrorCode::OK - && !exactTaskStateIsActive(exact_status.state) - && global_attribution_ready - && snapshot.task_type == 1 - && snapshot.task_status == 6) { - terminal_error = AgvErrorCode::TaskCanceled; - terminal_reason = - "SEER Robokit controller reported free-navigation cancellation: " - + last_detail; - } - if (terminal_error != AgvErrorCode::OK - && (exactTaskStateIsActive(exact_status.state) - || attributed_matching_global_active)) { - return failAndCancelTrackedNavigation_( - navigation_context, - options, - terminal_error, - terminal_reason - + "; the exact or global free-navigation task is still " - "active and must be canceled before returning; " - + last_detail); - } - - if (snapshot.blocked && navigationStopped(snapshot)) { - ++blocked_stopped_samples; - } else { - blocked_stopped_samples = 0; - } - if (blocked_stopped_samples >= kRequiredBlockedStopSamples) { - return failAndCancelTrackedNavigation_( - navigation_context, - options, - AgvErrorCode::TaskFailed, - "SEER Robokit free navigation remained blocked while stopped for " - + std::to_string(blocked_stopped_samples) - + " consecutive 1101 samples: " + last_detail); - } - - const bool expected_completed_snapshot = global_attribution_ready - && completion_observed - && exact_status.state == 4 - && (snapshot.task_status == 0 - || (snapshot.task_status == 4 - && snapshot.task_type == 1)); - if ((terminal_error != AgvErrorCode::OK - || expected_completed_snapshot) - && navigationStopped(snapshot)) { - ++terminal_stopped_samples; - } else { - terminal_stopped_samples = 0; - } - if (terminal_stopped_samples >= kRequiredCompletedStopSamples) { - if (terminal_error != AgvErrorCode::OK) { - clearPoseTaskIfTaskId_(pose_context.task_id); - clearTrackedNavigationIfToken_(navigation_context.token); - return AgvResult::failure( - terminal_error, - terminal_reason + "; stopped velocity confirmed; " - + last_detail); - } - - std::string final_pose_detail; - const bool final_pose_reached = - poseTargetReached_(pose_context, final_pose_detail); - TrackedNavigationContext post_pose_context; - if (!currentTrackedNavigation_(post_pose_context) - || post_pose_context.token != navigation_context.token) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit free-navigation completion was superseded during " - "the final stopped-pose verification; the newer task was " - "not canceled; " + final_pose_detail); - } - clearPoseTaskIfTaskId_(pose_context.task_id); - clearTrackedNavigationIfToken_(navigation_context.token); - if (!final_pose_reached) { - return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SEER Robokit free-navigation task stopped after reporting " - "Completed, but the final pose was outside the requested " - "tolerance: " + final_pose_detail); - } - return AgvResult::success(); - } - - sleepForNavigationPoll(poll_interval, deadline, options); - } - - TrackedNavigationContext active_context; - if (!currentTrackedNavigation_(active_context) - || active_context.token != navigation_context.token) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit superseded free navigation did not expose an exact " - "terminal state before timeout; the newer task was not canceled; " - "last_status=" + last_detail); - } - if (terminal_error != AgvErrorCode::OK || completion_observed) { - return failAndCancelTrackedNavigation_( - navigation_context, - options, - terminal_error != AgvErrorCode::OK - ? terminal_error - : AgvErrorCode::Timeout, - "SEER Robokit free-navigation task reached a terminal state, but " - "stopped velocity could not be confirmed before timeout; " - "last_status=" + last_detail); - } - return failAndCancelTrackedNavigation_( - navigation_context, - options, - AgvErrorCode::Timeout, - "SEER Robokit synchronous free navigation did not reach its verified " - "target and stop within " - + std::to_string(navigationWaitTimeout(options).count()) - + " ms; last_status=" + last_detail); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp deleted file mode 100644 index 475d4648..00000000 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp +++ /dev/null @@ -1,725 +0,0 @@ -#include "seer_robokit_agv.h" -#include "seer_robokit_protocol.h" -#include "seer_robokit_utils.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace cmvr::device { - -using namespace seer_robokit::protocol; -using namespace seer_robokit::detail; - -namespace { - -bool hasFaultArray(const Json::Value& value, const char* key) -{ - const auto* found = jsonFind(value, key); - return found && found->isArray() && !found->empty(); -} - -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: - case 7: - 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 - -AgvRuntimeState SeerRobokitAgv::runtimeState() const -{ - AgvRuntimeState cached_state; - bool has_cached_state = false; - if (state_push_enabled_) { - std::lock_guard lock(runtime_state_mutex_); - if (cached_runtime_state_valid_) { - cached_state = cached_runtime_state_; - has_cached_state = true; - } - } - - if (has_cached_state) { - std::string adapter_error; - { - std::lock_guard lock(mutex_); - cached_state.connected = connected_(); - adapter_error = last_error_; - } - if (!adapter_error.empty()) { - if (cached_state.last_error.empty()) { - cached_state.last_error = adapter_error; - } else if (cached_state.last_error != adapter_error) { - cached_state.last_error += "; adapter_error=" + adapter_error; - } - } - if (!cached_state.connected) { - cached_state.mode = AgvMode::Disconnected; - } - return cached_state; - } - - return queryRuntimeState_(); -} - -AgvRuntimeState SeerRobokitAgv::queryRuntimeState_() const -{ - AgvRuntimeState state; - { - std::lock_guard lock(mutex_); - state.connected = connected_(); - state.last_error = last_error_; - } - state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected; - - 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 SeerRobokitAgv::navigationStatus() const -{ - AgvNavigationStatus status; - std::string missing_pose_task_detail; - for (int attempt = 0; attempt < 2; ++attempt) { - PoseTaskContext pose_context; - if (!currentPoseTask_(pose_context)) { - break; - } - const auto observed_navigation_generation = - navigation_generation_.load(std::memory_order_relaxed); - if (pose_context.navigation_generation - != observed_navigation_generation) { - continue; - } - - PoseTaskStatus task_status; - const auto result = queryPoseTaskStatus_(pose_context.task_id, task_status); - PoseTaskContext latest_context; - if (navigation_generation_.load(std::memory_order_relaxed) - != observed_navigation_generation - || !currentPoseTask_(latest_context) - || latest_context.navigation_generation - != pose_context.navigation_generation - || latest_context.task_id != pose_context.task_id) { - continue; - } - - status.type = AgvTaskType::NavigateToPose; - const auto fault_monitoring_unavailable = - [this, &pose_context]() { - if (controller_fault_channel_epoch_.load( - std::memory_order_relaxed) - != pose_context - .controller_fault_channel_epoch_at_start) { - return std::string( - "the controller fault push channel changed or was " - "invalidated after the free-navigation command was " - "accepted"); - } - return freeNavigationFaultStateUnavailableDetail_(); - }; - if (!result.ok()) { - status.state = AgvTaskState::Failed; - status.message = result.message; - return status; - } - if (!task_status.found || task_status.state == 404) { - std::uint64_t missing_task_fault_control_attempt = 0; - const std::string missing_task_fault = - cachedControllerFaultDetail_( - pose_context.controller_fault_sequence_at_start, - controllerFaultCaptureGraceMs_(), - &missing_task_fault_control_attempt); - PoseTaskContext post_missing_context; - if (navigation_generation_.load(std::memory_order_relaxed) - != observed_navigation_generation - || !currentPoseTask_(post_missing_context) - || post_missing_context.navigation_generation - != pose_context.navigation_generation - || post_missing_context.task_id != pose_context.task_id) { - continue; - } - if (!missing_task_fault.empty()) { - std::string attribution; - if (missing_task_fault_control_attempt != 0 - && missing_task_fault_control_attempt - != pose_context.control_attempt_sequence_at_start) { - attribution = - "controller_fault_attribution=ambiguous because the " - "fault was observed after another control command " - "attempt had begun, "; - } - status.state = AgvTaskState::Failed; - status.message = - "SEER Robokit tracked free-navigation task disappeared from " - "1110 task_status_package while a new controller fault " - "was observed: " + task_status.detail + ", " - + attribution + missing_task_fault; - clearPoseTask_(pose_context.navigation_generation); - return status; - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - status.state = AgvTaskState::Failed; - status.message = - "SEER Robokit tracked free-navigation status is unsafe to " - "accept because controller fault monitoring is " - "unavailable: " + unavailable - + "; query the controller and cancel or stop before " - "another motion command"; - return status; - } - missing_pose_task_detail = task_status.detail; - clearPoseTask_(pose_context.navigation_generation); - break; - } - if (task_status.type_present && task_status.type != 1) { - status.state = AgvTaskState::Failed; - status.type = toTaskType(task_status.type); - status.message = - "SEER Robokit returned an unexpected task type for the tracked " - "free-navigation task: " + task_status.detail; - clearPoseTask_(pose_context.navigation_generation); - return status; - } - status.state = toTaskState(task_status.state); - status.progress = task_status.progress; - status.message = task_status.detail; - const auto controller_reported_state = status.state; - const bool controller_state_terminal = - controller_reported_state == AgvTaskState::Completed - || controller_reported_state == AgvTaskState::Failed - || controller_reported_state == AgvTaskState::Canceled; - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - pose_context.controller_fault_sequence_at_start, - controller_reported_state == AgvTaskState::Completed - || controller_reported_state == AgvTaskState::Failed - ? controllerFaultCaptureGraceMs_() - : 0, - &fault_control_attempt); - PoseTaskContext post_fault_context; - if (navigation_generation_.load(std::memory_order_relaxed) - != observed_navigation_generation - || !currentPoseTask_(post_fault_context) - || post_fault_context.navigation_generation - != pose_context.navigation_generation - || post_fault_context.task_id != pose_context.task_id) { - continue; - } - const std::string unavailable = - fault_monitoring_unavailable(); - if (!fault.empty()) { - std::string attribution; - if (fault_control_attempt != 0 - && fault_control_attempt - != pose_context.control_attempt_sequence_at_start) { - attribution = - "controller_fault_attribution=ambiguous because the " - "fault was observed after another control command " - "attempt had begun, "; - } - status.state = AgvTaskState::Failed; - status.message = - "SEER Robokit reported a new controller fault while the tracked " - "free-navigation task had controller_task_state=" - + std::to_string(task_status.state) + ": " - + task_status.detail + ", " + attribution + fault; - if (!unavailable.empty()) { - status.message += - ", controller_fault_monitoring_unavailable=" - + unavailable; - } - } else if (!unavailable.empty()) { - if (controller_reported_state == AgvTaskState::Failed - || controller_reported_state == AgvTaskState::Canceled) { - status.message += - ", controller_fault_monitoring_unavailable=" - + unavailable; - } else { - status.state = AgvTaskState::Failed; - status.message = - "SEER Robokit tracked free-navigation state is unsafe to accept " - "because controller fault monitoring became unavailable: " - + unavailable - + "; query the controller and cancel or stop before " - "another motion command"; - return status; - } - } - if (status.state == AgvTaskState::Completed) { - std::string pose_detail; - const bool target_reached = - poseTargetReached_(pose_context, pose_detail); - PoseTaskContext post_pose_context; - if (navigation_generation_.load(std::memory_order_relaxed) - != observed_navigation_generation - || !currentPoseTask_(post_pose_context) - || post_pose_context.navigation_generation - != pose_context.navigation_generation - || post_pose_context.task_id != pose_context.task_id) { - continue; - } - std::uint64_t post_pose_fault_control_attempt = 0; - const std::string post_pose_fault = - cachedControllerFaultDetail_( - pose_context.controller_fault_sequence_at_start, - 0, - &post_pose_fault_control_attempt); - if (!post_pose_fault.empty()) { - status.state = AgvTaskState::Failed; - std::string attribution; - if (post_pose_fault_control_attempt != 0 - && post_pose_fault_control_attempt - != pose_context - .control_attempt_sequence_at_start) { - attribution = - "controller_fault_attribution=ambiguous because the " - "fault was observed after another control command " - "attempt had begun, "; - } - status.message = - "SEER Robokit reported the tracked free-navigation task " - "Completed, but a new controller fault was observed during " - "target verification: " + task_status.detail + ", " - + attribution + post_pose_fault; - } else if (const std::string post_pose_unavailable = - fault_monitoring_unavailable(); - !post_pose_unavailable.empty()) { - status.state = AgvTaskState::Failed; - status.message = - "SEER Robokit tracked free-navigation completion is unsafe to " - "accept because controller fault monitoring became " - "unavailable: " + post_pose_unavailable - + "; query the controller and cancel or stop before " - "another motion command"; - return status; - } else if (!target_reached) { - status.state = AgvTaskState::Failed; - status.message = - "SEER Robokit reported the tracked free-navigation task " - "Completed, but the requested target was not reached: " - + task_status.detail + ", " + pose_detail; - } else { - status.message += ", target_verified: " + pose_detail; - } - } - if (controller_state_terminal) { - clearPoseTask_(pose_context.navigation_generation); - } - return status; - } - - PoseTaskContext changed_context; - if (currentPoseTask_(changed_context)) { - status.state = AgvTaskState::Waiting; - status.type = AgvTaskType::NavigateToPose; - status.message = - "SEER Robokit free-navigation task changed while its status was being " - "queried; query navigation status again"; - return 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 = missing_pose_task_detail.empty() - ? result.message - : missing_pose_task_detail + "; 1020 status query failed: " - + result.message; - return status; - } - const auto controller_result = resultFromResponse_(response); - if (!controller_result.ok()) { - status.state = AgvTaskState::Failed; - status.message = missing_pose_task_detail.empty() - ? controller_result.message - : missing_pose_task_detail + "; 1020 status query failed: " - + controller_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 (!missing_pose_task_detail.empty()) { - status.message = missing_pose_task_detail - + "; fallback_1020_status=" + std::to_string( - jsonGet(response, "task_status", 0).asInt()) - + ", fallback_1020_type=" + std::to_string( - jsonGet(response, "task_type", 0).asInt()) - + (status.message.empty() ? std::string{} : ", " + status.message); - } - if (const auto* task_status_package = jsonFind(response, "task_status_package")) { - status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble(); - } - return status; -} - -AgvResult SeerRobokitAgv::configurePush_() -{ - if (config_.state_push_included_fields_size() > 0 && config_.state_push_excluded_fields_size() > 0) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SEER Robokit 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, "SEER Robokit push socket not connected"); - } - if (::send(sock_push_, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast(frame.size())) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit 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 SeerRobokitAgv::startPushThread_() -{ - if (!state_push_enabled_) { - return; - } - if (push_running_.exchange(true)) { - return; - } - if (sock_push_ < 0) { - push_running_ = false; - return; - } - push_thread_ = std::thread(&SeerRobokitAgv::pushLoop_, this); -} - -void SeerRobokitAgv::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(); - } - invalidateControllerFaultState_(); -} - -void SeerRobokitAgv::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) { - invalidateControllerFaultState_(); - std::lock_guard lock(mutex_); - last_error_ = result.message; - closeSocket_(sock_push_); - } - continue; - } - if (command != kRobotPush || payload.empty()) { - continue; - } - - Json::Value parsed; - std::string error; - if (!parseJson_(payload, parsed, error)) { - invalidateControllerFaultState_(); - std::lock_guard lock(mutex_); - last_error_ = error; - continue; - } - updateCachedRuntimeState_(parsed); - } -} - -void SeerRobokitAgv::invalidateControllerFaultState_() -{ - std::lock_guard lock(runtime_state_mutex_); - controller_fault_channel_epoch_.fetch_add( - 1, - std::memory_order_relaxed); - controller_fault_state_observed_ = false; - controller_fault_state_observed_at_ = {}; - active_controller_fault_detail_.clear(); - runtime_state_cv_.notify_all(); -} - -void SeerRobokitAgv::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; - - 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; - const bool has_fatals = jsonHas(payload, "fatals"); - const bool has_errors = jsonHas(payload, "errors"); - const bool has_fault_fields = has_fatals || has_errors; - if (has_fault_fields) { - const auto* fatals = jsonFind(payload, "fatals"); - const auto* errors = jsonFind(payload, "errors"); - const bool valid_fatals = !has_fatals - || (fatals && fatals->isArray()); - const bool valid_errors = !has_errors - || (errors && errors->isArray()); - const bool complete_fault_state = - has_fatals && has_errors && valid_fatals && valid_errors; - if (complete_fault_state) { - controller_fault_state_observed_ = true; - controller_fault_state_observed_at_ = - std::chrono::steady_clock::now(); - } else { - controller_fault_state_observed_ = false; - controller_fault_state_observed_at_ = {}; - } - - const bool reported_fault = - hasFaultArray(payload, "fatals") - || hasFaultArray(payload, "errors"); - const bool invalid_or_incomplete_fault_state = - !complete_fault_state && !reported_fault; - state.fault = reported_fault - || invalid_or_incomplete_fault_state; - if (state.fault) { - std::ostringstream detail; - detail << (reported_fault - ? "SEER Robokit controller fault" - : "SEER Robokit controller fault state is incomplete or malformed"); - if (fatals - && (!fatals->isArray() - || !fatals->empty() - || !complete_fault_state)) { - detail << ": fatals=" - << (fatals->isNull() - ? std::string("null") - : jsonValueToString(*fatals)); - } - if (errors - && (!errors->isArray() - || !errors->empty() - || !complete_fault_state)) { - detail << ": errors=" - << (errors->isNull() - ? std::string("null") - : jsonValueToString(*errors)); - } - state.last_error = detail.str(); - if (state.last_error != active_controller_fault_detail_) { - active_controller_fault_detail_ = state.last_error; - ++controller_fault_sequence_; - last_controller_fault_timestamp_ = state.timestamp; - last_controller_fault_detail_ = state.last_error; - last_controller_fault_control_attempt_ = - control_attempt_sequence_.load( - std::memory_order_acquire); - } - } else { - state.last_error.clear(); - active_controller_fault_detail_.clear(); - } - } - 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; - runtime_state_cv_.notify_all(); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp deleted file mode 100644 index 97b7a515..00000000 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp +++ /dev/null @@ -1,362 +0,0 @@ -#include "seer_robokit_agv.h" -#include "seer_robokit_protocol.h" -#include "seer_robokit_utils.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::device { - -using namespace seer_robokit::protocol; -using namespace seer_robokit::detail; - -AgvResult SeerRobokitAgv::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 SEER Robokit ip: " + ip_; - return AgvResult::failure(AgvErrorCode::InvalidArgument, last_error_); - } - - if (::connect(sock, reinterpret_cast(&address), sizeof(address)) < 0) { - closeSocket_(sock); - last_error_ = "connect SEER Robokit 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 SeerRobokitAgv::ensureOtherSocket_() -{ - std::lock_guard lock(mutex_); - if (sock_other_ >= 0) { - return AgvResult::success(); - } - return connectSocket_(sock_other_, ports_.other); -} - -void SeerRobokitAgv::closeSocket_(int& sock) const -{ - if (sock >= 0) { - ::close(sock); - sock = -1; - } -} - -bool SeerRobokitAgv::connected_() const -{ - return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0; -} - -AgvResult SeerRobokitAgv::sendCommand_( - const int sock, - const std::uint16_t command, - const Json::Value& payload, - Json::Value* response, - CommandTransmissionState* transmission_state) const -{ - std::string response_payload; - auto result = sendCommandRaw_( - sock, - command, - payload, - &response_payload, - transmission_state); - 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 SeerRobokitAgv::sendCommandRaw_( - const int sock, - const std::uint16_t command, - const Json::Value& payload, - std::string* response_payload, - CommandTransmissionState* transmission_state) const -{ - if (transmission_state) { - *transmission_state = CommandTransmissionState::NotSent; - } - const auto exchange = [&]() { - const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload); - const auto frame = buildFrame_(command, payload_text); - const auto sent = ::send( - sock, - frame.data(), - frame.size(), - MSG_NOSIGNAL); - if (sent > 0 && transmission_state) { - *transmission_state = CommandTransmissionState::PossiblySent; - } - if (sent != static_cast(frame.size())) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit 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; - } - const auto expected_response_command = static_cast( - command + 10000U); - if (response_command != expected_response_command) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit response command mismatch: expected=" - + std::to_string(expected_response_command) - + ", actual=" + std::to_string(response_command)); - } - if (response_payload) { - *response_payload = std::move(payload_text_response); - } - return AgvResult::success(); - }; - const auto close_matching_socket_locked = [this, sock]() { - if (sock == sock_status_) { - closeSocket_(sock_status_); - } else if (sock == sock_control_) { - closeSocket_(sock_control_); - } else if (sock == sock_navigation_) { - closeSocket_(sock_navigation_); - } else if (sock == sock_config_) { - closeSocket_(sock_config_); - } else if (sock == sock_other_) { - closeSocket_(sock_other_); - } - }; - const auto mark_channel_desynchronized = [](AgvResult result) { - std::string detail = result.message.empty() - ? "unknown transport or frame error" - : result.message; - detail += - "; SEER Robokit channel closed because the response stream may be " - "desynchronized; reconnect before sending another command"; - return AgvResult::failure(result.code, detail); - }; - - bool is_status_socket = false; - { - std::lock_guard lock(mutex_); - if (sock < 0) { - return AgvResult::failure( - AgvErrorCode::NotConnected, - "SEER Robokit socket not connected"); - } - is_status_socket = sock == sock_status_; - } - - if (is_status_socket) { - // A slow 1110 status response must never hold the lifecycle/global I/O - // mutex needed by cancelNavigation() or emergencyStop(). The dedicated - // status lock still serializes requests on port 19204. connect_() and - // disconnect_() take this lock before changing the descriptor. - std::lock_guard status_lock(status_io_mutex_); - { - std::lock_guard lock(mutex_); - if (sock < 0 || sock != sock_status_) { - return AgvResult::failure( - AgvErrorCode::NotConnected, - "SEER Robokit status socket is no longer connected"); - } - } - auto result = exchange(); - if (!result.ok()) { - std::lock_guard lock(mutex_); - close_matching_socket_locked(); - return mark_channel_desynchronized(std::move(result)); - } - return result; - } - - std::lock_guard lock(mutex_); - if (sock < 0 - || (sock != sock_control_ - && sock != sock_navigation_ - && sock != sock_config_ - && sock != sock_other_)) { - return AgvResult::failure( - AgvErrorCode::NotConnected, - "SEER Robokit socket is no longer connected"); - } - auto result = exchange(); - if (!result.ok()) { - close_matching_socket_locked(); - return mark_channel_desynchronized(std::move(result)); - } - return result; -} - -AgvResult SeerRobokitAgv::sendCommandNoResponse_( - const int sock, - const std::uint16_t command, - const Json::Value& payload) const -{ - return sendCommand_(sock, command, payload, nullptr); -} - -std::vector SeerRobokitAgv::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 SeerRobokitAgv::toJsonString_(const Json::Value& value) -{ - Json::StreamWriterBuilder builder; - builder["indentation"] = ""; - return Json::writeString(builder, value); -} - -bool SeerRobokitAgv::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 SeerRobokitAgv::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 SeerRobokitAgv::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, "SEER Robokit socket closed"); - } - if (errno == EINTR) { - continue; - } - if (errno == EAGAIN || errno == EWOULDBLOCK) { - return AgvResult::failure(AgvErrorCode::Timeout, "SEER Robokit receive timeout"); - } - return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit 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, "SEER Robokit 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, "SEER Robokit 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(); -} - - -AgvResult SeerRobokitAgv::resultFromResponse_(const Json::Value& response) -{ - if (!hasNumericControllerRetCode(response)) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SEER Robokit controller response is missing a numeric ret_code"); - } - const auto* ret_code_value = jsonFind(response, "ret_code"); - const bool success = ret_code_value->isUInt() || ret_code_value->isUInt64() - ? ret_code_value->asUInt64() == 0 - : ret_code_value->asInt64() == 0; - const std::string ret_code = jsonValueToString(*ret_code_value); - const std::string message = jsonGet(response, "err_msg", "").asString(); - if (success) { - return AgvResult::success(); - } - std::string detail = "SEER Robokit command failed: ret_code=" + ret_code; - if (!message.empty()) { - detail += ", err_msg=" + message; - } - return AgvResult::failure(AgvErrorCode::CommandFailed, detail); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp b/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp deleted file mode 100644 index b1109f12..00000000 --- a/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp +++ /dev/null @@ -1,5541 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include "seer_robokit_agv.h" - -namespace cmvr::device { - -class SeerRobokitAgvTestPeer { -public: - static void installSockets( - SeerRobokitAgv& agv, - const int status, - const int control, - const int navigation, - const int config, - const int other) - { - agv.sock_status_ = status; - agv.sock_control_ = control; - agv.sock_navigation_ = navigation; - agv.sock_config_ = config; - agv.sock_other_ = other; - } - - static void cacheRuntimeState(SeerRobokitAgv& agv, const Json::Value& payload) - { - agv.updateCachedRuntimeState_(payload); - } - - static void setAdapterError(SeerRobokitAgv& agv, std::string error) - { - std::lock_guard lock(agv.mutex_); - agv.last_error_ = std::move(error); - } - - static void setFaultStateUnknown(SeerRobokitAgv& agv) - { - agv.invalidateControllerFaultState_(); - } - - static void setFaultStateAge( - SeerRobokitAgv& agv, - const std::chrono::milliseconds age) - { - std::lock_guard lock(agv.runtime_state_mutex_); - agv.controller_fault_state_observed_ = true; - agv.controller_fault_state_observed_at_ = - std::chrono::steady_clock::now() - age; - agv.active_controller_fault_detail_.clear(); - } - - static bool hasTrackedPoseTask(const SeerRobokitAgv& agv) - { - SeerRobokitAgv::PoseTaskContext context; - return agv.currentPoseTask_(context); - } - - static bool hasTrackedNavigation( - const SeerRobokitAgv& agv, - const AgvTaskType expected_type) - { - SeerRobokitAgv::TrackedNavigationContext context; - return agv.currentTrackedNavigation_(context) - && context.type == expected_type; - } - - static AgvResult disconnect(SeerRobokitAgv& agv) - { - return agv.disconnect_(); - } - - static void setNavigationReceiveTimeout( - SeerRobokitAgv& agv, - const std::chrono::milliseconds timeout) - { - timeval value{}; - value.tv_sec = static_cast(timeout.count() / 1000); - value.tv_usec = static_cast( - (timeout.count() % 1000) * 1000); - ASSERT_EQ( - ::setsockopt( - agv.sock_navigation_, - SOL_SOCKET, - SO_RCVTIMEO, - &value, - sizeof(value)), - 0); - } - - static void setStatusReceiveTimeout( - SeerRobokitAgv& agv, - const std::chrono::milliseconds timeout) - { - timeval value{}; - value.tv_sec = static_cast(timeout.count() / 1000); - value.tv_usec = static_cast( - (timeout.count() % 1000) * 1000); - ASSERT_EQ( - ::setsockopt( - agv.sock_status_, - SOL_SOCKET, - SO_RCVTIMEO, - &value, - sizeof(value)), - 0); - } - - static void closeNavigationSocket(SeerRobokitAgv& agv) - { - std::lock_guard lock(agv.mutex_); - agv.closeSocket_(agv.sock_navigation_); - } -}; - -namespace { - -constexpr std::uint16_t kRobotStatusTask = 1020; -constexpr std::uint16_t kRobotStatusLoc = 1004; -constexpr std::uint16_t kRobotStatusAll2 = 1101; -constexpr std::uint16_t kRobotStatusTaskPackage = 1110; -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 kRobotTaskClearTargetList = 3067; -constexpr std::uint16_t kRobotConfigLock = 4005; -constexpr std::uint16_t kRobotConfigUploadMap = 4010; -constexpr std::uint16_t kRobotConfigDownloadMap = 4011; -constexpr std::uint16_t kRobotOtherStartMapping = 6100; -constexpr std::uint16_t kRobotOtherStopMapping = 6101; - -enum class Channel : std::size_t { - Status = 0, - Control, - Navigation, - Config, - Other, - Count -}; - -struct CommandRecord { - std::uint16_t command{0}; - std::string payload; -}; - -Json::Value parsePayload(const CommandRecord& record) -{ - Json::Value payload; - Json::CharReaderBuilder builder; - std::string error; - std::unique_ptr reader(builder.newCharReader()); - if (!reader->parse( - record.payload.data(), - record.payload.data() + record.payload.size(), - &payload, - &error)) { - ADD_FAILURE() << "Failed to parse command " << record.command - << " payload: " << error; - } - return payload; -} - -const Json::Value& payloadValue(const Json::Value& payload, const char* key) -{ - const auto* value = payload.find(key, key + std::strlen(key)); - if (!value) { - ADD_FAILURE() << "Missing JSON field: " << key; - static const Json::Value null_value; - return null_value; - } - return *value; -} - -bool payloadHas(const Json::Value& payload, const char* key) -{ - return payload.find(key, key + std::strlen(key)) != nullptr; -} - -AgvMotionOptions asynchronousMotionOptions() -{ - AgvMotionOptions options; - options.asynchronous = true; - return options; -} - -bool receiveExact(const int fd, void* output, const std::size_t size) -{ - auto* bytes = static_cast(output); - std::size_t offset = 0; - while (offset < size) { - const auto count = ::recv(fd, bytes + offset, size - offset, 0); - if (count > 0) { - offset += static_cast(count); - continue; - } - if (count < 0 && errno == EINTR) { - continue; - } - return false; - } - return true; -} - -bool sendAll(const int fd, const std::vector& data) -{ - std::size_t offset = 0; - while (offset < data.size()) { - const auto count = ::send( - fd, - data.data() + offset, - data.size() - offset, - MSG_NOSIGNAL); - if (count > 0) { - offset += static_cast(count); - continue; - } - if (count < 0 && errno == EINTR) { - continue; - } - return false; - } - return true; -} - -std::vector responseFrame( - const std::uint16_t response_command, - const std::string& payload) -{ - std::vector frame(16 + payload.size(), 0); - frame[0] = 0x5A; - frame[1] = 0x01; - 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((response_command >> 8U) & 0xFFU); - frame[9] = static_cast(response_command & 0xFFU); - std::copy(payload.begin(), payload.end(), frame.begin() + 16); - return frame; -} - -std::string injectRequestedTaskId( - std::string response_payload, - const std::string& request_payload) -{ - constexpr char kTaskIdToken[] = "${TASK_ID}"; - Json::Value request; - Json::CharReaderBuilder builder; - std::string error; - std::unique_ptr reader(builder.newCharReader()); - if (!reader->parse( - request_payload.data(), - request_payload.data() + request_payload.size(), - &request, - &error)) { - return response_payload; - } - const auto* task_ids = request.find("task_ids", "task_ids" + std::strlen("task_ids")); - if (!task_ids || !task_ids->isArray() || task_ids->empty()) { - return response_payload; - } - - const auto replace_all = [&response_payload]( - const std::string& token, - const std::string& value) { - std::size_t position = 0; - while ((position = response_payload.find(token, position)) - != std::string::npos) { - response_payload.replace(position, token.size(), value); - position += value.size(); - } - }; - for (Json::ArrayIndex index = 0; index < task_ids->size(); ++index) { - replace_all( - "${TASK_ID_" + std::to_string(index) + "}", - (*task_ids)[index].asString()); - } - replace_all(kTaskIdToken, (*task_ids)[0].asString()); - return response_payload; -} - -class FakeSeerRobokitController { -public: - FakeSeerRobokitController() - { - for (auto& endpoint : endpoints_) { - int pair[2]{-1, -1}; - if (::socketpair(AF_UNIX, SOCK_STREAM, 0, pair) != 0) { - throw std::runtime_error("socketpair failed"); - } - endpoint.client = pair[0]; - endpoint.server = pair[1]; - } - for (std::size_t index = 0; index < endpoints_.size(); ++index) { - endpoints_[index].worker = std::thread( - &FakeSeerRobokitController::serve, - this, - index); - } - } - - ~FakeSeerRobokitController() - { - for (auto& endpoint : endpoints_) { - if (endpoint.client >= 0) { - ::shutdown(endpoint.client, SHUT_RDWR); - ::close(endpoint.client); - endpoint.client = -1; - } - if (endpoint.server >= 0) { - ::shutdown(endpoint.server, SHUT_RDWR); - } - } - for (auto& endpoint : endpoints_) { - if (endpoint.worker.joinable()) { - endpoint.worker.join(); - } - if (endpoint.server >= 0) { - ::close(endpoint.server); - endpoint.server = -1; - } - } - } - - int takeClient(const Channel channel) - { - auto& endpoint = endpoints_[static_cast(channel)]; - const int client = endpoint.client; - endpoint.client = -1; - return client; - } - - void setResponseCode(const std::uint16_t command, const int ret_code) - { - std::lock_guard lock(response_codes_mutex_); - response_codes_[command] = ret_code; - response_payloads_.erase(command); - } - - void setResponsePayload(const std::uint16_t command, std::string payload) - { - std::lock_guard lock(response_codes_mutex_); - response_codes_.erase(command); - response_payloads_[command] = {std::move(payload)}; - } - - void queueResponsePayload(const std::uint16_t command, std::string payload) - { - std::lock_guard lock(response_codes_mutex_); - response_codes_.erase(command); - response_payloads_[command].push_back(std::move(payload)); - } - - void setResponseDelay( - const std::uint16_t command, - const std::chrono::milliseconds delay) - { - std::lock_guard lock(response_codes_mutex_); - response_delays_[command] = delay; - } - - void setResponseCommand( - const std::uint16_t request_command, - const std::uint16_t response_command) - { - std::lock_guard lock(response_codes_mutex_); - response_commands_[request_command] = response_command; - } - - void clearRecords() - { - std::lock_guard lock(records_mutex_); - records_.clear(); - } - - std::vector records() const - { - std::lock_guard lock(records_mutex_); - return records_; - } - -private: - struct Endpoint { - int client{-1}; - int server{-1}; - std::thread worker; - }; - - void serve(const std::size_t index) - { - const int fd = endpoints_[index].server; - while (true) { - std::array header{}; - if (!receiveExact(fd, header.data(), header.size())) { - return; - } - const auto length = (static_cast(header[4]) << 24U) - | (static_cast(header[5]) << 16U) - | (static_cast(header[6]) << 8U) - | static_cast(header[7]); - const auto command = static_cast( - (static_cast(header[8]) << 8U) | header[9]); - std::string payload(length, '\0'); - if (length > 0 && !receiveExact(fd, payload.data(), payload.size())) { - return; - } - { - std::lock_guard lock(records_mutex_); - records_.push_back({command, payload}); - } - int ret_code = 0; - std::string response_payload; - std::chrono::milliseconds response_delay{0}; - std::uint16_t response_command = static_cast( - command + 10000U); - { - std::lock_guard lock(response_codes_mutex_); - const auto payloads = response_payloads_.find(command); - if (payloads != response_payloads_.end() && !payloads->second.empty()) { - response_payload = payloads->second.front(); - if (payloads->second.size() > 1U) { - payloads->second.pop_front(); - } - } - const auto response_code = response_codes_.find(command); - if (response_code != response_codes_.end()) { - ret_code = response_code->second; - } - const auto delay = response_delays_.find(command); - if (delay != response_delays_.end()) { - response_delay = delay->second; - } - const auto response_command_override = response_commands_.find(command); - if (response_command_override != response_commands_.end()) { - response_command = response_command_override->second; - } - } - if (response_delay.count() > 0) { - std::this_thread::sleep_for(response_delay); - } - if (response_payload.empty()) { - response_payload = ret_code == 0 - ? R"({"ret_code":0,"err_msg":""})" - : "{\"ret_code\":" + std::to_string(ret_code) - + R"(,"err_msg":"simulated command failure"})"; - } - response_payload = injectRequestedTaskId( - std::move(response_payload), - payload); - if (!sendAll(fd, responseFrame(response_command, response_payload))) { - return; - } - } - } - - std::array(Channel::Count)> endpoints_; - mutable std::mutex records_mutex_; - std::vector records_; - std::mutex response_codes_mutex_; - std::unordered_map response_codes_; - std::unordered_map> response_payloads_; - std::unordered_map response_delays_; - std::unordered_map response_commands_; -}; - -class SeerRobokitControlAuthorityTest : public ::testing::Test { -protected: - void SetUp() override - { - config::SeerRobokitAgvConfig cfg; - cfg.set_id("src1100"); - cfg.set_ip("invalid-ip"); - cfg.set_recv_timeout_ms(100); - cfg.set_control_nick_name("cmvr-test"); - cfg.set_enable_state_push(true); - cfg.set_state_push_interval_ms(200); - controller_.setResponsePayload( - kRobotStatusTask, - R"({"ret_code":0,"err_msg":"","task_status":2,"task_type":1,"target_point":[1.0,2.0,0.5]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"err_msg":"","task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"err_msg":"","task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[],"warnings":[]})"); - agv_ = std::make_unique(cfg); - const int status_socket = controller_.takeClient(Channel::Status); - const int control_socket = controller_.takeClient(Channel::Control); - const int navigation_socket = controller_.takeClient(Channel::Navigation); - const int config_socket = controller_.takeClient(Channel::Config); - const int other_socket = controller_.takeClient(Channel::Other); - SeerRobokitAgvTestPeer::installSockets( - *agv_, - status_socket, - control_socket, - navigation_socket, - config_socket, - other_socket); - Json::Value fault_state_push(Json::objectValue); - *fault_state_push.demand( - "errors", - "errors" + std::strlen("errors")) = - Json::Value(Json::arrayValue); - *fault_state_push.demand( - "fatals", - "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_state_push); - } - - void TearDown() override - { - agv_.reset(); - } - - void expectControlledSequence( - const std::vector& commands, - const std::function& invoke) - { - controller_.clearRecords(); - const auto result = invoke(); - ASSERT_TRUE(result.ok()) << result.message; - - const auto records = controller_.records(); - ASSERT_EQ(records.size(), commands.size() + 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - for (std::size_t index = 0; index < commands.size(); ++index) { - EXPECT_EQ(records[index + 1U].command, commands[index]); - } - - Json::Value lock_payload; - Json::CharReaderBuilder builder; - std::string error; - std::unique_ptr reader(builder.newCharReader()); - ASSERT_TRUE(reader->parse( - records[0].payload.data(), - records[0].payload.data() + records[0].payload.size(), - &lock_payload, - &error)) << error; - constexpr char kNickName[] = "nick_name"; - const auto* nick_name = lock_payload.find( - kNickName, - kNickName + std::strlen(kNickName)); - ASSERT_NE(nick_name, nullptr); - EXPECT_EQ(nick_name->asString(), "cmvr-test"); - } - - void expectControlled( - const std::uint16_t command, - const std::function& invoke) - { - expectControlledSequence({command}, invoke); - } - - bool waitForCommandCount( - const std::uint16_t command, - const std::size_t expected_count, - const int max_attempts = 3000) const - { - for (int attempt = 0; attempt < max_attempts; ++attempt) { - const auto records = controller_.records(); - const auto count = static_cast(std::count_if( - records.begin(), - records.end(), - [command](const CommandRecord& record) { - return record.command == command; - })); - if (count >= expected_count) { - return true; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - return false; - } - - FakeSeerRobokitController controller_; - std::unique_ptr agv_; -}; - -TEST_F(SeerRobokitControlAuthorityTest, DeclaresSynchronousActionSupport) -{ - EXPECT_TRUE(agv_->supportsSynchronousAction( - AgvActionKind::NavigateToPose)); - EXPECT_TRUE(agv_->supportsSynchronousAction( - AgvActionKind::NavigateToStation)); - EXPECT_TRUE(agv_->supportsSynchronousAction( - AgvActionKind::FollowPath)); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - ConfirmMotionStoppedWithoutTrackedTaskUsesGlobalStatusOnly) -{ - controller_.clearRecords(); - - const auto result = agv_->confirmMotionStopped(); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusAll2; - }), - 2); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - ConfirmMotionStoppedForTrackedStationChecksExactTaskAndGlobalVelocity) -{ - const auto navigate_result = agv_->navigateToStation( - "station-confirm-stop", - asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-confirm-stop","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.clearRecords(); - - const auto result = agv_->confirmMotionStopped(); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - EXPECT_GE( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }), - 2); - EXPECT_GE( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusAll2; - }), - 2); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - ConfirmMotionStoppedTimesOutWhenTrackedTaskIsTerminalButVelocityIsNonzero) -{ - const auto navigate_result = agv_->navigateToStation( - "station-still-moving", - asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-still-moving","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.clearRecords(); - - const auto result = agv_->confirmMotionStopped(); - - EXPECT_EQ(result.code, AgvErrorCode::Timeout); - EXPECT_NE( - result.message.find("stopped velocity were not confirmed"), - std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - ConfirmMotionStoppedWaitsForGlobalTaskToBecomeTerminal) -{ - const auto navigate_result = agv_->navigateToStation( - "station-global-active", - asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-global-active","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.clearRecords(); - std::atomic finished{false}; - AgvResult result; - - std::thread confirmation([this, &finished, &result]() { - result = agv_->confirmMotionStopped(); - finished.store(true, std::memory_order_release); - }); - const bool zero_samples_observed = waitForCommandCount( - kRobotStatusAll2, 2, 1000); - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - const bool returned_while_global_active = - finished.load(std::memory_order_acquire); - - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-global-active","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - confirmation.join(); - - EXPECT_TRUE(zero_samples_observed); - EXPECT_FALSE(returned_while_global_active); - EXPECT_TRUE(result.ok()) << result.message; -} - -TEST_F(SeerRobokitControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) -{ - controller_.clearRecords(); - const auto pose_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(pose_result.ok()) << pose_result.message; - const auto pose_records = controller_.records(); - ASSERT_GE(pose_records.size(), 4U); - EXPECT_EQ(pose_records[0].command, kRobotConfigLock); - EXPECT_EQ(pose_records[1].command, kRobotTaskGoTarget); - for (std::size_t index = 2; index < pose_records.size(); ++index) { - EXPECT_EQ(pose_records[index].command, kRobotStatusTaskPackage); - } - expectControlled(kRobotTaskGoTarget, [this]() { - return agv_->navigateToStation("station-1", asynchronousMotionOptions()); - }); - expectControlled(kRobotTaskGoTargetList, [this]() { - return agv_->followPath( - {AgvPathSegment{"station-1", "station-2"}}, - asynchronousMotionOptions()); - }); - expectControlled(kRobotTaskPause, [this]() { - return agv_->pauseNavigation(); - }); - expectControlled(kRobotTaskResume, [this]() { - return agv_->resumeNavigation(); - }); - controller_.clearRecords(); - const auto cancel_result = agv_->cancelNavigation(); - ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; - const auto cancel_records = controller_.records(); - ASSERT_EQ(cancel_records.size(), 4U); - EXPECT_EQ(cancel_records[0].command, kRobotStatusTaskPackage); - EXPECT_EQ(cancel_records[1].command, kRobotStatusAll2); - EXPECT_EQ(cancel_records[2].command, kRobotConfigLock); - EXPECT_EQ(cancel_records[3].command, kRobotTaskClearTargetList); - expectControlledSequence( - {kRobotControlStop, kRobotTaskClearTargetList}, - [this]() { - return agv_->emergencyStop(); - }); - expectControlled(kRobotControlMotion, [this]() { - return agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.2}); - }); - expectControlled(kRobotControlMotion, [this]() { - return agv_->stopVelocityControl(); - }); - expectControlled(kRobotControlLoadMap, [this]() { - return agv_->switchMap("map-1"); - }); - expectControlled(kRobotConfigUploadMap, [this]() { - return agv_->uploadMap("map-1", "{}"); - }); - expectControlled(kRobotOtherStartMapping, [this]() { - return agv_->startMapping(); - }); - expectControlled(kRobotOtherStopMapping, [this]() { - return agv_->stopMapping(); - }); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousPoseNavigationWaitsForVerifiedCompletionAndStoppedVelocity) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":1,"blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5,"confidence":1.0})"); - AgvMotionOptions options; - options.asynchronous = false; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - finished.store(true, std::memory_order_release); - }); - - const bool terminal_wait_observed = waitForCommandCount( - kRobotStatusAll2, - 2); - EXPECT_TRUE(terminal_wait_observed); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_TRUE(finished.load(std::memory_order_acquire)); - const auto records = controller_.records(); - EXPECT_GE( - static_cast(std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusAll2; - })), - 3U); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousPoseRechecksTargetAfterCompletedTaskStops) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.2,"y":2.0,"angle":0.5})"); - AgvMotionOptions options; - options.wait_timeout_ms = 2000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - finished.store(true, std::memory_order_release); - }); - - const bool moving_completion_observed = - waitForCommandCount(kRobotStatusAll2, 2); - EXPECT_TRUE(moving_completion_observed); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - const auto moving_records = controller_.records(); - EXPECT_EQ( - std::count_if( - moving_records.begin(), - moving_records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusLoc; - }), - 0); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE( - result.message.find("final pose was outside"), - std::string::npos); - EXPECT_NE( - result.message.find("distance_error=0.2"), - std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousPoseReconcilesIndeterminateCommandAcknowledgment) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":1,"blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.setResponsePayload( - kRobotTaskGoTarget, - R"({"err_msg":"pose acknowledgment lost"})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - finished.store(true, std::memory_order_release); - }); - - const bool pose_status_observed = waitForCommandCount( - kRobotStatusTaskPackage, - 3, - 1000); - EXPECT_TRUE(pose_status_observed); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_NE( - result.message.find("indeterminate command acknowledgment"), - std::string::npos); - EXPECT_NE( - result.message.find("missing a numeric ret_code"), - std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - PoseWaitDoesNotLetGlobalTerminalOverrideExactRunningAfterGrace) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":5,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - finished.store(true, std::memory_order_release); - }); - - const bool grace_elapsed_while_polling = waitForCommandCount( - kRobotStatusTaskPackage, - 90, - 2500); - EXPECT_TRUE(grace_elapsed_while_polling); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - const auto records_before_completion = controller_.records(); - EXPECT_EQ( - std::count_if( - records_before_completion.begin(), - records_before_completion.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousStationNavigationWaitsForExactTaskCompletion) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-2","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); - AgvMotionOptions options; - options.asynchronous = false; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToStation("station-2", options); - finished.store(true, std::memory_order_release); - }); - - const bool terminal_wait_observed = waitForCommandCount( - kRobotStatusAll2, - 2); - EXPECT_TRUE(terminal_wait_observed); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-2","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - const auto command = std::find_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTarget; - }); - ASSERT_NE(command, records.end()); - const auto payload = parsePayload(*command); - EXPECT_FALSE(payloadValue(payload, "task_id").asString().empty()); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousStationAcceptsExactCompletionAfterGlobalStateClears) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); - AgvMotionOptions options; - options.wait_timeout_ms = 2500; - options.poll_interval_ms = 20; - - const auto result = agv_->navigateToStation( - "station-cleared", - options); - - ASSERT_TRUE(result.ok()) << result.message; -} - -TEST_F( - SeerRobokitControlAuthorityTest, - StationWaitIgnoresStaleGlobalTaskUntilExactTaskIdAppears) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":1,"blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":5,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-delayed","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); - AgvMotionOptions options; - options.wait_timeout_ms = 2000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToStation("station-delayed", options); - finished.store(true, std::memory_order_release); - }); - - const bool exact_task_appeared = waitForCommandCount( - kRobotStatusTaskPackage, - 3, - 1000); - EXPECT_TRUE(exact_task_appeared); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - const auto records_before_completion = controller_.records(); - EXPECT_EQ( - std::count_if( - records_before_completion.begin(), - records_before_completion.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-delayed","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousStationReconcilesIndeterminateCommandAcknowledgment) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-ack","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); - controller_.setResponsePayload( - kRobotTaskGoTarget, - R"({"err_msg":"station acknowledgment lost"})"); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToStation("station-ack", options); - finished.store(true, std::memory_order_release); - }); - - const bool station_status_observed = waitForCommandCount( - kRobotStatusTaskPackage, - 3, - 1000); - EXPECT_TRUE(station_status_observed); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-ack","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_NE( - result.message.find("indeterminate command acknowledgment"), - std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - StationWaitDoesNotLetGlobalTerminalOverrideExactRunningAfterGrace) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":5,"task_type":2,"target_id":"station-current","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToStation("station-current", options); - finished.store(true, std::memory_order_release); - }); - - const bool grace_elapsed_while_polling = waitForCommandCount( - kRobotStatusTaskPackage, - 90, - 2500); - EXPECT_TRUE(grace_elapsed_while_polling); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - const auto records_before_completion = controller_.records(); - EXPECT_EQ( - std::count_if( - records_before_completion.begin(), - records_before_completion.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-current","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousStationMapsTaskStatusSevenToFailedAfterStop) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":7,"task_type":2,"target_id":"station-timeout","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":7,"type":2}]}})"); - AgvMotionOptions options; - options.wait_timeout_ms = 1000; - options.poll_interval_ms = 20; - controller_.clearRecords(); - - const auto result = agv_->navigateToStation( - "station-timeout", - options); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("task_status=7"), std::string::npos); - EXPECT_NE( - result.message.find("stopped velocity confirmed"), - std::string::npos); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousFollowPathWaitsForFinalExactTaskAndStoppedVelocity) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":2},{"task_id":"${TASK_ID_1}","status":1}]}})"); - AgvMotionOptions options; - options.asynchronous = false; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->followPath( - { - AgvPathSegment{"station-1", "station-2"}, - AgvPathSegment{"station-2", "station-3"}, - }, - options); - finished.store(true, std::memory_order_release); - }); - - const bool terminal_wait_observed = waitForCommandCount( - kRobotStatusAll2, - 2); - EXPECT_TRUE(terminal_wait_observed); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - - const auto records_before_completion = controller_.records(); - const auto all2_count_before_completion = static_cast( - std::count_if( - records_before_completion.begin(), - records_before_completion.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusAll2; - })); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":4},{"task_id":"${TASK_ID_1}","status":4}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - const bool completed_moving_sample_observed = waitForCommandCount( - kRobotStatusAll2, - all2_count_before_completion + 2U); - EXPECT_TRUE(completed_moving_sample_observed); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - const auto command = std::find_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTargetList; - }); - ASSERT_NE(command, records.end()); - const auto payload = parsePayload(*command); - const auto& tasks = payloadValue(payload, "move_task_list"); - ASSERT_TRUE(tasks.isArray()); - ASSERT_EQ(tasks.size(), 2U); - const std::string first_task_id = - payloadValue(tasks[0], "task_id").asString(); - const std::string final_task_id = - payloadValue(tasks[1], "task_id").asString(); - EXPECT_FALSE(first_task_id.empty()); - EXPECT_FALSE(final_task_id.empty()); - EXPECT_NE(first_task_id, final_task_id); - - const auto status_query = std::find_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - ASSERT_NE(status_query, records.end()); - const auto status_payload = parsePayload(*status_query); - const auto& requested_ids = payloadValue(status_payload, "task_ids"); - ASSERT_TRUE(requested_ids.isArray()); - ASSERT_EQ(requested_ids.size(), 2U); - EXPECT_EQ(requested_ids[0].asString(), first_task_id); - EXPECT_EQ(requested_ids[1].asString(), final_task_id); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - FollowPathDoesNotFinishWhileEarlierExactSegmentIsStillActive) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":2},{"task_id":"${TASK_ID_1}","status":4}]}})"); - AgvMotionOptions options; - options.wait_timeout_ms = 2000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->followPath( - { - AgvPathSegment{"station-1", "station-2"}, - AgvPathSegment{"station-2", "station-3"}, - }, - options); - finished.store(true, std::memory_order_release); - }); - - const bool continued_polling = waitForCommandCount( - kRobotStatusTaskPackage, - 5, - 1000); - EXPECT_TRUE(continued_polling); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":4},{"task_id":"${TASK_ID_1}","status":4}]}})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousFollowPathReconcilesIndeterminateCommandAcknowledgment) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":2},{"task_id":"${TASK_ID_1}","status":1}]}})"); - controller_.setResponsePayload( - kRobotTaskGoTargetList, - R"({"err_msg":"path acknowledgment lost"})"); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->followPath( - { - AgvPathSegment{"station-1", "station-2"}, - AgvPathSegment{"station-2", "station-3"}, - }, - options); - finished.store(true, std::memory_order_release); - }); - - const bool path_status_observed = waitForCommandCount( - kRobotStatusTaskPackage, - 3, - 1000); - EXPECT_TRUE(path_status_observed); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":4},{"task_id":"${TASK_ID_1}","status":4}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_NE( - result.message.find("indeterminate command acknowledgment"), - std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - TerminalPathTimeoutRetainsTrackingUntilStoppedVelocityIsConfirmed) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":4},{"task_id":"${TASK_ID_1}","status":4}]}})"); - AgvMotionOptions options; - options.wait_timeout_ms = 100; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->followPath( - { - AgvPathSegment{"station-1", "station-2"}, - AgvPathSegment{"station-2", "station-3"}, - }, - options); - finished.store(true, std::memory_order_release); - }); - - const bool timeout_cleanup_started = waitForCommandCount( - kRobotStatusAll2, - 2, - 1000); - EXPECT_TRUE(timeout_cleanup_started); - std::this_thread::sleep_for(std::chrono::milliseconds(150)); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedNavigation( - *agv_, - AgvTaskType::FollowPath)); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_EQ(result.code, AgvErrorCode::Timeout); - EXPECT_NE( - result.message.find("stopped state was confirmed"), - std::string::npos); - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedNavigation( - *agv_, - AgvTaskType::FollowPath)); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskClearTargetList; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - TerminalPoseTimeoutRetainsTrackingUntilStoppedVelocityIsConfirmed) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - AgvMotionOptions options; - options.wait_timeout_ms = 100; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - finished.store(true, std::memory_order_release); - }); - - const bool timeout_cleanup_started = waitForCommandCount( - kRobotStatusAll2, - 2, - 1000); - EXPECT_TRUE(timeout_cleanup_started); - std::this_thread::sleep_for(std::chrono::milliseconds(150)); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedNavigation( - *agv_, - AgvTaskType::NavigateToPose)); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_EQ(result.code, AgvErrorCode::Timeout); - EXPECT_NE( - result.message.find("stopped state was confirmed"), - std::string::npos); - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedNavigation( - *agv_, - AgvTaskType::NavigateToPose)); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - PersistentBlockedNavigationConditionallyCancelsAndConfirmsStoppedTask) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-blocked","blocked":true,"block_reason":3,"block_x":1.2,"block_y":0.1,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); - controller_.setResponseDelay( - kRobotTaskCancel, - std::chrono::milliseconds(50)); - AgvMotionOptions options; - options.asynchronous = false; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToStation("station-blocked", options); - finished.store(true, std::memory_order_release); - }); - - const bool cancel_observed = waitForCommandCount(kRobotTaskCancel, 1); - EXPECT_TRUE(cancel_observed); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":6,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":6,"task_type":2,"target_id":"station-blocked","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("block_reason=3(collision)"), std::string::npos); - EXPECT_NE(result.message.find("canceled"), std::string::npos); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 1); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotConfigLock; - }), - 2); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - FollowPathUsesUniqueTaskIdsAcrossCalls) -{ - controller_.clearRecords(); - const auto first_result = agv_->followPath( - {AgvPathSegment{"station-1", "station-2"}}, - asynchronousMotionOptions()); - ASSERT_TRUE(first_result.ok()) << first_result.message; - const auto first_records = controller_.records(); - const auto first_command = std::find_if( - first_records.begin(), - first_records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTargetList; - }); - ASSERT_NE(first_command, first_records.end()); - const auto first_payload = parsePayload(*first_command); - const std::string first_task_id = payloadValue( - payloadValue(first_payload, "move_task_list")[0], - "task_id").asString(); - - controller_.clearRecords(); - const auto second_result = agv_->followPath( - {AgvPathSegment{"station-1", "station-2"}}, - asynchronousMotionOptions()); - ASSERT_TRUE(second_result.ok()) << second_result.message; - const auto second_records = controller_.records(); - const auto second_command = std::find_if( - second_records.begin(), - second_records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTargetList; - }); - ASSERT_NE(second_command, second_records.end()); - const auto second_payload = parsePayload(*second_command); - const std::string second_task_id = payloadValue( - payloadValue(second_payload, "move_task_list")[0], - "task_id").asString(); - - EXPECT_FALSE(first_task_id.empty()); - EXPECT_FALSE(second_task_id.empty()); - EXPECT_NE(first_task_id, second_task_id); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - ExactTaskStatusWithoutTypeStillConfirmsAsynchronousPoseStart) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2}]}})"); - - const auto result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - asynchronousMotionOptions()); - - ASSERT_TRUE(result.ok()) << result.message; -} - -TEST_F( - SeerRobokitControlAuthorityTest, - PoseStartTreatsTaskStatus404AsNotYetPresent) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":404,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - asynchronousMotionOptions()); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - EXPECT_GE( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }), - 3); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - PoseStartMapsControllerTaskStatusSevenToTaskFailed) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"controller terminated free navigation","task_status_list":[{"task_id":"${TASK_ID}","status":7,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("controller terminated free navigation"), std::string::npos); - EXPECT_NE(result.message.find("task_status=7"), std::string::npos); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - CancelNavigationClearsTrackedAsynchronousPathWithCommand3067) -{ - const auto path_result = agv_->followPath( - {AgvPathSegment{"station-1", "station-2"}}, - asynchronousMotionOptions()); - ASSERT_TRUE(path_result.ok()) << path_result.message; - ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedNavigation( - *agv_, - AgvTaskType::FollowPath)); - controller_.clearRecords(); - - const auto cancel_result = agv_->cancelNavigation(); - - ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskClearTargetList; - }), - 1); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigationDefaultsToSynchronousAndRejectsUnboundedPolling) -{ - EXPECT_FALSE(AgvMotionOptions{}.asynchronous); - - AgvMotionOptions options; - options.poll_interval_ms = 5001; - controller_.clearRecords(); - const auto result = agv_->navigateToStation("station-1", options); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("must not exceed 5000"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - PreCanceledNavigationDoesNotAcquireAuthorityOrSendMotion) -{ - AgvMotionOptions options; - options.cancellation_requested = []() { return true; }; - - controller_.clearRecords(); - const auto pose_result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - EXPECT_EQ(pose_result.code, AgvErrorCode::TaskCanceled); - EXPECT_TRUE(controller_.records().empty()); - - const auto station_result = - agv_->navigateToStation("station-1", options); - EXPECT_EQ(station_result.code, AgvErrorCode::TaskCanceled); - EXPECT_TRUE(controller_.records().empty()); - - const auto path_result = agv_->followPath( - {AgvPathSegment{"station-1", "station-2"}}, - options); - EXPECT_EQ(path_result.code, AgvErrorCode::TaskCanceled); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - CancellationDuringAuthorityAcquisitionPreventsNavigationWrite) -{ - std::atomic canceled{false}; - AgvMotionOptions options = asynchronousMotionOptions(); - options.cancellation_requested = [&canceled]() { - return canceled.load(std::memory_order_acquire); - }; - controller_.setResponseDelay( - kRobotConfigLock, - std::chrono::milliseconds(100)); - controller_.clearRecords(); - AgvResult result; - - std::thread navigation_thread([this, &options, &result]() { - result = agv_->navigateToStation("station-1", options); - }); - ASSERT_TRUE(waitForCommandCount(kRobotConfigLock, 1)); - canceled.store(true, std::memory_order_release); - navigation_thread.join(); - - EXPECT_EQ(result.code, AgvErrorCode::TaskCanceled); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTarget; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousStationPublicCancelStillWaitsForTerminalZeroVelocity) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-public-cancel","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); - std::atomic caller_canceled{false}; - AgvMotionOptions options; - options.wait_timeout_ms = 2500; - options.poll_interval_ms = 20; - options.cancellation_requested = [&caller_canceled]() { - return caller_canceled.load(std::memory_order_acquire); - }; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread( - [this, &options, &finished, &result]() { - result = agv_->navigateToStation( - "station-public-cancel", - options); - finished.store(true, std::memory_order_release); - }); - - const bool terminal_wait_started = waitForCommandCount( - kRobotStatusAll2, - 2, - 1500); - const auto public_cancel_result = agv_->cancelNavigation(); - caller_canceled.store(true, std::memory_order_release); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - const bool returned_while_exact_task_was_active = - finished.load(std::memory_order_acquire); - - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":6,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":6,"task_type":2,"target_id":"station-public-cancel","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_TRUE(terminal_wait_started); - ASSERT_TRUE(public_cancel_result.ok()) - << public_cancel_result.message; - EXPECT_FALSE(returned_while_exact_task_was_active); - EXPECT_EQ(result.code, AgvErrorCode::TaskCanceled); - EXPECT_NE( - result.message.find("stopped state was confirmed"), - std::string::npos); - const auto records = controller_.records(); - EXPECT_GE( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 1); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - TerminalTaskWithoutVelocityStatusEscalatesToTrackedFailSafeStop) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":41101,"err_msg":"1101 unavailable after terminal task"})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); - AgvMotionOptions options; - options.wait_timeout_ms = 100; - options.poll_interval_ms = 20; - controller_.clearRecords(); - - const auto result = agv_->navigateToStation( - "station-terminal-no-velocity", - options); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Timeout); - EXPECT_NE( - result.message.find("tracked fail-safe software stop"), - std::string::npos); - EXPECT_NE( - result.message.find("stopped state remains unconfirmed"), - std::string::npos); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotControlStop; - }), - 1); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 1); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousPosePublicCancelStillWaitsForTerminalZeroVelocity) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":1,"blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - std::atomic caller_canceled{false}; - AgvMotionOptions options; - options.wait_timeout_ms = 2500; - options.poll_interval_ms = 20; - options.cancellation_requested = [&caller_canceled]() { - return caller_canceled.load(std::memory_order_acquire); - }; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread( - [this, &options, &finished, &result]() { - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - finished.store(true, std::memory_order_release); - }); - - const bool terminal_wait_started = waitForCommandCount( - kRobotStatusAll2, - 2, - 1500); - const auto public_cancel_result = agv_->cancelNavigation(); - caller_canceled.store(true, std::memory_order_release); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - const bool returned_while_exact_task_was_active = - finished.load(std::memory_order_acquire); - - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":6,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":6,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_TRUE(terminal_wait_started); - ASSERT_TRUE(public_cancel_result.ok()) - << public_cancel_result.message; - EXPECT_FALSE(returned_while_exact_task_was_active); - EXPECT_EQ(result.code, AgvErrorCode::TaskCanceled); - EXPECT_NE( - result.message.find("stopped state was confirmed"), - std::string::npos); - const auto records = controller_.records(); - EXPECT_GE( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 1); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousStationEmergencyStopStillWaitsForTerminalZeroVelocity) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-emergency-stop","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); - AgvMotionOptions options; - options.wait_timeout_ms = 2500; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - - std::thread navigation_thread( - [this, &options, &finished, &result]() { - result = agv_->navigateToStation( - "station-emergency-stop", - options); - finished.store(true, std::memory_order_release); - }); - - const bool terminal_wait_started = waitForCommandCount( - kRobotStatusAll2, - 2, - 1500); - const auto emergency_stop_result = agv_->emergencyStop(); - const auto records_before_terminal = controller_.records(); - const auto exact_queries_before_terminal = - static_cast(std::count_if( - records_before_terminal.begin(), - records_before_terminal.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - })); - - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":6,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":6,"task_type":2,"target_id":"station-emergency-stop","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - const bool moving_terminal_observed = waitForCommandCount( - kRobotStatusTaskPackage, - exact_queries_before_terminal + 1U, - 1000); - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - const bool returned_while_velocity_was_nonzero = - finished.load(std::memory_order_acquire); - - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":6,"task_type":2,"target_id":"station-emergency-stop","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_TRUE(terminal_wait_started); - ASSERT_TRUE(emergency_stop_result.ok()) - << emergency_stop_result.message; - EXPECT_TRUE(moving_terminal_observed); - EXPECT_FALSE(returned_while_velocity_was_nonzero); - EXPECT_EQ(result.code, AgvErrorCode::TaskCanceled); - EXPECT_NE( - result.message.find("stopped velocity confirmed"), - std::string::npos); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotControlStop; - }), - 1); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 1); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SynchronousStationStatusTimeoutStillCancelsAndReportsUnconfirmedStop) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-status-timeout","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); - SeerRobokitAgvTestPeer::setStatusReceiveTimeout( - *agv_, - std::chrono::milliseconds(50)); - AgvMotionOptions options; - options.wait_timeout_ms = 2000; - options.poll_interval_ms = 100; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &result]() { - result = agv_->navigateToStation( - "station-status-timeout", - options); - }); - - const bool running_status_observed = waitForCommandCount( - kRobotStatusAll2, - 2, - 1000); - controller_.setResponseDelay( - kRobotStatusTaskPackage, - std::chrono::milliseconds(200)); - navigation_thread.join(); - - EXPECT_TRUE(running_status_observed); - EXPECT_EQ(result.code, AgvErrorCode::Timeout); - EXPECT_NE( - result.message.find("navigation cancel was sent"), - std::string::npos); - EXPECT_NE( - result.message.find("termination could not be queried"), - std::string::npos); - EXPECT_EQ( - result.message.find("stopped state was confirmed"), - std::string::npos); - const auto records = controller_.records(); - EXPECT_GE( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 1); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotControlStop; - }), - 1); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - FollowPathAcceptsCurrentSegmentOnlyTaskStatusProgression) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-2","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":2}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_1}","status":2}]}})"); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->followPath( - { - AgvPathSegment{"station-1", "station-2"}, - AgvPathSegment{"station-2", "station-3"}, - }, - options); - finished.store(true, std::memory_order_release); - }); - - ASSERT_TRUE(waitForCommandCount(kRobotStatusTaskPackage, 3)); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_1}","status":4}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); - const auto command = std::find_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTargetList; - }); - ASSERT_NE(command, records.end()); - const auto command_payload = parsePayload(*command); - const auto& tasks = payloadValue(command_payload, "move_task_list"); - ASSERT_EQ(tasks.size(), 2U); - const std::string first_id = - payloadValue(tasks[0], "task_id").asString(); - const std::string second_id = - payloadValue(tasks[1], "task_id").asString(); - for (const auto& record : records) { - if (record.command != kRobotStatusTaskPackage) { - continue; - } - const auto payload = parsePayload(record); - const auto& requested = payloadValue(payload, "task_ids"); - ASSERT_EQ(requested.size(), 2U); - EXPECT_EQ(requested[0].asString(), first_id); - EXPECT_EQ(requested[1].asString(), second_id); - } -} - -TEST_F( - SeerRobokitControlAuthorityTest, - FailedStationTaskDoesNotReturnUntilStoppedVelocityIsConfirmed) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":5,"task_type":2,"target_id":"station-failed","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":5}]}})"); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToStation("station-failed", options); - finished.store(true, std::memory_order_release); - }); - - ASSERT_TRUE(waitForCommandCount(kRobotStatusAll2, 2)); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":5,"task_type":2,"target_id":"station-failed","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("stopped velocity confirmed"), std::string::npos); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - FailedPathSegmentCancelsRemainingActiveSegmentsBeforeReturning) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-2","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5},{"task_id":"${TASK_ID_1}","status":1}]}})"); - controller_.setResponseDelay( - kRobotTaskClearTargetList, - std::chrono::milliseconds(50)); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->followPath( - { - AgvPathSegment{"station-1", "station-2"}, - AgvPathSegment{"station-2", "station-3"}, - }, - options); - finished.store(true, std::memory_order_release); - }); - - ASSERT_TRUE(waitForCommandCount(kRobotTaskClearTargetList, 1)); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5},{"task_id":"${TASK_ID_1}","status":6}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":6,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("still active"), std::string::npos); - EXPECT_NE(result.message.find("conditionally canceled"), std::string::npos); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskClearTargetList; - }), - 1); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - CurrentSegmentOnlyFailureCancelsHiddenPathAndWaitsForGlobalTerminalStop) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-2","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - // This firmware shape reports only the current segment. The failed first - // segment is visible, while the later queued segment is temporarily absent. - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5}]}})"); - controller_.setResponseDelay( - kRobotTaskClearTargetList, - std::chrono::milliseconds(100)); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->followPath( - { - AgvPathSegment{"station-1", "station-2"}, - AgvPathSegment{"station-2", "station-3"}, - }, - options); - finished.store(true, std::memory_order_release); - }); - - const bool cancel_observed = waitForCommandCount( - kRobotTaskClearTargetList, - 1, - 750); - std::size_t all2_count_at_cancel = 0; - if (cancel_observed) { - const auto records = controller_.records(); - all2_count_at_cancel = static_cast(std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusAll2; - })); - - // The cancel ACK is delayed above, so these become the first - // post-cancel observations: one exact task is canceled, the other is - // still absent, and 1101 still says navigation is active but stationary. - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":6}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - } - - const bool active_zero_samples_observed = cancel_observed - && waitForCommandCount( - kRobotStatusAll2, - all2_count_at_cancel + 2U, - 1500); - const bool returned_before_global_terminal = - finished.load(std::memory_order_acquire); - - // Allow both the correct and the intentionally failing implementation to - // terminate cleanly before joining the test thread. - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":6}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":6,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_TRUE(cancel_observed); - EXPECT_TRUE(active_zero_samples_observed); - EXPECT_FALSE(returned_before_global_terminal); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskClearTargetList; - }), - 1); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - ExactActivePathEscalatesToFailSafeWhenGlobalOwnershipIsUnavailable) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - // If the waiter asks for 1101 after seeing exact states 5 + 1, this queued - // controller error is returned. A bare global 3067 is unsafe without the - // global ownership snapshot, so the adapter must use its explicit - // software-stop escalation sequence. - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":41101,"err_msg":"simulated 1101 failure after exact segment failure"})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5},{"task_id":"${TASK_ID_1}","status":1}]}})"); - controller_.setResponseDelay( - kRobotTaskClearTargetList, - std::chrono::milliseconds(100)); - AgvMotionOptions options; - options.wait_timeout_ms = 2000; - options.poll_interval_ms = 20; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &result]() { - result = agv_->followPath( - { - AgvPathSegment{"station-1", "station-2"}, - AgvPathSegment{"station-2", "station-3"}, - }, - options); - }); - - const bool cancel_observed = waitForCommandCount( - kRobotTaskClearTargetList, - 1, - 750); - // Replace the queued failing 1101 response while the cancel ACK is delayed, - // so a correct implementation can confirm the post-cancel terminal stop. - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5},{"task_id":"${TASK_ID_1}","status":6}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":6,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_TRUE(cancel_observed); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskClearTargetList; - }), - 1); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotControlStop; - }), - 1); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - AsynchronousPoseCancellationAfterAckCancelsAndConfirmsStoppedTask) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.setResponseDelay( - kRobotTaskCancel, - std::chrono::milliseconds(100)); - std::atomic canceled{false}; - AgvMotionOptions options = asynchronousMotionOptions(); - options.cancellation_requested = [&canceled]() { - return canceled.load(std::memory_order_acquire); - }; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &result]() { - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - }); - - // The first 1110 request proves that 3051 already returned its ACK and the - // asynchronous pose start-confirmation phase is in progress. - const bool post_ack_status_observed = waitForCommandCount( - kRobotStatusTaskPackage, - 1, - 750); - canceled.store(true, std::memory_order_release); - const bool cancel_observed = waitForCommandCount( - kRobotTaskCancel, - 1, - 750); - - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":6,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":6,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - EXPECT_TRUE(post_ack_status_observed); - EXPECT_TRUE(cancel_observed); - EXPECT_EQ(result.code, AgvErrorCode::TaskCanceled); - EXPECT_NE(result.message.find("conditionally canceled"), std::string::npos); - EXPECT_NE(result.message.find("stopped state was confirmed"), std::string::npos); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 1); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - GlobalTaskStatusSevenDoesNotOverrideExactRunningSynchronousPose) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":7,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &finished, &result]() { - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - finished.store(true, std::memory_order_release); - }); - - const bool grace_elapsed_while_exact_running = waitForCommandCount( - kRobotStatusTaskPackage, - 90, - 2500); - EXPECT_TRUE(grace_elapsed_while_exact_running); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - const auto records_before_completion = controller_.records(); - EXPECT_EQ( - std::count_if( - records_before_completion.begin(), - records_before_completion.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskClearTargetList; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - CompletedPoseDoesNotFinishAfterExactTaskReturnsToRunning) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread( - [this, &options, &finished, &result]() { - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - finished.store(true, std::memory_order_release); - }); - - const bool running_after_completed_observed = waitForCommandCount( - kRobotStatusTaskPackage, - 10, - 1500); - EXPECT_TRUE(running_after_completed_observed); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigationStatusDoesNotSupersedeSynchronousPoseWaiter) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":1,"blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - std::atomic finished{false}; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread( - [this, &options, &finished, &result]() { - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - finished.store(true, std::memory_order_release); - }); - - const bool terminal_wait_started = waitForCommandCount( - kRobotStatusAll2, - 2, - 2000); - const auto observed_status = agv_->navigationStatus(); - EXPECT_TRUE(terminal_wait_started); - EXPECT_EQ(observed_status.state, AgvTaskState::Running); - EXPECT_FALSE(finished.load(std::memory_order_acquire)); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - navigation_thread.join(); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - ConditionalCancelRefusesDifferentActiveControllerTask) -{ - const auto navigation_result = agv_->navigateToStation( - "station-owned", - asynchronousMotionOptions()); - ASSERT_TRUE(navigation_result.ok()) << navigation_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-external","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.clearRecords(); - - const auto cancel_result = agv_->cancelNavigation(); - - EXPECT_FALSE(cancel_result.ok()); - EXPECT_EQ(cancel_result.code, AgvErrorCode::TaskCanceled); - EXPECT_NE( - cancel_result.message.find("another active task"), - std::string::npos); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotConfigLock - || record.command == kRobotTaskCancel - || record.command == kRobotTaskClearTargetList; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - CanceledOldPathWithExactTerminalTasksIgnoresReplacementGlobalSnapshot) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5},{"task_id":"${TASK_ID_1}","status":1}]}})"); - controller_.setResponseDelay( - kRobotTaskClearTargetList, - std::chrono::milliseconds(100)); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - AgvResult old_result; - controller_.clearRecords(); - - std::thread old_navigation_thread([this, &options, &old_result]() { - old_result = agv_->followPath( - { - AgvPathSegment{"station-1", "station-2"}, - AgvPathSegment{"station-2", "station-3"}, - }, - options); - }); - - const bool cancel_observed = waitForCommandCount( - kRobotTaskClearTargetList, - 1, - 1000); - const auto records_at_cancel = controller_.records(); - const auto all2_count_at_cancel = static_cast(std::count_if( - records_at_cancel.begin(), - records_at_cancel.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusAll2; - })); - const auto exact_count_at_cancel = static_cast(std::count_if( - records_at_cancel.begin(), - records_at_cancel.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - })); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":6},{"task_id":"${TASK_ID_1}","status":6}]}})"); - controller_.setResponseDelay( - kRobotStatusTaskPackage, - std::chrono::milliseconds(300)); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":41101,"err_msg":"replacement task owns global 1101"})"); - const bool post_cancel_exact_query_observed = cancel_observed - && waitForCommandCount( - kRobotStatusTaskPackage, - exact_count_at_cancel + 1U, - 1000); - - const auto replacement_result = agv_->navigateToStation( - "replacement-station", - asynchronousMotionOptions()); - old_navigation_thread.join(); - - EXPECT_TRUE(cancel_observed); - EXPECT_TRUE(post_cancel_exact_query_observed); - ASSERT_TRUE(replacement_result.ok()) << replacement_result.message; - EXPECT_EQ(old_result.code, AgvErrorCode::TaskFailed); - EXPECT_NE( - old_result.message.find("old exact task ids are terminal"), - std::string::npos); - EXPECT_NE( - old_result.message.find("global stopped state was not inspected"), - std::string::npos); - EXPECT_EQ( - old_result.message.find("stopped state was confirmed"), - std::string::npos); - EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedNavigation( - *agv_, - AgvTaskType::NavigateToStation)); - const auto records = controller_.records(); - EXPECT_EQ( - static_cast(std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusAll2; - })), - all2_count_at_cancel); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskClearTargetList; - }), - 1); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - CurrentSegmentFailureClearsPathQueueDuringGlobalIdleTransitionWindow) -{ - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.queueResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5}]}})"); - controller_.setResponseDelay( - kRobotConfigLock, - std::chrono::milliseconds(100)); - controller_.setResponseDelay( - kRobotTaskClearTargetList, - std::chrono::milliseconds(100)); - AgvMotionOptions options; - options.wait_timeout_ms = 3000; - options.poll_interval_ms = 20; - AgvResult result; - controller_.clearRecords(); - - std::thread navigation_thread([this, &options, &result]() { - result = agv_->followPath( - { - AgvPathSegment{"station-1", "station-2"}, - AgvPathSegment{"station-2", "station-3"}, - }, - options); - }); - - const bool cancel_authority_observed = waitForCommandCount( - kRobotConfigLock, - 2, - 1500); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - const bool clear_path_observed = waitForCommandCount( - kRobotTaskClearTargetList, - 1, - 1000); - navigation_thread.join(); - - EXPECT_TRUE(cancel_authority_observed); - EXPECT_TRUE(clear_path_observed); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("conditionally canceled"), std::string::npos); - EXPECT_NE(result.message.find("stopped state was confirmed"), std::string::npos); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskClearTargetList; - }), - 1); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F(SeerRobokitControlAuthorityTest, AcquisitionFailureDoesNotSendControlCommand) -{ - controller_.setResponseCode(kRobotConfigLock, 40020); - controller_.clearRecords(); - - const auto result = agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.0}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos); - EXPECT_NE(result.message.find("ret_code=40020"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); -} - -TEST_F(SeerRobokitControlAuthorityTest, EmergencyStopAcquisitionFailureDoesNotSendStopCommands) -{ - controller_.setResponseCode(kRobotConfigLock, 40020); - controller_.clearRecords(); - - const auto result = agv_->emergencyStop(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos); - EXPECT_NE(result.message.find("ret_code=40020"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); -} - -TEST_F(SeerRobokitControlAuthorityTest, EmergencyStopAttemptsBothStopsAndAggregatesFailures) -{ - controller_.setResponseCode(kRobotControlStop, 50001); - controller_.setResponseCode(kRobotTaskCancel, 50002); - controller_.clearRecords(); - - const auto result = agv_->emergencyStop(); - - EXPECT_FALSE(result.ok()); - EXPECT_NE(result.message.find("control stop"), std::string::npos); - EXPECT_NE(result.message.find("cancel navigation"), std::string::npos); - EXPECT_NE(result.message.find("ret_code=50001"), std::string::npos); - EXPECT_NE(result.message.find("ret_code=50002"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 3U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotControlStop); - EXPECT_EQ(records[2].command, kRobotTaskCancel); -} - -TEST_F(SeerRobokitControlAuthorityTest, UnsupportedClearFaultDoesNotAcquireAuthority) -{ - controller_.clearRecords(); - - const auto result = agv_->clearFault(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::UnsupportedCommand); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F(SeerRobokitControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority) -{ - controller_.clearRecords(); - std::string content; - - const auto result = agv_->downloadMap("map-1", content); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigDownloadMap); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseUsesLegacyCompatibleFreeGoPayloadWithTypedMotionLimits) -{ - AgvMotionOptions options; - options.asynchronous = true; - options.max_speed = 0.6; - options.max_angular_speed = 0.7; - options.max_acceleration = 0.8; - options.max_angular_acceleration = 0.9; - options.reach_distance = 0.1; - options.reach_angle = 0.2; - controller_.clearRecords(); - - const auto result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_GE(records.size(), 4U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - for (std::size_t index = 2; index < records.size(); ++index) { - EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); - } - - const auto payload = parsePayload(records[1]); - EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); - EXPECT_EQ(payloadValue(payload, "id").asString(), ""); - EXPECT_FALSE(payloadValue(payload, "task_id").asString().empty()); - const auto& free_go = payloadValue(payload, "freeGo"); - EXPECT_TRUE(payloadValue(free_go, "x").isNumeric()); - EXPECT_TRUE(payloadValue(free_go, "y").isNumeric()); - EXPECT_TRUE(payloadValue(free_go, "theta").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "x").asDouble(), 1.0); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "y").asDouble(), 2.0); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "theta").asDouble(), 0.5); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_speed").asDouble(), 0.6); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wspeed").asDouble(), 0.7); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_acc").asDouble(), 0.8); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.9); - EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_dist").asDouble(), 0.1); - EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_angle").asDouble(), 0.2); - EXPECT_EQ( - payloadValue(payload, "skill_name").asString(), - "GotoSpecifiedPose"); - EXPECT_FALSE(payloadHas(payload, "x")); - EXPECT_FALSE(payloadHas(payload, "y")); - EXPECT_FALSE(payloadHas(payload, "angle")); - EXPECT_FALSE(payloadHas(payload, "jack_height")); - - const auto status_payload = parsePayload(records[2]); - const auto& requested_task_ids = payloadValue(status_payload, "task_ids"); - ASSERT_TRUE(requested_task_ids.isArray()); - ASSERT_EQ(requested_task_ids.size(), 1U); - ASSERT_TRUE(requested_task_ids[0].isString()); - EXPECT_EQ( - requested_task_ids[0].asString(), - payloadValue(payload, "task_id").asString()); - const auto second_status_payload = parsePayload(records.back()); - const auto& second_requested_task_ids = - payloadValue(second_status_payload, "task_ids"); - ASSERT_TRUE(second_requested_task_ids.isArray()); - ASSERT_EQ(second_requested_task_ids.size(), 1U); - EXPECT_EQ( - second_requested_task_ids[0].asString(), - payloadValue(payload, "task_id").asString()); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseUsesExplicitTaskIdAsUniquePrefixAndWhitelistsAdapterFields) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - AgvAdapterParams adapter_params; - adapter_params.values.emplace("source_id", "SELF_POSITION"); - adapter_params.values.emplace("target_id", "SELF_POSITION"); - adapter_params.values.emplace("task_id", "pose-task"); - adapter_params.values.emplace("skill_name", "GotoSpecifiedPose"); - adapter_params.values.emplace("operation", "JackHeight"); - adapter_params.values.emplace("jack_height", "0.5"); - adapter_params.values.emplace("script_name", "unsafe-script"); - adapter_params.values.emplace("unknown_field", "unsafe-value"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose( - math::Pose2d{0.0, 0.0, 0.5}, - asynchronousMotionOptions(), - adapter_params); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_GE(records.size(), 4U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - for (std::size_t index = 2; index < records.size(); ++index) { - EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); - } - - const auto payload = parsePayload(records[1]); - const std::string first_task_id = - payloadValue(payload, "task_id").asString(); - EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); - EXPECT_EQ(payloadValue(payload, "id").asString(), ""); - EXPECT_EQ( - first_task_id.find("pose-task_pose_"), - 0U); - EXPECT_EQ(payloadValue(payload, "skill_name").asString(), "GotoSpecifiedPose"); - const auto& free_go = payloadValue(payload, "freeGo"); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "x").asDouble(), 0.0); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "y").asDouble(), 0.0); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "theta").asDouble(), 0.5); - EXPECT_FALSE(payloadHas(payload, "operation")); - EXPECT_FALSE(payloadHas(payload, "jack_height")); - EXPECT_FALSE(payloadHas(payload, "script_name")); - EXPECT_FALSE(payloadHas(payload, "unknown_field")); - - controller_.clearRecords(); - const auto second_result = agv_->navigateToPose( - math::Pose2d{0.0, 0.0, 0.5}, - asynchronousMotionOptions(), - adapter_params); - ASSERT_TRUE(second_result.ok()) << second_result.message; - const auto second_records = controller_.records(); - ASSERT_GE(second_records.size(), 2U); - const auto second_payload = parsePayload(second_records[1]); - const std::string second_task_id = - payloadValue(second_payload, "task_id").asString(); - EXPECT_EQ(second_task_id.find("pose-task_pose_"), 0U); - EXPECT_NE(first_task_id, second_task_id); -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseRejectsUnsafeFallbackStationAndSkill) -{ - AgvAdapterParams adapter_params; - adapter_params.values.emplace("source_id", "station-0"); - controller_.clearRecords(); - - auto result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - {}, - adapter_params); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("source_id must be SELF_POSITION"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - adapter_params.values.clear(); - adapter_params.values.emplace("target_id", "station-1"); - controller_.clearRecords(); - - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - {}, - adapter_params); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("target_id must be empty"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - adapter_params.values.clear(); - adapter_params.values.emplace("skill_name", "unsafe-custom-skill"); - controller_.clearRecords(); - - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - {}, - adapter_params); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("skill_name must be GotoSpecifiedPose"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - MotionCommandsRejectNonFiniteNumericInputsBeforeAcquiringAuthority) -{ - const double nan = std::numeric_limits::quiet_NaN(); - const double infinity = std::numeric_limits::infinity(); - - controller_.clearRecords(); - auto result = agv_->navigateToPose(math::Pose2d{nan, 0.0, 0.0}, asynchronousMotionOptions()); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("pose"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - AgvMotionOptions pose_options; - pose_options.reach_distance = infinity; - controller_.clearRecords(); - result = agv_->navigateToPose( - math::Pose2d{0.0, 0.0, 0.0}, - pose_options); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("reach_distance"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - AgvMotionOptions station_options; - station_options.max_acceleration = nan; - controller_.clearRecords(); - result = agv_->navigateToStation("station-1", station_options); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("max_acceleration"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - AgvMotionOptions negative_options; - negative_options.max_speed = -0.1; - controller_.clearRecords(); - result = agv_->navigateToPose( - math::Pose2d{0.0, 0.0, 0.0}, - negative_options); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("non-negative"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - AgvAdapterParams invalid_adapter; - invalid_adapter.values.emplace("jack_height", "inf"); - controller_.clearRecords(); - result = agv_->navigateToStation( - "station-1", - {}, - invalid_adapter); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("jack_height"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - controller_.clearRecords(); - result = agv_->setVelocity(AgvVelocity{0.0, infinity, 0.0}); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("velocity"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseIgnoresUncorrelatedStatusUntilPoseTaskAppears) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"old-pose-task","status":2,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_GE(records.size(), 5U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - for (std::size_t index = 2; index < records.size(); ++index) { - EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); - } -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseWaitsForStableRunningAfterWaiting) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_GE(records.size(), 5U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - for (std::size_t index = 2; index < records.size(); ++index) { - EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); - } -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseDoesNotReturnSuccessBeforeLateRunningFaultPush) -{ - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_RUNNING_31: safety controller rejected free navigation"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("E_RUNNING_31"), std::string::npos); - EXPECT_NE(result.message.find("Running state"), std::string::npos); - EXPECT_NE( - result.message.find("do not retry automatically"), - std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseFailsIfFaultPushChannelInvalidatesDuringStartConfirmation) -{ - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - SeerRobokitAgvTestPeer::setFaultStateUnknown(*agv_); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE( - result.message.find("fault monitoring became unavailable"), - std::string::npos); - EXPECT_NE( - result.message.find("push channel changed or was invalidated"), - std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseRejectsCompletedTargetWhenLateControllerFaultArrives) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_COMPLETED_45: controller rejected completed pose"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("reported Completed"), std::string::npos); - EXPECT_NE(result.message.find("E_COMPLETED_45"), std::string::npos); - const auto records = controller_.records(); - EXPECT_FALSE(std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusLoc; - })); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseRejectsFaultArrivingDuringCompletedPoseVerification) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - controller_.setResponseDelay( - kRobotStatusLoc, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool location_query_observed = false; - for (int attempt = 0; attempt < 700; ++attempt) { - const auto records = controller_.records(); - location_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusLoc; - }); - if (location_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(location_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_POSE_VERIFY_46: fault during target verification"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("target verification"), std::string::npos); - EXPECT_NE(result.message.find("E_POSE_VERIFY_46"), std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseDoesNotMisattributeFaultFromCommandAwaitingAck) -{ - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - AgvResult station_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - controller_.setResponseDelay( - kRobotTaskGoTarget, - std::chrono::milliseconds(300)); - std::thread station_thread([this, &station_result]() { - station_result = agv_->navigateToStation("station-1", asynchronousMotionOptions()); - }); - - bool station_command_observed = false; - for (int attempt = 0; attempt < 500; ++attempt) { - const auto records = controller_.records(); - const auto go_target_count = std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTarget; - }); - station_command_observed = go_target_count >= 2; - if (station_command_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(station_command_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_STATION_ACK_18: fault from concurrent station command"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - - pose_thread.join(); - station_thread.join(); - - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::Fault) - << pose_result.message; - EXPECT_NE( - pose_result.message.find("another control command attempt"), - std::string::npos); - EXPECT_NE( - pose_result.message.find("cannot be attributed"), - std::string::npos); - EXPECT_NE( - pose_result.message.find("E_STATION_ACK_18"), - std::string::npos); - ASSERT_TRUE(station_result.ok()) << station_result.message; -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseDoesNotMisattributeFaultObservedAfterAnotherCommandAck) -{ - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - controller_.setResponseCode(kRobotTaskGoTarget, 4188); - const auto station_result = agv_->navigateToStation("station-1", asynchronousMotionOptions()); - EXPECT_FALSE(station_result.ok()); - EXPECT_NE(station_result.message.find("ret_code=4188"), std::string::npos); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_AFTER_ACK_19: delayed station command alarm"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::Fault); - EXPECT_NE( - pose_result.message.find("another control command attempt"), - std::string::npos); - EXPECT_NE( - pose_result.message.find("cannot be attributed"), - std::string::npos); - EXPECT_NE(pose_result.message.find("E_AFTER_ACK_19"), std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - PauseDuringPoseCommandAckPreservesPublishedTaskContext) -{ - controller_.setResponseDelay( - kRobotTaskGoTarget, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - AgvResult pause_result = AgvResult::failure( - AgvErrorCode::CommandFailed, - "pause not called"); - - std::thread pose_thread([this, &pose_result]() { - pose_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool pose_command_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - pose_command_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTarget; - }); - if (pose_command_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(pose_command_observed); - - std::thread pause_thread([this, &pause_result]() { - pause_result = agv_->pauseNavigation(); - }); - pause_thread.join(); - pose_thread.join(); - - ASSERT_TRUE(pause_result.ok()) << pause_result.message; - EXPECT_FALSE(pose_result.ok()); - EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); - EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseReturnsAcceptedWhenMatchingTaskRemainsQueued) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"queued","task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - const auto status_query_count = std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - EXPECT_GT(status_query_count, 2); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseReturnsControllerFaultWhenQueuedTaskRaisesAlarm) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"queued","task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_WAIT_19: safety interlock"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("remains active"), std::string::npos); - EXPECT_NE(result.message.find("E_WAIT_19"), std::string::npos); - EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseDoesNotReportAcceptedAfterMatchingTaskDisappears) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE( - result.message.find("not present in task_status_package"), - std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseDoesNotHangWhenRunningTaskDisappears) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - - const auto started_at = std::chrono::steady_clock::now(); - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - const auto elapsed = std::chrono::duration_cast( - std::chrono::steady_clock::now() - started_at); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE( - result.message.find("not present in task_status_package"), - std::string::npos); - EXPECT_LT(elapsed, std::chrono::seconds(3)); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseReturnsFailureAfterWaitingTransitionsToFailed) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"create_on":"2026-07-31T10:00:00Z","err_msg":"controller task failed","task_status_package":{"closest_target":"goal-7","source_name":"SELF_POSITION","target_name":"free-goal","percentage":0.0,"distance":1.4,"info":"planner rejected pose","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("planner rejected pose"), std::string::npos); - EXPECT_NE(result.message.find("task_status=5"), std::string::npos); - EXPECT_NE(result.message.find("status_query_ret_code=0"), std::string::npos); - EXPECT_NE(result.message.find("controller task failed"), std::string::npos); - EXPECT_NE(result.message.find("closest_target=goal-7"), std::string::npos); - EXPECT_NE(result.message.find("distance=1.400000"), std::string::npos); - const auto records = controller_.records(); - EXPECT_GE( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }), - 2); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseRejectsCompletedTaskWhenRequestedTargetWasNotReached) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":0.0,"y":0.0,"angle":0.0})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 0.0, 0.0}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE(result.message.find("target was not reached"), std::string::npos); - EXPECT_NE(result.message.find("distance_error=1"), std::string::npos); - const auto records = controller_.records(); - EXPECT_GE( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }), - 2); - EXPECT_GE( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusLoc; - }), - 1); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseAcceptsCompletedTaskOnlyWhenRequestedTargetWasReached) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 4U); - EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); - EXPECT_EQ(records[3].command, kRobotStatusLoc); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseDoesNotUseFaultOnlyPushAsAValidCompletedPose) -{ - Json::Value push_payload(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("controller fault without pose fields"); - *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, push_payload); - Json::Value cleared_push(Json::objectValue); - *cleared_push.demand("errors", "errors" + std::strlen("errors")) = - Json::Value(Json::arrayValue); - *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, cleared_push); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"err_msg":""})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{0.0, 0.0, 0.0}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE( - result.message.find("target pose could not be verified"), - std::string::npos); - EXPECT_NE( - result.message.find("did not contain numeric x/y/angle"), - std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseRejectsCompletedTaskWithNonNumericControllerPose) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":null,"y":false,"angle":"0.0"})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{0.0, 0.0, 0.0}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE( - result.message.find("did not contain numeric x/y/angle"), - std::string::npos); -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseReturnsAsynchronousControllerFailure) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"planner rejected pose","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("planner rejected pose"), std::string::npos); - EXPECT_NE(result.message.find("task_status=5"), std::string::npos); - EXPECT_NE(result.message.find("task_type=1"), std::string::npos); -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigateToPosePreservesSynchronousControllerCode) -{ - controller_.setResponseCode(kRobotTaskGoTarget, 43051); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("ret_code=43051"), std::string::npos); - EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigateToPosePreservesStatusQueryControllerCode) -{ - controller_.setResponseCode(kRobotStatusTaskPackage, 41110); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("ret_code=41110"), std::string::npos); - EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseRejectsWrongResponseCommand) -{ - controller_.setResponseCommand(kRobotTaskGoTarget, 13052); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("expected=13051"), std::string::npos); - EXPECT_NE(result.message.find("actual=13052"), std::string::npos); - EXPECT_NE(result.message.find("channel closed"), std::string::npos); - EXPECT_NE(result.message.find("controller outcome is unknown"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseRejectsMissingControllerCode) -{ - controller_.setResponsePayload( - kRobotTaskGoTarget, - R"({"err_msg":"missing acknowledgment code"})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("missing a numeric ret_code"), std::string::npos); - EXPECT_NE(result.message.find("controller outcome is unknown"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseReportsPausedTaskExplicitly) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"safety pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE(result.message.find("established but is paused"), std::string::npos); - EXPECT_NE(result.message.find("safety pause"), std::string::npos); - EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPosePreservesControllerFaultWhenTaskImmediatelyPauses) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"safety pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_PAUSED_55: safety controller paused failed task"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("paused"), std::string::npos); - EXPECT_NE(result.message.find("E_PAUSED_55"), std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseIncludesTaskCorrelatedRawControllerFaultDetail) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_NAV_42: planner alarm"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - - Json::Value cleared_push(Json::objectValue); - *cleared_push.demand("errors", "errors" + std::strlen("errors")) = - Json::Value(Json::arrayValue); - *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, cleared_push); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"navigation failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("E_NAV_42"), std::string::npos); - EXPECT_NE(result.message.find("planner alarm"), std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseRejectsPreexistingControllerFaultWithoutSendingTask) -{ - Json::Value push_payload(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("OLD_FAULT_FROM_PREVIOUS_TASK"); - *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, push_payload); - controller_.clearRecords(); - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE( - result.message.find("OLD_FAULT_FROM_PREVIOUS_TASK"), - std::string::npos); - EXPECT_NE(result.message.find("was not sent"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseRejectsUnknownOrStaleFaultStateWithoutSendingTask) -{ - SeerRobokitAgvTestPeer::setFaultStateUnknown(*agv_); - controller_.clearRecords(); - - auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE( - result.message.find("no state push containing fatals/errors"), - std::string::npos); - auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - - SeerRobokitAgvTestPeer::setFaultStateAge( - *agv_, - std::chrono::seconds(3)); - controller_.clearRecords(); - - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("state push is stale"), std::string::npos); - records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseRejectsMalformedFaultStateWithoutSendingTask) -{ - Json::Value malformed_push(Json::objectValue); - *malformed_push.demand("errors", "errors" + std::strlen("errors")) = - Json::Value(); - *malformed_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, malformed_push); - controller_.clearRecords(); - - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE( - result.message.find("incomplete or malformed"), - std::string::npos); - EXPECT_NE(result.message.find("errors=null"), std::string::npos) - << result.message; - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToPoseDoesNotAttributeClearedFaultHistoryToNewTask) -{ - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("OLD_CLEARED_FAULT"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - - Json::Value cleared_push(Json::objectValue); - *cleared_push.demand("errors", "errors" + std::strlen("errors")) = - Json::Value(Json::arrayValue); - *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, cleared_push); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"new task failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("new task failed"), std::string::npos); - EXPECT_EQ(result.message.find("OLD_CLEARED_FAULT"), std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - CancelDoesNotSupersedeAlreadyCompletedPoseDuringLocationVerification) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - controller_.setResponseDelay( - kRobotStatusLoc, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool location_query_observed = false; - for (int attempt = 0; attempt < 500; ++attempt) { - const auto records = controller_.records(); - location_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusLoc; - }); - if (location_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(location_query_observed); - - const auto cancel_result = agv_->cancelNavigation(); - pose_thread.join(); - - ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; - ASSERT_TRUE(pose_result.ok()) << pose_result.message; - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F(SeerRobokitControlAuthorityTest, CancelSupersedesPoseStartConfirmation) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - - const auto cancel_result = agv_->cancelNavigation(); - pose_thread.join(); - - EXPECT_TRUE(status_query_observed); - ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); - EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); -} - -TEST_F(SeerRobokitControlAuthorityTest, FailedCancelDoesNotSupersedePoseStartConfirmation) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - std::atomic_bool pose_finished{false}; - - std::thread pose_thread([this, &pose_result, &pose_finished]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - pose_finished.store(true, std::memory_order_release); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - controller_.setResponseCode(kRobotConfigLock, 17); - const auto authority_failure = agv_->cancelNavigation(); - EXPECT_FALSE(authority_failure.ok()); - std::this_thread::sleep_for(std::chrono::milliseconds(75)); - EXPECT_FALSE(pose_finished.load(std::memory_order_acquire)); - - controller_.setResponseCode(kRobotConfigLock, 0); - controller_.setResponseCode(kRobotTaskCancel, 23); - const auto command_failure = agv_->cancelNavigation(); - EXPECT_FALSE(command_failure.ok()); - std::this_thread::sleep_for(std::chrono::milliseconds(75)); - EXPECT_FALSE(pose_finished.load(std::memory_order_acquire)); - - controller_.setResponseCode(kRobotTaskCancel, 0); - const auto successful_cancel = agv_->cancelNavigation(); - pose_thread.join(); - - ASSERT_TRUE(successful_cancel.ok()) << successful_cancel.message; - EXPECT_TRUE(pose_finished.load(std::memory_order_acquire)); - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); - EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); -} - -TEST_F(SeerRobokitControlAuthorityTest, IndeterminateCancelSupersedesPoseStartConfirmation) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - controller_.setResponsePayload( - kRobotTaskCancel, - R"({"err_msg":"acknowledgment lost"})"); - const auto cancel_result = agv_->cancelNavigation(); - pose_thread.join(); - - EXPECT_FALSE(cancel_result.ok()); - EXPECT_NE(cancel_result.message.find("missing a numeric ret_code"), std::string::npos); - EXPECT_NE(cancel_result.message.find("controller outcome is unknown"), std::string::npos); - EXPECT_NE( - cancel_result.message.find("do not issue another motion command automatically"), - std::string::npos); - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); -} - -TEST_F(SeerRobokitControlAuthorityTest, TimedOutChannelIsClosedBeforeSameCommandCanRetry) -{ - SeerRobokitAgvTestPeer::setNavigationReceiveTimeout( - *agv_, - std::chrono::milliseconds(50)); - controller_.setResponseDelay( - kRobotTaskCancel, - std::chrono::milliseconds(200)); - controller_.clearRecords(); - - const auto first_result = agv_->cancelNavigation(); - const auto second_result = agv_->cancelNavigation(); - - EXPECT_FALSE(first_result.ok()); - EXPECT_EQ(first_result.code, AgvErrorCode::Timeout); - EXPECT_NE(first_result.message.find("channel closed"), std::string::npos); - EXPECT_NE( - first_result.message.find("controller outcome is unknown"), - std::string::npos); - EXPECT_FALSE(second_result.ok()); - EXPECT_EQ(second_result.code, AgvErrorCode::NotConnected); - EXPECT_EQ( - second_result.message.find("controller outcome is unknown"), - std::string::npos); - - const auto records = controller_.records(); - const auto cancel_count = std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }); - EXPECT_EQ(cancel_count, 1); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigationWriteNotStartedDoesNotPublishFalseTracking) -{ - SeerRobokitAgvTestPeer::closeNavigationSocket(*agv_); - AgvMotionOptions options; - options.wait_timeout_ms = 500; - options.poll_interval_ms = 20; - controller_.clearRecords(); - - const auto result = agv_->navigateToStation( - "station-not-sent", - options); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::NotConnected); - EXPECT_EQ( - result.message.find("controller outcome is unknown"), - std::string::npos); - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedNavigation( - *agv_, - AgvTaskType::NavigateToStation)); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTarget - || record.command == kRobotStatusTaskPackage - || record.command == kRobotTaskCancel; - }), - 0); -} - -TEST_F(SeerRobokitControlAuthorityTest, SlowTaskStatusDoesNotBlockEmergencyStop) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.setResponseDelay( - kRobotStatusTaskPackage, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(status_query_observed); - - const auto start = std::chrono::steady_clock::now(); - const auto stop_result = agv_->emergencyStop(); - const auto elapsed = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start); - pose_thread.join(); - - ASSERT_TRUE(stop_result.ok()) << stop_result.message; - EXPECT_LT(elapsed.count(), 150); - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); - - const auto records = controller_.records(); - const auto control_stop = std::find_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotControlStop; - }); - const auto navigation_cancel = std::find_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }); - ASSERT_NE(control_stop, records.end()); - ASSERT_NE(navigation_cancel, records.end()); - EXPECT_LT(control_stop, navigation_cancel); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - SlowConditionalCancelPreflightDoesNotBlockOrFollowEmergencyStop) -{ - const auto path_result = agv_->followPath( - {AgvPathSegment{"station-1", "station-2"}}, - asynchronousMotionOptions()); - ASSERT_TRUE(path_result.ok()) << path_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":3}]}})"); - controller_.setResponsePayload( - kRobotStatusAll2, - R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-2","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); - controller_.setResponseDelay( - kRobotStatusTaskPackage, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvResult cancel_result; - - std::thread cancel_thread([this, &cancel_result]() { - cancel_result = agv_->cancelNavigation(); - }); - const bool cancel_preflight_started = waitForCommandCount( - kRobotStatusTaskPackage, - 1, - 1000); - const auto stop_started_at = std::chrono::steady_clock::now(); - const auto stop_result = agv_->emergencyStop(); - const auto stop_elapsed = std::chrono::duration_cast< - std::chrono::milliseconds>( - std::chrono::steady_clock::now() - stop_started_at); - cancel_thread.join(); - - EXPECT_TRUE(cancel_preflight_started); - ASSERT_TRUE(stop_result.ok()) << stop_result.message; - EXPECT_LT(stop_elapsed.count(), 200); - EXPECT_FALSE(cancel_result.ok()); - EXPECT_EQ(cancel_result.code, AgvErrorCode::TaskCanceled); - const auto records = controller_.records(); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotControlStop; - }), - 1); - EXPECT_EQ( - std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskClearTargetList; - }), - 1); -} - -TEST_F(SeerRobokitControlAuthorityTest, EmergencyStopInvalidatesPoseAfterFirstAcceptedStop) -{ - controller_.setResponseDelay( - kRobotStatusTaskPackage, - std::chrono::milliseconds(200)); - controller_.setResponseDelay( - kRobotTaskCancel, - std::chrono::milliseconds(400)); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - AgvResult stop_result = AgvResult::success(); - std::atomic_bool pose_finished{false}; - std::atomic_bool stop_finished{false}; - - std::thread pose_thread([this, &pose_result, &pose_finished]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - pose_finished.store(true, std::memory_order_release); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - std::thread stop_thread([this, &stop_result, &stop_finished]() { - stop_result = agv_->emergencyStop(); - stop_finished.store(true, std::memory_order_release); - }); - - bool control_stop_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - control_stop_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotControlStop; - }); - if (control_stop_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(control_stop_observed); - - for (int attempt = 0; attempt < 350; ++attempt) { - if (pose_finished.load(std::memory_order_acquire)) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - const bool pose_finished_before_navigation_cancel = - pose_finished.load(std::memory_order_acquire); - const bool stop_finished_before_pose_result = - stop_finished.load(std::memory_order_acquire); - stop_thread.join(); - pose_thread.join(); - EXPECT_TRUE(pose_finished_before_navigation_cancel); - EXPECT_FALSE(stop_finished_before_pose_result); - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); - ASSERT_TRUE(stop_result.ok()) << stop_result.message; -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigateToStationForwardsTypedMotionOptions) -{ - AgvMotionOptions options; - options.asynchronous = true; - options.max_speed = 0.4; - options.max_angular_speed = 0.5; - options.max_acceleration = 0.6; - options.max_angular_acceleration = 0.7; - AgvAdapterParams adapter_params; - adapter_params.values.emplace("id", "wrong-station"); - adapter_params.values.emplace("x", "99.0"); - adapter_params.values.emplace("freeGo", "invalid"); - adapter_params.values.emplace("max_speed", "not-a-number"); - adapter_params.values.emplace("reach_dist", "not-a-number"); - controller_.clearRecords(); - - const auto result = agv_->navigateToStation( - "station-1", - options, - adapter_params); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - - const auto payload = parsePayload(records[1]); - EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); - EXPECT_EQ(payloadValue(payload, "id").asString(), "station-1"); - EXPECT_TRUE(payloadValue(payload, "max_speed").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "max_wspeed").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "max_acc").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "max_wacc").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_speed").asDouble(), 0.4); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wspeed").asDouble(), 0.5); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_acc").asDouble(), 0.6); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.7); - EXPECT_FALSE(payloadHas(payload, "x")); - EXPECT_FALSE(payloadHas(payload, "freeGo")); - EXPECT_FALSE(payloadHas(payload, "reach_dist")); - EXPECT_FALSE(payloadHas(payload, "jack_height")); - EXPECT_FALSE(payloadHas(payload, "use_pgv")); - EXPECT_FALSE(payloadHas(payload, "use_down_pgv")); - EXPECT_FALSE(payloadHas(payload, "pgv_adjust_dist")); - EXPECT_FALSE(payloadHas(payload, "pgv_adjust_cx")); - EXPECT_FALSE(payloadHas(payload, "pgv_adjust_cy")); - EXPECT_FALSE(payloadHas(payload, "pgv_x_adjust")); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToStationForwardsPgvAdjustmentUsingNativeJsonTypes) -{ - AgvMotionOptions options; - options.asynchronous = true; - AgvAdapterParams adapter_params; - adapter_params.values.emplace("source_id", "LM2"); - adapter_params.values.emplace("use_pgv", "true"); - adapter_params.values.emplace("pgv_adjust_dist", "0.3"); - adapter_params.values.emplace("pgv_adjust_cx", "-0.3"); - adapter_params.values.emplace("pgv_adjust_cy", "0"); - adapter_params.values.emplace("pgv_x_adjust", "1"); - adapter_params.values.emplace("use_down_pgv", "false"); - controller_.clearRecords(); - - const auto result = agv_->navigateToStation( - "AP1", - options, - adapter_params); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - - const auto payload = parsePayload(records[1]); - EXPECT_EQ(payloadValue(payload, "source_id").asString(), "LM2"); - EXPECT_EQ(payloadValue(payload, "id").asString(), "AP1"); - EXPECT_TRUE(payloadValue(payload, "use_pgv").isBool()); - EXPECT_TRUE(payloadValue(payload, "use_pgv").asBool()); - EXPECT_TRUE(payloadValue(payload, "use_down_pgv").isBool()); - EXPECT_FALSE(payloadValue(payload, "use_down_pgv").asBool()); - EXPECT_TRUE(payloadValue(payload, "pgv_adjust_dist").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "pgv_adjust_cx").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "pgv_adjust_cy").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "pgv_x_adjust").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(payload, "pgv_adjust_dist").asDouble(), 0.3); - EXPECT_DOUBLE_EQ(payloadValue(payload, "pgv_adjust_cx").asDouble(), -0.3); - EXPECT_DOUBLE_EQ(payloadValue(payload, "pgv_adjust_cy").asDouble(), 0.0); - EXPECT_DOUBLE_EQ(payloadValue(payload, "pgv_x_adjust").asDouble(), 1.0); - EXPECT_FALSE(payloadHas(payload, "pgv_adjustuse_pgv_dist")); - EXPECT_FALSE(payloadHas(payload, "pgv_ajdust_cy")); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToStationNormalizesVendorDocumentPgvCyAlias) -{ - AgvMotionOptions options; - options.asynchronous = true; - AgvAdapterParams adapter_params; - adapter_params.values.emplace("use_pgv", "true"); - adapter_params.values.emplace("pgv_ajdust_cy", "-0.2"); - controller_.clearRecords(); - - const auto result = agv_->navigateToStation( - "AP1", - options, - adapter_params); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - const auto payload = parsePayload(records[1]); - EXPECT_TRUE(payloadValue(payload, "pgv_adjust_cy").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(payload, "pgv_adjust_cy").asDouble(), -0.2); - EXPECT_FALSE(payloadHas(payload, "pgv_ajdust_cy")); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigateToStationRejectsInvalidPgvAdjustmentBeforeAcquiringAuthority) -{ - struct InvalidPgvCase { - const char* key; - const char* value; - const char* expected_detail; - }; - const InvalidPgvCase cases[] = { - {"use_pgv", "enabled", "must be a boolean string"}, - {"pgv_adjust_dist", "-0.1", "must be non-negative"}, - {"pgv_adjust_cx", "nan", "must be a complete finite number"}, - {"pgv_x_adjust", "0.5m", "must be a complete finite number"}, - {"pgv_ajdust_cy", "inf", "must be a complete finite number"}, - {"pgv_adjustuse_pgv_dist", "0.3", "vendor-document typo"}, - }; - - AgvMotionOptions options; - options.asynchronous = true; - for (const auto& test_case : cases) { - SCOPED_TRACE(test_case.key); - AgvAdapterParams adapter_params; - adapter_params.values.emplace(test_case.key, test_case.value); - controller_.clearRecords(); - - const auto result = agv_->navigateToStation( - "AP1", - options, - adapter_params); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE( - result.message.find(test_case.expected_detail), - std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - } - - AgvAdapterParams ambiguous_adapter_params; - ambiguous_adapter_params.values.emplace("pgv_adjust_cy", "0.1"); - ambiguous_adapter_params.values.emplace("pgv_ajdust_cy", "0.2"); - controller_.clearRecords(); - - const auto ambiguous_result = agv_->navigateToStation( - "AP1", - options, - ambiguous_adapter_params); - - EXPECT_FALSE(ambiguous_result.ok()); - EXPECT_EQ(ambiguous_result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE( - ambiguous_result.message.find("must not both be set"), - std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - AgvAdapterParams mixed_action_adapter_params; - mixed_action_adapter_params.values.emplace("use_pgv", "true"); - mixed_action_adapter_params.values.emplace("operation", "JackHeight"); - mixed_action_adapter_params.values.emplace("jack_height", "0.5"); - controller_.clearRecords(); - - const auto mixed_action_result = agv_->navigateToStation( - "AP1", - options, - mixed_action_adapter_params); - - EXPECT_FALSE(mixed_action_result.ok()); - EXPECT_EQ(mixed_action_result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE( - mixed_action_result.message.find( - "PGV adjustment must not be combined with adapter field"), - std::string::npos); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F(SeerRobokitControlAuthorityTest, SetVelocityUsesOnlyDocumentedNumericFields) -{ - controller_.clearRecords(); - - const auto result = agv_->setVelocity(AgvVelocity{0.1, -0.2, 0.3}); - - ASSERT_TRUE(result.ok()) << result.message; - auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotControlMotion); - - auto payload = parsePayload(records[1]); - EXPECT_TRUE(payloadValue(payload, "vx").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "vy").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "w").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(payload, "vx").asDouble(), 0.1); - EXPECT_DOUBLE_EQ(payloadValue(payload, "vy").asDouble(), -0.2); - EXPECT_DOUBLE_EQ(payloadValue(payload, "w").asDouble(), 0.3); - EXPECT_FALSE(payloadHas(payload, "duration")); - - controller_.clearRecords(); - const auto stop_result = agv_->stopVelocityControl(); - - ASSERT_TRUE(stop_result.ok()) << stop_result.message; - records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[1].command, kRobotControlMotion); - payload = parsePayload(records[1]); - EXPECT_TRUE(payloadValue(payload, "vx").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "vy").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "w").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(payload, "vx").asDouble(), 0.0); - EXPECT_DOUBLE_EQ(payloadValue(payload, "vy").asDouble(), 0.0); - EXPECT_DOUBLE_EQ(payloadValue(payload, "w").asDouble(), 0.0); - EXPECT_FALSE(payloadHas(payload, "duration")); -} - -TEST_F(SeerRobokitControlAuthorityTest, ControllerErrorCodeIsPreservedInResultMessage) -{ - controller_.setResponseCode(kRobotControlMotion, 41200); - controller_.clearRecords(); - - const auto result = agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.0}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("ret_code=41200"), std::string::npos); - EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - MapModeCommandsSupersedeTrackedFreeNavigation) -{ - auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(result.ok()) << result.message; - ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->switchMap("map-1"); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(result.ok()) << result.message; - ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->startMapping(); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(result.ok()) << result.message; - ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->stopMapping(); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - PauseResumeAndStopVelocityPreserveTrackedFreeNavigation) -{ - auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(result.ok()) << result.message; - ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->pauseNavigation(); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->resumeNavigation(); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->stopVelocityControl(); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - - controller_.clearRecords(); - const auto status = agv_->navigationStatus(); - EXPECT_EQ(status.state, AgvTaskState::Running); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigationStatusQueriesTrackedPoseTaskPackage) -{ - AgvAdapterParams adapter_params; - adapter_params.values.emplace("task_id", "pose-task-current"); - const auto navigate_result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - asynchronousMotionOptions(), - adapter_params); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - const auto navigate_records = controller_.records(); - ASSERT_GE(navigate_records.size(), 2U); - const auto navigate_payload = parsePayload(navigate_records[1]); - const std::string generated_task_id = - payloadValue(navigate_payload, "task_id").asString(); - ASSERT_FALSE(generated_task_id.empty()); - - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"create_on":"2026-07-31T10:00:01Z","err_msg":"","task_status_package":{"percentage":42.5,"distance":0.7,"info":"operator pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); - controller_.clearRecords(); - - const auto status = agv_->navigationStatus(); - - EXPECT_EQ(status.state, AgvTaskState::Paused); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_DOUBLE_EQ(status.progress, 42.5); - EXPECT_NE(status.message.find("task_id=" + generated_task_id), std::string::npos); - EXPECT_NE(status.message.find("operator pause"), std::string::npos); - EXPECT_NE(status.message.find("create_on=2026-07-31T10:00:01Z"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); - const auto payload = parsePayload(records[0]); - const auto& task_ids = payloadValue(payload, "task_ids"); - ASSERT_TRUE(task_ids.isArray()); - ASSERT_EQ(task_ids.size(), 1U); - EXPECT_EQ(task_ids[0].asString(), generated_task_id); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigationStatusMapsControllerTaskStatusSevenToFailed) -{ - const auto navigate_result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"controller terminated tracked pose","task_status_list":[{"task_id":"${TASK_ID}","status":7,"type":1}]}})"); - controller_.clearRecords(); - - const auto status = agv_->navigationStatus(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE(status.message.find("controller terminated tracked pose"), std::string::npos); - EXPECT_NE(status.message.find("task_status=7"), std::string::npos); - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigationStatusReturnsControllerFaultWhileTaskStillReportsRunning) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_RUNNING_STATUS_52: collision input active"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - controller_.clearRecords(); - - const auto status = agv_->navigationStatus(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE(status.message.find("controller_task_state=2"), std::string::npos); - EXPECT_NE(status.message.find("E_RUNNING_STATUS_52"), std::string::npos); - EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigationStatusPreservesFaultWhenTrackedTaskDisappears) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"task vanished","task_status_list":[]}})"); - controller_.clearRecords(); - AgvNavigationStatus status; - - std::thread status_thread([this, &status]() { - status = agv_->navigationStatus(); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_TASK_GONE_54: controller removed failed task"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - status_thread.join(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE(status.message.find("task disappeared"), std::string::npos); - EXPECT_NE(status.message.find("E_TASK_GONE_54"), std::string::npos); - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - const auto records = controller_.records(); - EXPECT_FALSE(std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTask; - })); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigationStatusRejectsFaultArrivingDuringCompletedPoseVerification) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"controller says complete","task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - controller_.setResponseDelay( - kRobotStatusLoc, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvNavigationStatus status; - - std::thread status_thread([this, &status]() { - status = agv_->navigationStatus(); - }); - - bool location_query_observed = false; - for (int attempt = 0; attempt < 700; ++attempt) { - const auto records = controller_.records(); - location_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusLoc; - }); - if (location_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(location_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_STATUS_VERIFY_53: fault during completed pose check"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - status_thread.join(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE(status.message.find("target verification"), std::string::npos); - EXPECT_NE(status.message.find("E_STATUS_VERIFY_53"), std::string::npos); - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigationStatusRejectsCompletedPoseWhenTargetWasNotReached) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"controller says complete","task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":0.0,"y":0.0,"angle":0.0})"); - controller_.clearRecords(); - - const auto status = agv_->navigationStatus(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE( - status.message.find("requested target was not reached"), - std::string::npos); - EXPECT_NE(status.message.find("distance_error"), std::string::npos); - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); - EXPECT_EQ(records[1].command, kRobotStatusLoc); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigationStatusWaitsBrieflyForLateControllerFaultDetail) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"planner failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - controller_.clearRecords(); - AgvNavigationStatus status; - - std::thread status_thread([this, &status]() { - status = agv_->navigationStatus(); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value fatals(Json::arrayValue); - fatals.append("E_LATE_77: localization alarm"); - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = fatals; - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); - status_thread.join(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE(status.message.find("planner failed"), std::string::npos); - EXPECT_NE(status.message.find("E_LATE_77"), std::string::npos); - EXPECT_NE(status.message.find("localization alarm"), std::string::npos); - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); -} - -TEST_F( - SeerRobokitControlAuthorityTest, - NavigationStatusDoesNotReturnOldPoseTaskAfterStationSupersedesIt) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"old pose paused","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); - controller_.setResponseDelay( - kRobotStatusTaskPackage, - std::chrono::milliseconds(300)); - controller_.setResponsePayload( - kRobotStatusTask, - R"({"ret_code":0,"err_msg":"","task_status":2,"task_type":2,"move_status_info":"station task running"})"); - controller_.clearRecords(); - AgvNavigationStatus status; - - std::thread status_thread([this, &status]() { - status = agv_->navigationStatus(); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 500; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - const auto station_result = agv_->navigateToStation("station-1", asynchronousMotionOptions()); - status_thread.join(); - - ASSERT_TRUE(station_result.ok()) << station_result.message; - EXPECT_EQ(status.state, AgvTaskState::Running); - EXPECT_EQ(status.type, AgvTaskType::NavigateToStation); - EXPECT_EQ(status.message, "station task running"); - const auto records = controller_.records(); - EXPECT_TRUE(std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTask; - })); -} - -TEST_F(SeerRobokitControlAuthorityTest, DisconnectClearsTrackedPoseTask) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); - - const auto disconnect_result = SeerRobokitAgvTestPeer::disconnect(*agv_); - - ASSERT_TRUE(disconnect_result.ok()) << disconnect_result.message; - EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); -} - -TEST_F(SeerRobokitControlAuthorityTest, RuntimeStatePreservesCachedControllerFaultDetail) -{ - Json::Value push_payload(Json::objectValue); - Json::Value errors(Json::arrayValue); - Json::Value error(Json::objectValue); - *error.demand("code", "code" + std::strlen("code")) = "E_NAV_42"; - *error.demand("message", "message" + std::strlen("message")) = "planner alarm"; - errors.append(error); - *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; - SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, push_payload); - SeerRobokitAgvTestPeer::setAdapterError( - *agv_, - "SEER Robokit map file is empty after stripping the transport header"); - controller_.clearRecords(); - - const auto state = agv_->runtimeState(); - - EXPECT_TRUE(state.connected); - EXPECT_TRUE(state.fault); - EXPECT_EQ(state.mode, AgvMode::Fault); - EXPECT_NE(state.last_error.find("E_NAV_42"), std::string::npos); - EXPECT_NE(state.last_error.find("planner alarm"), std::string::npos); - EXPECT_NE(state.last_error.find("adapter_error="), std::string::npos); - EXPECT_NE(state.last_error.find("map file is empty"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F(SeerRobokitControlAuthorityTest, NavigationStatusPreservesControllerErrorCode) -{ - controller_.setResponseCode(kRobotStatusTask, 51020); - controller_.clearRecords(); - - const auto status = agv_->navigationStatus(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_NE(status.message.find("ret_code=51020"), std::string::npos); - EXPECT_NE(status.message.find("err_msg=simulated command failure"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotStatusTask); -} - -} // namespace -} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/src1100/CMakeLists.txt b/cmvr-es/devices/agv/src1100/CMakeLists.txt new file mode 100644 index 00000000..6ad1f3fe --- /dev/null +++ b/cmvr-es/devices/agv/src1100/CMakeLists.txt @@ -0,0 +1,12 @@ +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 new file mode 100644 index 00000000..ab5bcb92 --- /dev/null +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -0,0 +1,179 @@ +#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 new file mode 100644 index 00000000..7c1b1b2b --- /dev/null +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -0,0 +1,1898 @@ +#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/arm/CMakeLists.txt b/cmvr-es/devices/arm/CMakeLists.txt index 74f23c89..d2780c71 100644 --- a/cmvr-es/devices/arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/CMakeLists.txt @@ -1,7 +1,6 @@ add_subdirectory(motor_robot_arm) add_subdirectory(aubo_arm) add_subdirectory(huayan_arm) -add_subdirectory(ume_robot_arm) add_library(robot_arm INTERFACE) @@ -12,7 +11,6 @@ target_link_libraries(robot_arm cmvr_es::device::motor_robot_arm cmvr_es::device::aubo_arm cmvr_es::device::huayan_arm - cmvr_es::device::ume_robot_arm cmvr_es::proto ) diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index c684bc34..d56d09cb 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -2,8 +2,6 @@ add_library(aubo_arm SHARED aubo_arm.cpp ) -find_package(Threads REQUIRED) - target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1) @@ -73,94 +71,7 @@ target_link_libraries(aubo_arm cmvr_es::proto PRIVATE glog - jsoncpp - Threads::Threads ) add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm) install(TARGETS aubo_arm LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - enable_testing() - add_executable(aubo_arm_motion_result_test - tests/aubo_arm_motion_result_test.cpp - ) - target_include_directories(aubo_arm_motion_result_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - add_test( - NAME aubo_arm_motion_result_test - COMMAND aubo_arm_motion_result_test - ) - set_tests_properties(aubo_arm_motion_result_test PROPERTIES TIMEOUT 10) - - add_executable(aubo_motion_state_test - tests/aubo_motion_state_test.cpp - ) - target_include_directories(aubo_motion_state_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - add_test( - NAME aubo_motion_state_test - COMMAND aubo_motion_state_test - ) - set_tests_properties(aubo_motion_state_test PROPERTIES TIMEOUT 10) - - add_executable(aubo_safety_state_test - tests/aubo_safety_state_test.cpp - ) - target_include_directories(aubo_safety_state_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - add_test( - NAME aubo_safety_state_test - COMMAND aubo_safety_state_test - ) - set_tests_properties(aubo_safety_state_test PROPERTIES TIMEOUT 10) - - add_executable(aubo_arm_json_command_test - tests/aubo_arm_json_command_test.cpp - ) - target_link_libraries(aubo_arm_json_command_test - PRIVATE - cmvr_es::device::aubo_arm - cmvr_es::proto - ) - add_test( - NAME aubo_arm_json_command_test - COMMAND aubo_arm_json_command_test - ) - set_tests_properties(aubo_arm_json_command_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(UNIX AND NOT APPLE) - # The imported AUBO target still contributes its vendor directory to - # direct consumers' build-tree RUNPATH. Put the system runtime first - # for this test; installed artifacts exclude the vendor libstdc++. - execute_process( - COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so.6 - OUTPUT_VARIABLE AUBO_TEST_SYSTEM_LIBSTDCXX - OUTPUT_STRIP_TRAILING_WHITESPACE - ) - if(EXISTS "${AUBO_TEST_SYSTEM_LIBSTDCXX}") - get_filename_component( - AUBO_TEST_SYSTEM_LIBSTDCXX_REAL - "${AUBO_TEST_SYSTEM_LIBSTDCXX}" - REALPATH - ) - get_filename_component( - AUBO_TEST_SYSTEM_LIBSTDCXX_DIR - "${AUBO_TEST_SYSTEM_LIBSTDCXX_REAL}" - DIRECTORY - ) - set_property( - TARGET aubo_arm_json_command_test - PROPERTY BUILD_RPATH "${AUBO_TEST_SYSTEM_LIBSTDCXX_DIR}" - ) - endif() - endif() -endif() diff --git a/cmvr-es/devices/arm/aubo_arm/README.md b/cmvr-es/devices/arm/aubo_arm/README.md deleted file mode 100644 index f895c6cd..00000000 --- a/cmvr-es/devices/arm/aubo_arm/README.md +++ /dev/null @@ -1,153 +0,0 @@ -# AUBO RobotArm 与控制柜 IO - -`AuboArm` 是 AUBO SDK v0.27.1 的 `RobotArm` 后端。控制柜 Standard 数字 IO -通过设备通用的 `executeJsonCommand` 接口访问，远程调用复用 -`cmvr.api.ArmService/ExecuteJsonCommand`，不经过 `SystemService` 或 -`MotorService`。该 RPC 只路由到 `RobotArm`，不会把 JSON 命令转发给其他设备类型。 -旧的 `cmvr.api.SystemService/ExecuteJsonCommand` 不再注册，调用方必须更新服务路径； -请求和响应消息结构保持不变。 - -返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。 - -## 代码与配置 - -- 实现：[`aubo_arm.h`](aubo_arm.h)、[`aubo_arm.cpp`](aubo_arm.cpp) -- 测试：[`tests/aubo_arm_json_command_test.cpp`](tests/aubo_arm_json_command_test.cpp) -- 设备配置：[`../../../config/devices/arm/aubo_arm.pb.txt`](../../../config/devices/arm/aubo_arm.pb.txt) -- DeviceManager 配置： - [`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt) -- ArmService 实现： - [`../../../service/grpc/server/src/grpc_arm_service.cpp`](../../../service/grpc/server/src/grpc_arm_service.cpp) -- Proto：[`../../../../protos/cmvr/api/arm_service.proto`](../../../../protos/cmvr/api/arm_service.proto) - -仓库配置使用 SDK RPC 端口 `30004`。现场部署必须填写真实控制器地址和凭据， -不要把生产密码提交到默认配置。 - -## 控制柜 Standard 数字 IO - -当前支持： - -| `operation` | 说明 | 必填字段 | -| --- | --- | --- | -| `get_di` | 读取控制柜数字输入 | `index` | -| `get_do` | 读取控制柜数字输出及其 runstate | `index` | -| `set_do` | 设置控制柜数字输出 | `index`、`value` | - -JSON 命令： - -```json -{"command":"cabinet_io","operation":"get_di","index":0} -{"command":"cabinet_io","operation":"get_do","index":0} -{"command":"cabinet_io","operation":"set_do","index":0,"value":true} -``` - -`index` 从 `0` 开始，运行时根据控制器返回的 IO 数量检查范围。 -`set_do.value` 必须是 JSON 布尔值 `true` 或 `false`，不接受 `0/1` 或字符串。 -`set_do` 成功响应中的 `requested_value` 只表示 SDK 已接受请求；确认实际输出时 -必须再调用 `get_do`。 - -读取成功响应示例： - -```json -{ - "success": true, - "command": "cabinet_io", - "operation": "get_di", - "index": 0, - "count": 16, - "value": false -} -``` - -## 通过 gRPC 调用 - -默认 gRPC 端口为 `50052`。读取 DI0： - -```shell -grpcurl -plaintext \ - -d '{ - "header":{"deviceId":"aubo_arm"}, - "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"get_di\",\"index\":0}" - }' \ - 127.0.0.1:50052 \ - cmvr.api.ArmService/ExecuteJsonCommand -``` - -设置 DO0 为高电平： - -```shell -grpcurl -plaintext \ - -d '{ - "header":{"deviceId":"aubo_arm"}, - "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"set_do\",\"index\":0,\"value\":true}" - }' \ - 127.0.0.1:50052 \ - cmvr.api.ArmService/ExecuteJsonCommand -``` - -使用源码默认配置时： - -1. 在 `cmvr-es/config/devices/arm/aubo_arm.pb.txt` 填写正确地址和登录信息； -2. 在 `cmvr-es/config/manager/device_manager.pb.txt` 将 `aubo_arm.enable` - 改为 `true`； -3. 重新安装配置并启动安装产物。 - -```shell -cmake --install build -./output/bin/cmvr_es -``` - -`output/bin/cmvr_es` 默认读取 `output/bin/config/`。使用 `--config` 时，应修改 -对应外部配置根。设备未启用或初始化失败时，gRPC 返回 -`Device not found: aubo_arm`。 - -## 安全与语义边界 - -- 后端使用独立 SDK RPC 会话持续读取控制器的 `SafetyModeType`、 - `RobotModeType` 和硬件急停来源；首次有效样本前、监控断线或样本过期时， - 所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝； -- 硬件急停会立即使当前运动 generation 失效，并在急停输入有效期间保持锁存。 - 检测到硬件急停输入消失且控制器重新报告 `Normal`/`ReducedMode` 后，后端应 - 自动执行 `poweron()` 和 `startup()`，恢复到 `Running` 后再完成安全确认并开放新的 - gRPC 控制指令；防护停机和 Safety Fault/Violation 仍保持显式恢复语义； -- `emergencyStop()` 使用独立的 `SoftwareEmergencyStop` 锁存。即使软件急停在真实 - 硬件急停有效期间触发，后续硬件采样也不能覆盖该锁存，释放硬件急停开关不会 - 自动清除软件急停；它只能通过显式安全恢复流程解除； -- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。硬件急停 - 自动恢复先上电到 `Idle`，在刹车释放前清理 runtime、servo 和轨迹队列，再执行 - `startup()`；到达 `Running` 后还会再次确认 `ExecId == -1`、普通队列和轨迹队列 - 均为空、运行时已停止且机械臂稳定，全部成立后才能解除锁存； -- 当前 AUBO 配置通过 `auto_power_on_after_hardware_estop_release: true` 显式启用自动 - 上电。自动确认失败时继续保持 fail-closed，并允许通过 `torqueOn`/`clearFault`/ - `unlockProtectiveStop` 显式重试；本轮释放期间收到 `stopMotion()` 或 `torqueOff()` - 会取消自动上电，显式停止始终优先； -- 恢复流程只调用 `poweron()` 和 `startup()`，不会调用 `resume`、`arbitraryResume`、 - `startMove`，也不会重新提交急停前的目标、速度、servo 指令或程序； -- AUBO SDK 未在本地文档中保证急停期间 `clearPath` 的可用性，也未说明释放 - 急停开关后的控制器恢复时序。因此自动恢复必须在释放后再次清队列并完成上述 - 安全确认；无法确认时不得解除锁存。“释放开关后零位移”的最终保证仍需真机 - 验证及控制器侧安全配置配合； -- 只访问控制柜 Standard 数字 IO，不访问工具端 IO、可配置 IO 或安全 IO； -- `set_do` 不修改输出 runstate； -- 只有 `StandardOutputRunState::None` 的通道允许写入，否则返回 - `output_managed_by_runstate`； -- 普通访问不会调用会重置全部输出配置的 - `setDigitalOutputRunstateDefault()`； -- 模拟量 IO 涉及 domain、单位和量程，当前 JSON 接口不开放； -- gRPC/JSON 返回成功不代表目标 IO 具备功能安全等级； -- 真实写测试前应确认通道用途、负载、电气隔离、默认电平和控制器程序所有权。 - -## 测试 - -```bash -cmake --build build --target \ - aubo_safety_state_test \ - aubo_motion_state_test \ - aubo_arm_json_command_test -j4 -ctest --test-dir build \ - -R 'aubo_(safety_state|motion_state|arm_json_command)_test' \ - --output-on-failure -``` - -该测试覆盖 JSON 校验和无硬件错误路径，不代表已在真实 AUBO 控制柜完成 DI/DO -读取、写入或 runstate 拒绝验证。 diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 4c0da356..41dedc9f 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -1,230 +1,23 @@ #include "devices/arm/aubo_arm/aubo_arm.h" -#include "devices/arm/aubo_arm/aubo_motion_result.h" -#include "devices/arm/aubo_arm/aubo_motion_state.h" -#include "devices/arm/aubo_arm/aubo_safety_state.h" -#include "devices/arm/aubo_arm/aubo_torque_on_result.h" - #include -#include #include -#include -#include #include -#include #include #include -#include #include "common/base/logging/logger.h" -#include "json/json.h" #include "aubo_sdk/rpc.h" namespace cmvr::device { namespace { -bool cancellationRequested( - const std::function& cancellation_requested) noexcept -{ - if (!cancellation_requested) { - return false; - } - try { - return cancellation_requested(); - } catch (...) { - // A broken cancellation source must never permit a queued motion to be - // submitted after its ownership can no longer be established. - return true; - } -} - -class MotionOwnerGuard final { -public: - MotionOwnerGuard( - aubo_internal::MotionState& state, - std::atomic& busy, - const aubo_internal::MotionToken token) - : state_(state), busy_(busy), token_(token) - { - } - - ~MotionOwnerGuard() noexcept - { - try { - if (requires_settlement_ && !settled_) { - state_.failMotion(token_); - } else { - state_.finish(token_, finish_mode_); - } - busy_.store(state_.busy()); - } catch (...) { - // A failed state lock must not terminate an RPC unwind. Preserve - // the conservative externally visible state instead. - busy_.store(true); - } - } - - MotionOwnerGuard(const MotionOwnerGuard&) = delete; - MotionOwnerGuard& operator=(const MotionOwnerGuard&) = delete; - - void requireExplicitSettlement() noexcept - { - requires_settlement_ = true; - } - - void settle() noexcept { settled_ = true; } - - void clearOnFinish() noexcept - { - finish_mode_ = aubo_internal::MotionFinishMode::Clear; - } - - void retainKind() noexcept - { - finish_mode_ = aubo_internal::MotionFinishMode::Retain; - } - -private: - aubo_internal::MotionState& state_; - std::atomic& busy_; - aubo_internal::MotionToken token_; - aubo_internal::MotionFinishMode finish_mode_{ - aubo_internal::MotionFinishMode::RestorePrevious}; - bool requires_settlement_{false}; - bool settled_{false}; +struct BusyGuard { + std::atomic& busy; + ~BusyGuard() { busy.store(false); } }; -class StopStateGuard final { -public: - StopStateGuard( - aubo_internal::MotionState& state, - std::atomic& busy) - : state_(state), busy_(busy) - { - } - - ~StopStateGuard() noexcept - { - if (!completed_) { - try { - state_.failStop(); - } catch (...) { - // Keep the facade fail-closed even if state cleanup fails. - } - busy_.store(true); - } - } - - StopStateGuard(const StopStateGuard&) = delete; - StopStateGuard& operator=(const StopStateGuard&) = delete; - - bool complete() - { - if (!state_.completeStop()) { - return false; - } - busy_.store(false); - completed_ = true; - return true; - } - -private: - aubo_internal::MotionState& state_; - std::atomic& busy_; - bool completed_{false}; -}; - -class SafetyRecoveryGuard final { -public: - SafetyRecoveryGuard( - std::shared_ptr state, - const aubo_internal::RecoveryToken token) - : state_(std::move(state)), token_(token) - { - } - - ~SafetyRecoveryGuard() noexcept - { - if (!completed_ && state_) { - try { - state_->failRecovery(token_); - } catch (...) { - // The safety latch itself remains set on every failure path. - } - } - } - - SafetyRecoveryGuard(const SafetyRecoveryGuard&) = delete; - SafetyRecoveryGuard& operator=(const SafetyRecoveryGuard&) = delete; - - bool complete( - const bool robot_running, - const bool controller_idle, - const bool cancellation_confirmed) - { - if (!state_->completeRecovery( - token_, - robot_running, - controller_idle, - cancellation_confirmed)) { - return false; - } - completed_ = true; - return true; - } - - bool completeHardwareEmergencyStop( - const bool robot_running, - const bool controller_idle, - const bool cancellation_confirmed) - { - if (!state_->completeHardwareEmergencyStopRecovery( - token_, robot_running, controller_idle, - cancellation_confirmed)) { - return false; - } - completed_ = true; - return true; - } - -private: - std::shared_ptr state_; - aubo_internal::RecoveryToken token_; - bool completed_{false}; -}; - -const char* motionStartFailure( - const aubo_internal::MotionStartStatus status) noexcept -{ - switch (status) { - case aubo_internal::MotionStartStatus::Invalid: - return "invalid motion type"; - case aubo_internal::MotionStartStatus::Busy: - return "another motion is active"; - case aubo_internal::MotionStartStatus::Stopping: - return "a stop operation is in progress"; - case aubo_internal::MotionStartStatus::Blocked: - return "the previous stop did not complete; retry stopMotion or reconnect"; - case aubo_internal::MotionStartStatus::Started: - break; - } - return "unknown motion state"; -} - -const char* motionKindName(const aubo_internal::MotionKind kind) noexcept -{ - switch (kind) { - case aubo_internal::MotionKind::Joint: - return "joint"; - case aubo_internal::MotionKind::Linear: - return "linear"; - case aubo_internal::MotionKind::None: - break; - } - return "unknown"; -} - std::vector defaultJointNames(const std::size_t dof) { std::vector names; @@ -247,1000 +40,9 @@ std::string vendorBrandName(const config::VendorRobotArmBrand brand) } using arcs::common_interface::RobotModeType; -using arcs::common_interface::RuntimeState; -using arcs::common_interface::SafetyModeType; using arcs::aubo_sdk::RobotInterfacePtr; -RobotInterfacePtr getPrimaryRobotInterface( - const std::shared_ptr& rpc_client, - const std::string& context, - Result& result); - constexpr int kAuboServoMode = 3; -constexpr auto kSafetyPollInterval = std::chrono::milliseconds(50); -constexpr auto kSafetyReconnectInterval = std::chrono::milliseconds(250); -constexpr auto kSafetySampleMaxAge = std::chrono::milliseconds(500); - -std::int64_t monotonicNowNs() noexcept -{ - return std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()) - .count(); -} - -aubo_internal::SafetyCondition safetyConditionFromSdk( - const SafetyModeType mode) noexcept -{ - using Condition = aubo_internal::SafetyCondition; - switch (mode) { - case SafetyModeType::Normal: - return Condition::Normal; - case SafetyModeType::ReducedMode: - return Condition::Reduced; - case SafetyModeType::Recovery: - return Condition::Recovery; - case SafetyModeType::Violation: - return Condition::Violation; - case SafetyModeType::ProtectiveStop: - return Condition::ProtectiveStop; - case SafetyModeType::SafeguardStop: - return Condition::SafeguardStop; - case SafetyModeType::SystemEmergencyStop: - return Condition::SystemEmergencyStop; - case SafetyModeType::RobotEmergencyStop: - return Condition::RobotEmergencyStop; - case SafetyModeType::Fault: - return Condition::Fault; - case SafetyModeType::Undefined: - break; - } - return Condition::Unknown; -} - -const char* safetyConditionName( - const aubo_internal::SafetyCondition condition) noexcept -{ - using Condition = aubo_internal::SafetyCondition; - switch (condition) { - case Condition::Normal: - return "Normal"; - case Condition::Reduced: - return "Reduced"; - case Condition::Recovery: - return "Recovery"; - case Condition::Violation: - return "Violation"; - case Condition::ProtectiveStop: - return "ProtectiveStop"; - case Condition::SafeguardStop: - return "SafeguardStop"; - case Condition::SystemEmergencyStop: - return "SystemEmergencyStop"; - case Condition::RobotEmergencyStop: - return "RobotEmergencyStop"; - case Condition::SoftwareEmergencyStop: - return "SoftwareEmergencyStop"; - case Condition::Fault: - return "Fault"; - case Condition::Unknown: - break; - } - return "Unknown"; -} - -SafetyMode publicSafetyMode( - const aubo_internal::SafetyCondition condition) noexcept -{ - using Condition = aubo_internal::SafetyCondition; - switch (condition) { - case Condition::Normal: - return SafetyMode::Normal; - case Condition::Reduced: - return SafetyMode::Reduced; - case Condition::ProtectiveStop: - return SafetyMode::ProtectiveStop; - case Condition::SafeguardStop: - return SafetyMode::SafeguardStop; - case Condition::SystemEmergencyStop: - return SafetyMode::SystemEmergencyStop; - case Condition::RobotEmergencyStop: - case Condition::SoftwareEmergencyStop: - return SafetyMode::EmergencyStop; - case Condition::Violation: - case Condition::Fault: - return SafetyMode::Fault; - case Condition::Recovery: - case Condition::Unknown: - break; - } - return SafetyMode::Unknown; -} - -RobotMode publicRobotMode(const RobotModeType mode) noexcept -{ - switch (mode) { - case RobotModeType::NoController: - case RobotModeType::Disconnected: - return RobotMode::Disconnected; - case RobotModeType::PowerOff: - case RobotModeType::PowerOffing: - return RobotMode::PowerOff; - case RobotModeType::Running: - return RobotMode::Running; - case RobotModeType::Error: - return RobotMode::Fault; - case RobotModeType::PowerOn: - case RobotModeType::Idle: - case RobotModeType::BrakeReleasing: - case RobotModeType::BackDrive: - return RobotMode::Idle; - case RobotModeType::ConfirmSafety: - case RobotModeType::Booting: - case RobotModeType::Maintaince: - break; - } - return RobotMode::Unknown; -} - -struct AuboSafetyMonitor final { - std::shared_ptr safety_state{ - std::make_shared()}; - std::shared_ptr motion_state; - std::atomic safety_mode{ - static_cast(SafetyModeType::Undefined)}; - std::atomic robot_mode{ - static_cast(RobotModeType::Disconnected)}; - std::atomic runtime_state{ - static_cast(RuntimeState::Stopped)}; - std::atomic emergency_stop_source{-1}; - std::atomic hardware_emergency_stop_latched{false}; - std::atomic automatic_recovery_suppressed{false}; - std::atomic servo_mode_select{0}; - std::atomic last_sample_ns{0}; - std::atomic cancellation_confirmed{true}; - std::atomic runtime_abort_required{false}; - std::atomic servo_disable_required{false}; - std::atomic path_clear_required{false}; - std::atomic stop_requested{false}; - std::mutex wait_mutex; - std::condition_variable wait_cv; - std::mutex termination_mutex; - std::recursive_mutex command_rpc_mutex; - bool auto_power_on_after_hardware_estop_release{false}; - std::function on_hardware_estop_auto_recovered; - std::string arm_id; -}; - -std::shared_ptr makeRpcClient() -{ - return std::shared_ptr( - ::createRpcClient(), - [](arcs::aubo_sdk::RpcClient* client) { - if (client) { - ::destroyRpcClient(client); - } - }); -} - -bool monitorWait( - const std::shared_ptr& monitor, - const std::chrono::milliseconds duration) -{ - std::unique_lock lock(monitor->wait_mutex); - return monitor->wait_cv.wait_for( - lock, - duration, - [&monitor]() { return monitor->stop_requested.load(); }); -} - -void cancelForSafetyTransition( - const std::shared_ptr& monitor) -{ - monitor->cancellation_confirmed.store(false); - if (monitor->runtime_state.load() != - static_cast(RuntimeState::Stopped)) { - monitor->runtime_abort_required.store(true); - } - if (monitor->servo_mode_select.load() != 0) { - monitor->servo_disable_required.store(true); - } - monitor->motion_state->cancelActiveForSafety(); -} - -void publishSafetySample( - const std::shared_ptr& monitor, - const SafetyModeType safety_mode, - const RobotModeType robot_mode, - const RuntimeState runtime_state, - const int emergency_stop_source, - const int servo_mode_select) -{ - const auto previous = monitor->safety_state->snapshot(); - const int previous_runtime_state = monitor->runtime_state.load(); - const int previous_servo_mode = monitor->servo_mode_select.load(); - const auto condition = aubo_internal::effectiveSafetyCondition( - safetyConditionFromSdk(safety_mode), emergency_stop_source); - monitor->safety_state->observe(condition); - const auto current = monitor->safety_state->snapshot(); - - monitor->safety_mode.store(static_cast(safety_mode)); - monitor->robot_mode.store(static_cast(robot_mode)); - monitor->runtime_state.store(static_cast(runtime_state)); - monitor->emergency_stop_source.store(emergency_stop_source); - monitor->servo_mode_select.store(servo_mode_select); - monitor->last_sample_ns.store(monotonicNowNs()); - - if (emergency_stop_source != 0) { - const bool first_sample_for_event = - !monitor->hardware_emergency_stop_latched.exchange(true); - if (first_sample_for_event) { - monitor->automatic_recovery_suppressed.store(false); - } - } else if (!current.latched) { - monitor->hardware_emergency_stop_latched.store(false); - } - - if (previous.observed != condition || - (!previous.latched && current.latched)) { - if (current.latched) { - if (previous_runtime_state != - static_cast(RuntimeState::Stopped) || - runtime_state != RuntimeState::Stopped) { - monitor->runtime_abort_required.store(true); - } - if (previous_servo_mode != 0 || servo_mode_select != 0) { - monitor->servo_disable_required.store(true); - } - cancelForSafetyTransition(monitor); - } - if (current.latched) { - CMVR_LOG(WARNING) - << "[AuboArm] safety state changed, id=" << monitor->arm_id - << ", state=" << safetyConditionName(condition) - << ", latched=true"; - } else { - CMVR_LOG(INFO) - << "[AuboArm] safety state changed, id=" << monitor->arm_id - << ", state=" << safetyConditionName(condition) - << ", latched=false"; - } - } -} - -void refreshSafetySample( - const std::shared_ptr& rpc_client, - const std::shared_ptr& monitor, - const RobotInterfacePtr& robot_interface) -{ - auto robot_state = robot_interface->getRobotState(); - publishSafetySample( - monitor, - robot_state->getSafetyModeType(), - robot_state->getRobotModeType(), - rpc_client->getRuntimeMachine()->getRuntimeState(), - robot_interface->getRobotConfig() - ->getRobotEmergencyStopSource(), - robot_interface->getMotionControl()->getServoModeSelect()); -} - -void publishSafetyUnavailable( - const std::shared_ptr& monitor, - const std::string& reason) -{ - const auto previous = monitor->safety_state->snapshot(); - monitor->safety_state->observe( - aubo_internal::SafetyCondition::Unknown); - monitor->safety_mode.store( - static_cast(SafetyModeType::Undefined)); - monitor->robot_mode.store( - static_cast(RobotModeType::Disconnected)); - monitor->emergency_stop_source.store(-1); - monitor->last_sample_ns.store(0); - if (previous.observed != aubo_internal::SafetyCondition::Unknown || - !previous.latched) { - cancelForSafetyTransition(monitor); - CMVR_LOG(WARNING) - << "[AuboArm] safety monitor unavailable, id=" - << monitor->arm_id << ", reason=" << reason; - } -} - -bool safetySampleFresh( - const std::shared_ptr& monitor) noexcept -{ - const auto sample_ns = monitor->last_sample_ns.load(); - if (sample_ns <= 0) { - return false; - } - const auto age_ns = monotonicNowNs() - sample_ns; - return age_ns >= 0 && - age_ns <= std::chrono::duration_cast( - kSafetySampleMaxAge) - .count(); -} - -bool validateSafetyPermit( - const std::shared_ptr& monitor, - const aubo_internal::SafetyPermit permit) -{ - if (!safetySampleFresh(monitor)) { - publishSafetyUnavailable(monitor, "sample is stale"); - return false; - } - return monitor->emergency_stop_source.load() == 0 && - monitor->robot_mode.load() == - static_cast(RobotModeType::Running) && - monitor->safety_state->validate(permit); -} - -bool enforceControllerTermination( - const std::shared_ptr& rpc_client, - const std::shared_ptr& monitor) -{ - std::unique_lock command_rpc_lock( - monitor->command_rpc_mutex); - std::unique_lock termination_lock(monitor->termination_mutex); - monitor->motion_state->cancelActiveForSafety(); - auto stop_request = monitor->motion_state->beginStop(); - if (!stop_request.started()) { - constexpr auto kExistingStopTimeout = std::chrono::seconds(6); - const auto existing_result = - monitor->motion_state->waitForStopCompletion( - stop_request, - std::chrono::duration_cast( - kExistingStopTimeout)); - if (existing_result == aubo_internal::StopWaitStatus::Timeout) { - monitor->cancellation_confirmed.store(false); - return false; - } - - // A regular stopMotion confirms direct-motion idle, while safety - // termination additionally verifies runtime, servo and controller - // queues. Re-enter the stop state and perform that stronger check. A - // failed existing stop is retried here as well, while MotionState - // remains fail-closed between the two attempts. - monitor->motion_state->cancelActiveForSafety(); - stop_request = monitor->motion_state->beginStop(); - if (!stop_request.started()) { - monitor->cancellation_confirmed.store(false); - return false; - } - } - - const auto fail = [&monitor]() { - monitor->motion_state->failStop(); - monitor->cancellation_confirmed.store(false); - return false; - }; - - try { - Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - rpc_client, "safety termination", interface_result); - if (!interface_result.ok()) { - return fail(); - } - auto motion_control = robot_interface->getMotionControl(); - auto robot_state = robot_interface->getRobotState(); - auto runtime = rpc_client->getRuntimeMachine(); - - constexpr auto kTerminationTimeout = std::chrono::seconds(2); - constexpr int kStableSamples = 3; - const auto deadline = - std::chrono::steady_clock::now() + kTerminationTimeout; - int stable_samples = 0; - int iteration = 0; - bool typed_stop_acknowledged = - !stop_request.tracked_motion; - while (!monitor->stop_requested.load() && - std::chrono::steady_clock::now() < deadline) { - const int exec_id = motion_control->getExecId(); - const bool steady = robot_state->isSteady(); - int queue_size = motion_control->getQueueSize(); - int trajectory_queue_size = - motion_control->getTrajectoryQueueSize(); - int servo_mode = motion_control->getServoModeSelect(); - auto runtime_state = runtime->getRuntimeState(); - - if (runtime_state != RuntimeState::Stopped) { - monitor->runtime_abort_required.store(true); - } - if (servo_mode != 0) { - monitor->servo_disable_required.store(true); - } - if (queue_size != 0 || trajectory_queue_size != 0) { - monitor->path_clear_required.store(true); - } - - if (monitor->runtime_abort_required.load() && - iteration % 4 == 0 && - runtime->abort() == arcs::common_interface::AUBO_OK) { - monitor->runtime_abort_required.store(false); - } - if (monitor->servo_disable_required.load() && - iteration % 4 == 0 && - motion_control->setServoModeSelect(0) == - arcs::common_interface::AUBO_OK) { - monitor->servo_disable_required.store(false); - } - - const bool controller_moving = exec_id != -1 || !steady; - if ((stop_request.tracked_motion || controller_moving) && - iteration % 4 == 0) { - int stop_ret = arcs::common_interface::AUBO_OK; - if (stop_request.kind == aubo_internal::MotionKind::Joint) { - stop_ret = motion_control->stopJoint(31.0); - } else if (stop_request.kind == - aubo_internal::MotionKind::Linear) { - stop_ret = motion_control->stopLine(10.0, 10.0); - } else { - // RuntimeMachine::abort() is the only typed-independent - // SDK primitive documented to stop arbitrary operation. - monitor->runtime_abort_required.store(true); - stop_ret = runtime->abort(); - if (stop_ret == arcs::common_interface::AUBO_OK) { - monitor->runtime_abort_required.store(false); - } - } - if (stop_request.kind != - aubo_internal::MotionKind::None && - stop_ret == arcs::common_interface::AUBO_OK) { - typed_stop_acknowledged = true; - } - } - - if (monitor->path_clear_required.load() && - iteration % 4 == 0 && - motion_control->clearPath() == - arcs::common_interface::AUBO_OK) { - monitor->path_clear_required.store(false); - } - - queue_size = motion_control->getQueueSize(); - trajectory_queue_size = - motion_control->getTrajectoryQueueSize(); - servo_mode = motion_control->getServoModeSelect(); - runtime_state = runtime->getRuntimeState(); - const bool owner_active = monitor->motion_state->ownerActive( - stop_request.active_token); - const bool idle = - motion_control->getExecId() == -1 && - robot_state->isSteady() && - queue_size == 0 && - trajectory_queue_size == 0 && servo_mode == 0 && - runtime_state == RuntimeState::Stopped && !owner_active && - typed_stop_acknowledged && - !monitor->runtime_abort_required.load() && - !monitor->servo_disable_required.load() && - !monitor->path_clear_required.load() && - aubo_internal::isMotionSafe( - monitor->safety_state->snapshot().observed) && - monitor->emergency_stop_source.load() == 0; - - if (idle) { - if (++stable_samples >= kStableSamples) { - if (!monitor->motion_state->completeStop()) { - return fail(); - } - monitor->servo_mode_select.store(0); - monitor->runtime_state.store( - static_cast(RuntimeState::Stopped)); - monitor->cancellation_confirmed.store(true); - CMVR_LOG(INFO) - << "[AuboArm] safety termination confirmed, id=" - << monitor->arm_id; - return true; - } - } else { - stable_samples = 0; - } - - ++iteration; - if (monitorWait(monitor, kSafetyPollInterval)) { - break; - } - } - } catch (const std::exception& e) { - CMVR_LOG(WARNING) - << "[AuboArm] safety termination attempt failed, id=" - << monitor->arm_id << ", error=" << e.what(); - } - return fail(); -} - -// Powering on to Idle keeps the brakes engaged. This pre-startup phase clears -// the controller queues without completing MotionState, so the retained -// Joint/Linear kind survives until a typed stop is acknowledged in Running. -bool prepareControllerForStartup( - const std::shared_ptr& rpc_client, - const std::shared_ptr& monitor) -{ - std::unique_lock termination_lock(monitor->termination_mutex); - const auto cancellation = - monitor->motion_state->cancelActiveForSafety(); - - try { - Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - rpc_client, "pre-startup safety cleanup", interface_result); - if (!interface_result.ok()) { - return false; - } - auto motion_control = robot_interface->getMotionControl(); - auto runtime = rpc_client->getRuntimeMachine(); - - constexpr auto kCleanupTimeout = std::chrono::seconds(2); - constexpr int kStableSamples = 3; - const auto deadline = - std::chrono::steady_clock::now() + kCleanupTimeout; - int stable_samples = 0; - int iteration = 0; - while (!monitor->stop_requested.load() && - std::chrono::steady_clock::now() < deadline) { - int queue_size = motion_control->getQueueSize(); - int trajectory_queue_size = - motion_control->getTrajectoryQueueSize(); - int servo_mode = motion_control->getServoModeSelect(); - auto runtime_state = runtime->getRuntimeState(); - - if (runtime_state != RuntimeState::Stopped) { - monitor->runtime_abort_required.store(true); - } - if (servo_mode != 0) { - monitor->servo_disable_required.store(true); - } - if (queue_size != 0 || trajectory_queue_size != 0) { - monitor->path_clear_required.store(true); - } - - if (iteration % 4 == 0) { - if (monitor->runtime_abort_required.load() && - runtime->abort() == arcs::common_interface::AUBO_OK) { - monitor->runtime_abort_required.store(false); - } - if (monitor->servo_disable_required.load() && - motion_control->setServoModeSelect(0) == - arcs::common_interface::AUBO_OK) { - monitor->servo_disable_required.store(false); - } - if (monitor->path_clear_required.load() && - motion_control->clearPath() == - arcs::common_interface::AUBO_OK) { - monitor->path_clear_required.store(false); - } - } - - queue_size = motion_control->getQueueSize(); - trajectory_queue_size = - motion_control->getTrajectoryQueueSize(); - servo_mode = motion_control->getServoModeSelect(); - runtime_state = runtime->getRuntimeState(); - const bool owner_active = - monitor->motion_state->ownerActive( - cancellation.active_token); - const bool queues_cleared = - motion_control->getExecId() == -1 && - queue_size == 0 && trajectory_queue_size == 0 && - servo_mode == 0 && - runtime_state == RuntimeState::Stopped && - !owner_active && - !monitor->runtime_abort_required.load() && - !monitor->servo_disable_required.load() && - !monitor->path_clear_required.load(); - if (queues_cleared) { - if (++stable_samples >= kStableSamples) { - CMVR_LOG(INFO) - << "[AuboArm] pre-startup safety cleanup confirmed, id=" - << monitor->arm_id; - return true; - } - } else { - stable_samples = 0; - } - - ++iteration; - if (monitorWait(monitor, kSafetyPollInterval)) { - break; - } - } - } catch (const std::exception& e) { - CMVR_LOG(WARNING) - << "[AuboArm] pre-startup safety cleanup failed, id=" - << monitor->arm_id << ", error=" << e.what(); - } - return false; -} - -bool controllerStillQuiescent( - const std::shared_ptr& rpc_client, - const RobotInterfacePtr& robot_interface) -{ - auto motion_control = robot_interface->getMotionControl(); - return motion_control->getExecId() == -1 && - motion_control->getQueueSize() == 0 && - motion_control->getTrajectoryQueueSize() == 0 && - robot_interface->getRobotState()->isSteady() && - motion_control->getServoModeSelect() == 0 && - rpc_client->getRuntimeMachine()->getRuntimeState() == - RuntimeState::Stopped; -} - -bool hardwareEmergencyStopRecoveryCurrent( - const std::shared_ptr& monitor, - const aubo_internal::RecoveryToken token) -{ - const auto snapshot = monitor->safety_state->snapshot(); - return token.valid() && !monitor->stop_requested.load() && - !monitor->automatic_recovery_suppressed.load() && - monitor->emergency_stop_source.load() == 0 && snapshot.latched && - snapshot.recovery_in_progress && snapshot.epoch == token.epoch && - !snapshot.software_emergency_stop_latched && - snapshot.latched_reason == - aubo_internal::SafetyCondition::RobotEmergencyStop && - aubo_internal::isMotionSafe(snapshot.observed); -} - -bool waitForHardwareEmergencyStopRecoveryMode( - const RobotInterfacePtr& robot_interface, - const std::shared_ptr& monitor, - const aubo_internal::RecoveryToken token, - const RobotModeType target_mode) -{ - const auto deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(20); - while (std::chrono::steady_clock::now() < deadline) { - if (!hardwareEmergencyStopRecoveryCurrent(monitor, token)) { - return false; - } - if (robot_interface->getRobotState()->getRobotModeType() == - target_mode) { - return true; - } - if (monitorWait(monitor, std::chrono::milliseconds(100))) { - return false; - } - } - return false; -} - -bool autoPowerOnAfterHardwareEmergencyStop( - const std::shared_ptr& rpc_client, - const std::shared_ptr& monitor, - const RobotInterfacePtr& robot_interface) -{ - std::unique_lock command_rpc_lock( - monitor->command_rpc_mutex); - refreshSafetySample(rpc_client, monitor, robot_interface); - const auto snapshot = monitor->safety_state->snapshot(); - if (!aubo_internal::shouldAutoRecoverHardwareEmergencyStop( - snapshot, - monitor->hardware_emergency_stop_latched.load(), - monitor->emergency_stop_source.load(), - monitor->auto_power_on_after_hardware_estop_release, - monitor->automatic_recovery_suppressed.load())) { - return false; - } - - const auto token = monitor->safety_state->beginRecovery(snapshot.epoch); - if (!token.has_value()) { - return false; - } - SafetyRecoveryGuard recovery{monitor->safety_state, *token}; - const auto fail = [&monitor](const std::string& detail) { - CMVR_LOG(WARNING) - << "[AuboArm] hardware emergency-stop automatic power-on " - "failed, id=" - << monitor->arm_id << ", detail=" << detail; - return false; - }; - - try { - cancelForSafetyTransition(monitor); - if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) { - return fail("recovery was cancelled before controller setup"); - } - - double mass = 0.0; - std::vector cog(3, 0.0); - std::vector aom(3, 0.0); - std::vector inertia(6, 0.0); - const int payload_ret = robot_interface->getRobotConfig()->setPayload( - mass, cog, aom, inertia); - if (payload_ret != arcs::common_interface::AUBO_OK) { - return fail("setPayload ret=" + std::to_string(payload_ret)); - } - if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) { - return fail("recovery was cancelled after payload setup"); - } - - auto current_mode = - robot_interface->getRobotState()->getRobotModeType(); - if (current_mode != RobotModeType::Running && - current_mode != RobotModeType::Idle) { - const int power_on_ret = - robot_interface->getRobotManage()->poweron(); - if (power_on_ret != arcs::common_interface::AUBO_OK) { - return fail("poweron ret=" + std::to_string(power_on_ret)); - } - if (!waitForHardwareEmergencyStopRecoveryMode( - robot_interface, monitor, *token, - RobotModeType::Idle)) { - return fail("Idle was not reached after poweron"); - } - current_mode = RobotModeType::Idle; - } - - refreshSafetySample(rpc_client, monitor, robot_interface); - if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) { - return fail("safety state changed before brake release"); - } - - const bool cleanup_ok = current_mode == RobotModeType::Running - ? enforceControllerTermination(rpc_client, monitor) - : prepareControllerForStartup(rpc_client, monitor); - if (!cleanup_ok) { - return fail("old runtime, servo, or path state could not be cleared"); - } - if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token)) { - return fail("safety state changed during pre-startup cleanup"); - } - - if (current_mode != RobotModeType::Running) { - const int startup_ret = - robot_interface->getRobotManage()->startup(); - if (startup_ret != arcs::common_interface::AUBO_OK) { - return fail("startup ret=" + std::to_string(startup_ret)); - } - if (!waitForHardwareEmergencyStopRecoveryMode( - robot_interface, monitor, *token, - RobotModeType::Running)) { - return fail("Running was not reached after startup"); - } - } - - refreshSafetySample(rpc_client, monitor, robot_interface); - if (!hardwareEmergencyStopRecoveryCurrent(monitor, *token) || - monitor->robot_mode.load() != - static_cast(RobotModeType::Running)) { - return fail("controller safety changed during startup"); - } - - // Startup is allowed to energize the arm, but it must not revive an - // old controller operation. Terminate once more in Running mode and - // require a fresh empty/steady observation before reopening commands. - cancelForSafetyTransition(monitor); - if (!enforceControllerTermination(rpc_client, monitor)) { - return fail("post-startup controller quiescence was not confirmed"); - } - refreshSafetySample(rpc_client, monitor, robot_interface); - const bool robot_running = - monitor->robot_mode.load() == - static_cast(RobotModeType::Running); - const bool controller_idle = - robot_running && - hardwareEmergencyStopRecoveryCurrent(monitor, *token) && - controllerStillQuiescent(rpc_client, robot_interface); - if (!recovery.completeHardwareEmergencyStop( - robot_running, - controller_idle, - monitor->cancellation_confirmed.load())) { - return fail("safety epoch changed before recovery commit"); - } - - monitor->hardware_emergency_stop_latched.store(false); - monitor->automatic_recovery_suppressed.store(false); - if (monitor->on_hardware_estop_auto_recovered) { - monitor->on_hardware_estop_auto_recovered(); - } - CMVR_LOG(INFO) - << "[AuboArm] hardware emergency-stop release automatically " - "powered on and enabled, id=" - << monitor->arm_id; - return true; - } catch (const std::exception& error) { - return fail(error.what()); - } catch (...) { - return fail("unknown exception"); - } -} - -void runSafetyMonitor( - const std::shared_ptr& monitor, - const std::string& ip, - const int port, - const std::string& username, - const std::string& password) -{ - while (!monitor->stop_requested.load()) { - auto rpc_client = makeRpcClient(); - try { - if (!rpc_client) { - publishSafetyUnavailable(monitor, "create RPC client failed"); - } else { - rpc_client->setRequestTimeout(250); - const int connect_ret = rpc_client->connect(ip, port); - const int login_ret = connect_ret == 0 - ? rpc_client->login(username, password) - : connect_ret; - if (connect_ret != 0 || login_ret != 0) { - publishSafetyUnavailable( - monitor, - "monitor RPC connect/login failed"); - } else { - Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - rpc_client, "safety monitor", interface_result); - if (!interface_result.ok()) { - publishSafetyUnavailable( - monitor, interface_result.message); - } else { - while (!monitor->stop_requested.load()) { - refreshSafetySample( - rpc_client, monitor, robot_interface); - - auto safety = monitor->safety_state->snapshot(); - const bool hardware_estop_released = - aubo_internal:: - shouldAutoRecoverHardwareEmergencyStop( - safety, - monitor - ->hardware_emergency_stop_latched - .load(), - monitor->emergency_stop_source.load(), - monitor - ->auto_power_on_after_hardware_estop_release, - monitor - ->automatic_recovery_suppressed - .load()); - if (hardware_estop_released) { - (void)autoPowerOnAfterHardwareEmergencyStop( - rpc_client, monitor, robot_interface); - safety = monitor->safety_state->snapshot(); - } - - if (safety.latched) { - if (monitor->cancellation_confirmed.load() && - !controllerStillQuiescent( - rpc_client, robot_interface)) { - CMVR_LOG(WARNING) - << "[AuboArm] controller activity reappeared while safety was latched, id=" - << monitor->arm_id; - cancelForSafetyTransition(monitor); - } - if (!monitor->cancellation_confirmed.load()) { - (void)enforceControllerTermination( - rpc_client, monitor); - } - } - if (monitorWait( - monitor, kSafetyPollInterval)) { - break; - } - } - } - } - } - } catch (const std::exception& e) { - publishSafetyUnavailable(monitor, e.what()); - } - - if (rpc_client) { - try { - if (rpc_client->hasLogined()) { - rpc_client->logout(); - } - if (rpc_client->hasConnected()) { - rpc_client->disconnect(); - } - } catch (const std::exception& e) { - CMVR_LOG(WARNING) - << "[AuboArm] safety monitor cleanup failed, id=" - << monitor->arm_id << ", error=" << e.what(); - } - } - if (!monitor->stop_requested.load()) { - publishSafetyUnavailable(monitor, "monitor RPC disconnected"); - (void)monitorWait(monitor, kSafetyReconnectInterval); - } - } -} - -enum class CabinetIoOperation { - GetDigitalInput, - GetDigitalOutput, - SetDigitalOutput, -}; - -std::string lowerString(std::string value) -{ - std::transform(value.begin(), value.end(), value.begin(), [](const unsigned char c) { - return static_cast(std::tolower(c)); - }); - return value; -} - -bool parseJsonCommand(const std::string& request_json, - Json::Value& root, - std::string& error) -{ - Json::CharReaderBuilder builder; - std::unique_ptr reader(builder.newCharReader()); - return reader->parse( - request_json.data(), - request_json.data() + request_json.size(), - &root, - &error); -} - -std::string compactJson(const Json::Value& value) -{ - Json::StreamWriterBuilder builder; - builder[std::string("indentation")] = ""; - return Json::writeString(builder, value); -} - -Json::Value& jsonMember(Json::Value& root, const char* name) -{ - return *root.demand(name, name + std::strlen(name)); -} - -const Json::Value* findJsonMember(const Json::Value& root, const char* name) -{ - return root.find(name, name + std::strlen(name)); -} - -bool requiredJsonString(const Json::Value& root, - const char* name, - std::string& value) -{ - const Json::Value* member = findJsonMember(root, name); - if (!member || !member->isString() || member->asString().empty()) { - return false; - } - value = member->asString(); - return true; -} - -bool requiredJsonInt(const Json::Value& root, const char* name, int& value) -{ - const Json::Value* member = findJsonMember(root, name); - if (!member || !member->isInt()) { - return false; - } - value = member->asInt(); - return true; -} - -bool requiredJsonBool(const Json::Value& root, const char* name, bool& value) -{ - const Json::Value* member = findJsonMember(root, name); - if (!member || !member->isBool()) { - return false; - } - value = member->asBool(); - return true; -} - -bool parseCabinetIoOperation(const std::string& name, CabinetIoOperation& operation) -{ - const std::string normalized = lowerString(name); - if (normalized == "get_di") { - operation = CabinetIoOperation::GetDigitalInput; - } else if (normalized == "get_do") { - operation = CabinetIoOperation::GetDigitalOutput; - } else if (normalized == "set_do") { - operation = CabinetIoOperation::SetDigitalOutput; - } else { - return false; - } - return true; -} - -std::string standardOutputRunstateName( - const arcs::common_interface::StandardOutputRunState runstate) -{ - return arcs::common_interface::toString(runstate); -} RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr& rpc_client, const std::string& context, @@ -1264,74 +66,35 @@ RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr& cancellation_requested) -{ - const auto start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(20)) { - if (cancellationRequested(cancellation_requested)) { - return RobotModeWaitResult::Cancelled; - } - const auto current_mode = robot_interface->getRobotState()->getRobotModeType(); - if (current_mode == target_mode) { - return RobotModeWaitResult::Reached; - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - return cancellationRequested(cancellation_requested) - ? RobotModeWaitResult::Cancelled - : RobotModeWaitResult::Timeout; -} - bool waitForRobotMode(const RobotInterfacePtr& robot_interface, const RobotModeType& target_mode) { - return waitForRobotMode(robot_interface, target_mode, {}) == - RobotModeWaitResult::Reached; + const auto start_time = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(20)) { + const auto current_mode = robot_interface->getRobotState()->getRobotModeType(); + if (current_mode == target_mode) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + return false; } -template -aubo_internal::MotionWaitResult waitArrival( - const RobotInterfacePtr& robot_interface, - IsCancelled&& is_cancelled) +int waitArrival(const RobotInterfacePtr& robot_interface) { int retry_count = 0; - if (is_cancelled()) { - return aubo_internal::MotionWaitResult::Cancelled; - } int exec_id = robot_interface->getMotionControl()->getExecId(); while (exec_id == -1 && retry_count++ < 5) { - if (is_cancelled()) { - return aubo_internal::MotionWaitResult::Cancelled; - } std::this_thread::sleep_for(std::chrono::milliseconds(50)); - if (is_cancelled()) { - return aubo_internal::MotionWaitResult::Cancelled; - } exec_id = robot_interface->getMotionControl()->getExecId(); } if (exec_id == -1) { - return is_cancelled() - ? aubo_internal::MotionWaitResult::Cancelled - : aubo_internal::MotionWaitResult::Failed; + return -1; } while (robot_interface->getMotionControl()->getExecId() != -1) { - if (is_cancelled()) { - return aubo_internal::MotionWaitResult::Cancelled; - } std::this_thread::sleep_for(std::chrono::milliseconds(50)); } - return is_cancelled() - ? aubo_internal::MotionWaitResult::Cancelled - : aubo_internal::MotionWaitResult::Completed; + return 0; } bool waitServoModeSelect(const RobotInterfacePtr& robot_interface, const int mode) @@ -1363,21 +126,6 @@ CartesianPose poseFromVector(const std::vector& values) struct AuboArm::SdkState { std::shared_ptr rpc_client; - std::shared_ptr motion_state{ - std::make_shared()}; - std::shared_ptr safety_monitor; - std::thread safety_monitor_thread; - - ~SdkState() - { - if (safety_monitor) { - safety_monitor->stop_requested.store(true); - safety_monitor->wait_cv.notify_all(); - } - if (safety_monitor_thread.joinable()) { - safety_monitor_thread.join(); - } - } }; AuboArm::AuboArm(const config::RobotArmConfig& cfg) @@ -1431,171 +179,17 @@ bool AuboArm::stop() return stopMotion().ok(); } -bool AuboArm::executeJsonCommand(const std::string& request_json, - std::string& response_json) -{ - Json::Value response(Json::objectValue); - jsonMember(response, "success") = false; - const auto fail = [&](const std::string& error_code, - const std::string& error_message) { - jsonMember(response, "success") = false; - jsonMember(response, "error_code") = error_code; - jsonMember(response, "error_message") = error_message; - response_json = compactJson(response); - return false; - }; - - Json::Value root; - std::string parse_error; - if (!parseJsonCommand(request_json, root, parse_error)) { - return fail("invalid_json", "invalid json: " + parse_error); - } - if (!root.isObject()) { - return fail("invalid_json", "invalid json: root must be an object"); - } - - std::string command; - if (!requiredJsonString(root, "command", command)) { - return fail("invalid_argument", - "field 'command' is required and must be a non-empty string"); - } - command = lowerString(command); - if (command != "cabinet_io") { - return fail("unsupported_command", "unsupported json command: " + command); - } - jsonMember(response, "command") = command; - - std::string operation_name; - if (!requiredJsonString(root, "operation", operation_name)) { - return fail("invalid_argument", - "field 'operation' is required and must be a non-empty string"); - } - operation_name = lowerString(operation_name); - CabinetIoOperation operation{}; - if (!parseCabinetIoOperation(operation_name, operation)) { - return fail("invalid_operation", - "unsupported cabinet_io operation: " + operation_name); - } - jsonMember(response, "operation") = operation_name; - - int index = -1; - if (!requiredJsonInt(root, "index", index) || index < 0) { - return fail("invalid_argument", - "field 'index' is required and must be a non-negative JSON integer"); - } - jsonMember(response, "index") = index; - - bool output_value = false; - if (operation == CabinetIoOperation::SetDigitalOutput && - !requiredJsonBool(root, "value", output_value)) { - return fail("invalid_argument", - "field 'value' is required for set_do and must be a JSON boolean"); - } - - std::lock_guard lock(mutex_); - const auto ready = ensureConnected_("cabinet_io"); - if (!ready.ok()) { - return fail("not_connected", ready.message); - } - - try { - Result interface_result; - auto robot_interface = - getPrimaryRobotInterface(sdk_->rpc_client, "cabinet_io", interface_result); - if (!interface_result.ok() || !robot_interface) { - return fail("robot_interface_unavailable", interface_result.message); - } - - auto io = robot_interface->getIoControl(); - if (!io) { - return fail("io_interface_unavailable", - "[AuboArm] cabinet_io failed: IO interface is null"); - } - - const bool is_input = operation == CabinetIoOperation::GetDigitalInput; - const int count = is_input - ? io->getStandardDigitalInputNum() - : io->getStandardDigitalOutputNum(); - jsonMember(response, "count") = count; - if (index >= count) { - return fail( - "index_out_of_range", - "[AuboArm] cabinet_io index out of range: index=" + - std::to_string(index) + ", count=" + std::to_string(count)); - } - - if (operation == CabinetIoOperation::GetDigitalInput) { - jsonMember(response, "value") = io->getStandardDigitalInput(index); - } else { - const auto runstate = io->getStandardDigitalOutputRunstate(index); - jsonMember(response, "runstate") = standardOutputRunstateName(runstate); - jsonMember(response, "runstate_code") = static_cast(runstate); - - if (operation == CabinetIoOperation::GetDigitalOutput) { - jsonMember(response, "value") = - io->getStandardDigitalOutput(index); - } else { - if (runstate != arcs::common_interface::StandardOutputRunState::None) { - return fail( - "output_managed_by_runstate", - "[AuboArm] cabinet_io set_do rejected: output is managed by " - "controller runstate; configure this channel as None before writing"); - } - - const int ret = io->setStandardDigitalOutput(index, output_value); - jsonMember(response, "sdk_return_code") = ret; - if (ret != 0) { - return fail( - "sdk_command_failed", - "[AuboArm] cabinet_io set_do failed: sdk ret=" + - std::to_string(ret)); - } - jsonMember(response, "requested_value") = output_value; - } - } - - jsonMember(response, "success") = true; - response_json = compactJson(response); - return true; - } catch (const arcs::common_interface::AuboException& e) { - jsonMember(response, "sdk_return_code") = e.code(); - return fail("sdk_exception", - std::string("[AuboArm] cabinet_io failed: ") + e.what()); - } catch (const std::exception& e) { - return fail("sdk_exception", - std::string("[AuboArm] cabinet_io failed: ") + e.what()); - } -} - ArmState AuboArm::getRobotState() const { ArmState state; state.connected = connected_.load(); - state.moving = busy(); + state.powered_on = state.connected; + state.brake_released = state.connected; + state.moving = busy_.load(); state.robot_mode = getRobotMode(); state.safety_mode = getSafetyMode(); state.control_mode = getControlMode(); - state.powered_on = state.connected && - state.robot_mode != RobotMode::Disconnected && - state.robot_mode != RobotMode::PowerOff && - state.robot_mode != RobotMode::Unknown; - state.brake_released = - state.robot_mode == RobotMode::Running && - state.safety_mode != SafetyMode::EmergencyStop && - state.safety_mode != SafetyMode::SystemEmergencyStop && - state.safety_mode != SafetyMode::SafeguardStop; - state.program_running = false; - { - std::lock_guard lock(mutex_); - if (sdk_ && sdk_->safety_monitor) { - state.program_running = - sdk_->safety_monitor->runtime_state.load() == - static_cast(RuntimeState::Running); - } - } - state.protective_stopped = isProtectiveStopped(); - state.emergency_stopped = isEmergencyStopped(); - state.fault = isFault(); + state.emergency_stopped = emergency_stopped_; state.speed_scaling = speed_scaling_; state.actual_joint_state = getJointState(); state.target_joint_state = state.actual_joint_state; @@ -1680,548 +274,71 @@ RobotMode AuboArm::getRobotMode() const if (!connected_.load()) { return RobotMode::Disconnected; } - const auto safety_mode = getSafetyMode(); - if (safety_mode == SafetyMode::Fault) { - return RobotMode::Fault; - } - if (safety_mode == SafetyMode::ProtectiveStop || - safety_mode == SafetyMode::SafeguardStop || - safety_mode == SafetyMode::EmergencyStop || - safety_mode == SafetyMode::SystemEmergencyStop) { + if (emergency_stopped_) { return RobotMode::Stopped; } - - std::lock_guard lock(mutex_); - if (!sdk_ || !sdk_->safety_monitor) { - return RobotMode::Unknown; - } - return publicRobotMode(static_cast( - sdk_->safety_monitor->robot_mode.load())); -} - -SafetyMode AuboArm::getSafetyMode() const -{ - if (!connected_.load()) { - return SafetyMode::Unknown; - } - std::lock_guard lock(mutex_); - if (!sdk_ || !sdk_->safety_monitor) { - return SafetyMode::Unknown; - } - const auto monitor = sdk_->safety_monitor; - if (!safetySampleFresh(monitor)) { - publishSafetyUnavailable(monitor, "sample is stale"); - } - const auto snapshot = monitor->safety_state->snapshot(); - return publicSafetyMode( - snapshot.latched ? snapshot.latched_reason : snapshot.observed); -} - -ControlMode AuboArm::getControlMode() const -{ - if (!connected_.load()) { - return ControlMode::None; - } - std::lock_guard lock(mutex_); - if (sdk_ && sdk_->safety_monitor && - sdk_->safety_monitor->servo_mode_select.load() != 0) { - return ControlMode::Servo; - } - return ControlMode::Position; -} - -bool AuboArm::isProtectiveStopped() const -{ - const auto mode = getSafetyMode(); - return mode == SafetyMode::ProtectiveStop || - mode == SafetyMode::SafeguardStop; -} - -bool AuboArm::isEmergencyStopped() const -{ - const auto mode = getSafetyMode(); - return emergency_stopped_.load() || - mode == SafetyMode::EmergencyStop || - mode == SafetyMode::SystemEmergencyStop; -} - -bool AuboArm::isFault() const -{ - return getSafetyMode() == SafetyMode::Fault || - getRobotMode() == RobotMode::Fault; -} - -bool AuboArm::busy() const -{ - std::lock_guard lock(mutex_); - if (sdk_) { - return sdk_->motion_state->busy(); - } - return false; + return busy_.load() ? RobotMode::Running : RobotMode::Idle; } Result AuboArm::torqueOn() { - return torqueOn({}); -} - -Result AuboArm::torqueOn( - const std::function& cancellation_requested) -{ - std::shared_ptr rpc_client; - std::shared_ptr monitor; - { - std::lock_guard lock(mutex_); - const auto ready = ensureConnected_("torqueOn"); - if (!ready.ok()) { - return ready; - } - rpc_client = sdk_->rpc_client; - monitor = sdk_->safety_monitor; + const auto ready = ensureConnected_("torqueOn"); + if (!ready.ok()) { + return ready; } - bool cancellation_latched = false; - bool controller_mutated = false; - const std::function cancellation_check = - [&cancellation_requested, &cancellation_latched]() { - if (!cancellation_latched) { - cancellation_latched = cancellationRequested( - cancellation_requested); - } - return cancellation_latched; - }; - const auto cancelled_before_startup = []() { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] torqueOn cancelled before controller startup"); - }; - const auto cancellation_result = [&]() -> std::optional { - if (!cancellation_check()) { - return std::nullopt; - } - if (!controller_mutated) { - return cancelled_before_startup(); - } - - try { - std::unique_lock cleanup_rpc_lock( - monitor->command_rpc_mutex); - cancelForSafetyTransition(monitor); - if (!enforceControllerTermination(rpc_client, monitor)) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn cancelled: control ownership was revoked, but controller safety termination could not be confirmed"); - } - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] torqueOn cancelled: control ownership was revoked; controller safety termination confirmed"); - } catch (const std::exception& e) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn cancelled: controller safety termination raised an exception: " + - std::string(e.what())); - } catch (...) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn cancelled: controller safety termination raised an unknown exception"); - } - }; - try { - if (!monitor) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] torqueOn failed: hardware safety monitor is unavailable"); + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); } - - if (cancellation_check()) { - return cancelled_before_startup(); - } - std::unique_lock command_rpc_lock( - monitor->command_rpc_mutex); - if (cancellation_check()) { - return cancelled_before_startup(); - } - Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - rpc_client, "torqueOn", interface_result); - if (!interface_result.ok()) { - return interface_result; - } - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - - refreshSafetySample(rpc_client, monitor, robot_interface); - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - auto safety_snapshot = monitor->safety_state->snapshot(); - const std::uint64_t entry_safety_epoch = safety_snapshot.epoch; - if (monitor->emergency_stop_source.load() != 0) { - return Result::failure( - ArmErrorCode::RobotInEmergencyStop, - "[AuboArm] torqueOn rejected: hardware emergency-stop input is still active"); - } - - aubo_internal::RecoveryToken recovery_token; - std::unique_ptr recovery_guard; - bool recovering = safety_snapshot.latched; - if (recovering && - !aubo_internal::isMotionSafe(safety_snapshot.observed)) { - const auto condition = safety_snapshot.observed; - if (aubo_internal::needsProtectiveUnlock(condition)) { - return Result::failure( - ArmErrorCode::RobotInProtectiveStop, - "[AuboArm] torqueOn rejected: ProtectiveStop/Violation must be cleared with unlockProtectiveStop first"); - } - if (condition == - aubo_internal::SafetyCondition::SafeguardStop) { - return Result::failure( - ArmErrorCode::RobotInProtectiveStop, - "[AuboArm] torqueOn rejected: SafeguardStop requires the external safety IO to be cleared"); - } - if (condition == - aubo_internal::SafetyCondition::Recovery) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] torqueOn rejected: Recovery mode requires manually moving the arm inside its safety limits"); - } - if (!aubo_internal::needsInterfaceBoardRestart(condition)) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] torqueOn rejected: safety state is " + - std::string(safetyConditionName(condition))); - } - - controller_mutated = true; - if (const auto mutation_result = - aubo_internal::runTorqueOnControllerMutation( - arcs::common_interface::AUBO_OK, - [&robot_interface] { - return robot_interface->getRobotManage() - ->restartInterfaceBoard(); - }, - [](const int return_code) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn recovery failed: " - "restartInterfaceBoard ret=" + - std::to_string(return_code)); - }, - cancellation_result)) { - return *mutation_result; - } - - const auto safety_deadline = - std::chrono::steady_clock::now() + - std::chrono::seconds(10); - do { - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - std::this_thread::sleep_for( - std::chrono::milliseconds(100)); - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - refreshSafetySample( - rpc_client, monitor, robot_interface); - safety_snapshot = monitor->safety_state->snapshot(); - if (aubo_internal::isMotionSafe( - safety_snapshot.observed)) { - break; - } - } while (std::chrono::steady_clock::now() < - safety_deadline); - } - - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - - safety_snapshot = monitor->safety_state->snapshot(); - if (safety_snapshot.epoch != entry_safety_epoch) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] torqueOn recovery rejected: a new safety event occurred while resetting the controller; retry recovery explicitly"); - } - if (!aubo_internal::isMotionSafe(safety_snapshot.observed)) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] torqueOn rejected: safety state is " + - std::string(safetyConditionName( - safety_snapshot.observed))); - } - - if (recovering) { - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - const auto token = monitor->safety_state->beginRecovery( - entry_safety_epoch); - if (!token.has_value()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] torqueOn recovery rejected: safety latch changed"); - } - recovery_token = *token; - recovery_guard = std::make_unique( - monitor->safety_state, recovery_token); - } - - if (const auto cancelled = cancellation_result()) { - return *cancelled; + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } double mass = 0.0; std::vector cog(3, 0.0); std::vector aom(3, 0.0); std::vector inertia(6, 0.0); - controller_mutated = true; - if (const auto mutation_result = - aubo_internal::runTorqueOnControllerMutation( - arcs::common_interface::AUBO_OK, - [&robot_interface, mass, &cog, &aom, &inertia] { - return robot_interface->getRobotConfig()->setPayload( - mass, cog, aom, inertia); - }, - [](const int return_code) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn failed: setPayload ret=" + - std::to_string(return_code)); - }, - cancellation_result)) { - return *mutation_result; - } + robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia); - auto current_mode = - robot_interface->getRobotState()->getRobotModeType(); - if (current_mode != RobotModeType::Running && - current_mode != RobotModeType::Idle) { - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - controller_mutated = true; - if (const auto mutation_result = - aubo_internal::runTorqueOnControllerMutation( - arcs::common_interface::AUBO_OK, - [&robot_interface] { - return robot_interface->getRobotManage()->poweron(); - }, - [](const int return_code) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn failed: poweron ret=" + - std::to_string(return_code)); - }, - cancellation_result)) { - return *mutation_result; - } - const auto idle_wait = waitForRobotMode( - robot_interface, - arcs::common_interface::RobotModeType::Idle, - cancellation_check); - if (idle_wait != RobotModeWaitResult::Reached) { - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } + const auto current_mode = robot_interface->getRobotState()->getRobotModeType(); + if (current_mode != arcs::common_interface::RobotModeType::Running) { + robot_interface->getRobotManage()->poweron(); + if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Idle)) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Idle"); } - current_mode = RobotModeType::Idle; - } - - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - refreshSafetySample(rpc_client, monitor, robot_interface); - auto before_brake_release = - monitor->safety_state->snapshot(); - const std::uint64_t expected_epoch = recovering - ? recovery_token.epoch - : entry_safety_epoch; - const bool recovery_token_current = !recovering || - (before_brake_release.recovery_in_progress && - before_brake_release.epoch == recovery_token.epoch); - if (before_brake_release.epoch != expected_epoch || - !recovery_token_current || before_brake_release.latched != recovering || - !aubo_internal::isMotionSafe( - before_brake_release.observed) || - monitor->emergency_stop_source.load() != 0) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] torqueOn rejected: safety state changed before brake release; the new event remains latched"); - } - - if (recovering) { - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - controller_mutated = true; - cancelForSafetyTransition(monitor); - const bool cleanup_ok = current_mode == RobotModeType::Running - ? enforceControllerTermination(rpc_client, monitor) - : prepareControllerForStartup(rpc_client, monitor); - if (!cleanup_ok) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn recovery failed: old controller queue could not be acknowledged and cleared before brake release"); - } - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - } - - if (current_mode != RobotModeType::Running) { - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - controller_mutated = true; - if (const auto mutation_result = - aubo_internal::runTorqueOnControllerMutation( - arcs::common_interface::AUBO_OK, - [&robot_interface] { - return robot_interface->getRobotManage()->startup(); - }, - [](const int return_code) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn failed: startup ret=" + - std::to_string(return_code)); - }, - cancellation_result)) { - return *mutation_result; - } - const auto running_wait = waitForRobotMode( - robot_interface, - arcs::common_interface::RobotModeType::Running, - cancellation_check); - if (running_wait != RobotModeWaitResult::Reached) { - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } + robot_interface->getRobotManage()->startup(); + if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Running)) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Running"); } } - - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - refreshSafetySample(rpc_client, monitor, robot_interface); - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - const auto after_startup = monitor->safety_state->snapshot(); - const bool post_recovery_token_current = !recovering || - (after_startup.recovery_in_progress && - after_startup.epoch == recovery_token.epoch); - if (after_startup.epoch != expected_epoch || - !post_recovery_token_current || after_startup.latched != recovering || - !aubo_internal::isMotionSafe(after_startup.observed) || - monitor->emergency_stop_source.load() != 0 || - monitor->robot_mode.load() != - static_cast(RobotModeType::Running)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] torqueOn rejected: safety state changed during startup; the new event remains latched"); - } - if (recovering) { - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - cancelForSafetyTransition(monitor); - if (!enforceControllerTermination(rpc_client, monitor)) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn recovery failed: controller did not reach an empty, steady state after startup"); - } - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - refreshSafetySample(rpc_client, monitor, robot_interface); - const bool robot_running = - monitor->robot_mode.load() == - static_cast(RobotModeType::Running); - if (!recovery_guard->complete( - robot_running, - true, - monitor->cancellation_confirmed.load())) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] torqueOn recovery failed: safety state changed during recovery"); - } - } - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } - emergency_stopped_.store(false); - servo_mode_.store(false); - monitor->hardware_emergency_stop_latched.store(false); - monitor->automatic_recovery_suppressed.store(false); - if (const auto cancelled = cancellation_result()) { - return *cancelled; - } + emergency_stopped_ = false; return Result::success(); } catch (const std::exception& e) { - auto primary_failure = Result::failure( - ArmErrorCode::CommandFailed, - std::string("[AuboArm] torqueOn failed: ") + e.what()); - return controller_mutated - ? aubo_internal::preservePrimaryTorqueOnFailure( - std::move(primary_failure), cancellation_result()) - : primary_failure; - } catch (...) { - auto primary_failure = Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOn failed: unknown exception"); - return controller_mutated - ? aubo_internal::preservePrimaryTorqueOnFailure( - std::move(primary_failure), cancellation_result()) - : primary_failure; + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what()); } } Result AuboArm::torqueOff() { - std::shared_ptr rpc_client; - std::shared_ptr monitor; - { - std::lock_guard lock(mutex_); - const auto ready = ensureConnected_("torqueOff"); - if (!ready.ok()) { - return ready; - } - rpc_client = sdk_->rpc_client; - monitor = sdk_->safety_monitor; - if (monitor) { - monitor->automatic_recovery_suppressed.store(true); - } + const auto ready = ensureConnected_("torqueOff"); + if (!ready.ok()) { + return ready; } try { - std::unique_lock command_rpc_lock; - if (monitor) { - command_rpc_lock = std::unique_lock( - monitor->command_rpc_mutex); + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); } - Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - rpc_client, "torqueOff", interface_result); - if (!interface_result.ok()) { - return interface_result; + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } - const int power_off_ret = - robot_interface->getRobotManage()->poweroff(); - if (power_off_ret != arcs::common_interface::AUBO_OK) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] torqueOff failed: poweroff ret=" + - std::to_string(power_off_ret)); - } - if (!waitForRobotMode( - robot_interface, - arcs::common_interface::RobotModeType::PowerOff)) { + robot_interface->getRobotManage()->poweroff(); + if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::PowerOff)) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOff failed: timeout waiting for PowerOff"); } return Result::success(); @@ -2238,28 +355,7 @@ Result AuboArm::calibrateZeroQ(const std::string& joint_name) Result AuboArm::emergencyStop() { - emergency_stopped_.store(true); - { - std::lock_guard lock(mutex_); - if (sdk_ && sdk_->safety_monitor) { - sdk_->safety_monitor->safety_state->observe( - aubo_internal::SafetyCondition::SoftwareEmergencyStop); - cancelForSafetyTransition(sdk_->safety_monitor); - } - } - return stopMotion(); -} - -Result AuboArm::protectiveStop() -{ - { - std::lock_guard lock(mutex_); - if (sdk_ && sdk_->safety_monitor) { - sdk_->safety_monitor->safety_state->observe( - aubo_internal::SafetyCondition::ProtectiveStop); - cancelForSafetyTransition(sdk_->safety_monitor); - } - } + emergency_stopped_ = true; return stopMotion(); } @@ -2278,135 +374,35 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o if (!validDof_(target.position.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - try { - std::unique_lock submit_lock(mutex_); - const auto locked_ready = ensureConnected_("moveJ"); - if (!locked_ready.ok()) { - return locked_ready; - } - std::uint64_t safety_epoch = 0; - const auto safety_ready = ensureMotionReady_( - "moveJ", safety_epoch); - if (!safety_ready.ok()) { - return safety_ready; - } - const auto rpc_client = sdk_->rpc_client; - const auto motion_state = sdk_->motion_state; - const auto safety_monitor = sdk_->safety_monitor; - const aubo_internal::SafetyPermit safety_permit{safety_epoch}; - const auto motion = motion_state->begin( - aubo_internal::MotionKind::Joint); - if (!motion.started()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] moveJ rejected: " + - std::string(motionStartFailure(motion.status)) + - ", id=" + id_); - } - busy_.store(true); - MotionOwnerGuard motion_owner{ - *motion_state, busy_, motion.token}; + const auto ready = ensureConnected_("moveJ"); + if (!ready.ok()) { + return ready; + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; - const auto robot_names = rpc_client->getRobotNames(); + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); if (robot_names.empty()) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); } - auto robot_interface = rpc_client->getRobotInterface(robot_names.front()); + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); if (!robot_interface) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } - auto motion_control = robot_interface->getMotionControl(); - motion_control->setSpeedFraction(speed_scaling_); - motion_owner.requireExplicitSettlement(); - if (cancellationRequested(options.cancellation_requested) || - !validateSafetyPermit(safety_monitor, safety_permit)) { - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] moveJ cancelled before submission"); - } - const int ret = motion_control->moveJoint( + robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); + robot_interface->getMotionControl()->moveJoint( target.position, options.acceleration > 0.0 ? options.acceleration : 0.5, options.velocity > 0.0 ? options.velocity : 0.5, options.blend_radius, 0); - if (ret == arcs::common_interface::AUBO_OK) { - submit_lock.unlock(); - } else { - motion_owner.settle(); - } - const auto outcome = aubo_internal::resolveMotionCommand( - ret, - arcs::common_interface::AUBO_OK, - arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface, - motion_state, - safety_monitor, - safety_permit, - cancellation_requested = options.cancellation_requested, - token = motion.token]() { - return waitArrival( - robot_interface, - [motion_state, - safety_monitor, - safety_permit, - cancellation_requested, - token]() { - return cancellationRequested( - cancellation_requested) || - motion_state->cancelled(token) || - !validateSafetyPermit( - safety_monitor, safety_permit); - }); - }); - if (cancellationRequested(options.cancellation_requested)) { - if (ret == arcs::common_interface::AUBO_OK) { - if (outcome == - aubo_internal::MotionCommandOutcome::CompletedAfterMotion) { - motion_owner.clearOnFinish(); - } else { - // The request was accepted but its completion is no longer - // owned by this caller. Preserve the typed motion state so - // the cancellation owner can issue stopJoint/stopLine. - motion_owner.retainKind(); - } - } - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] moveJ cancelled by its caller"); - } - if (!validateSafetyPermit(safety_monitor, safety_permit)) { - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] moveJ cancelled by hardware safety event"); - } - switch (outcome) { - case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: - CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret=" - << ret << " (" - << arcs::common_interface::returnValue2Str(ret) << ")"; - return Result::success(); - case aubo_internal::MotionCommandOutcome::CompletedAfterMotion: - motion_owner.clearOnFinish(); - motion_owner.settle(); - return Result::success(); - case aubo_internal::MotionCommandOutcome::Cancelled: - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] moveJ cancelled by stopMotion or hardware safety event"); - case aubo_internal::MotionCommandOutcome::SubmitFailed: - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] moveJ failed: sdk ret=" + std::to_string(ret) + - " (" + arcs::common_interface::returnValue2Str(ret) + ")"); - case aubo_internal::MotionCommandOutcome::CompletionFailed: + if (waitArrival(robot_interface) != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ did not complete"); } - return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ failed: unknown outcome"); + return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveJ failed: ") + e.what()); } @@ -2418,38 +414,18 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration if (!validDof_(velocity.velocity.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - try { - std::unique_lock submit_lock(mutex_); - const auto locked_ready = ensureConnected_("speedJ"); - if (!locked_ready.ok()) { - return locked_ready; - } - std::uint64_t safety_epoch = 0; - const auto safety_ready = ensureMotionReady_( - "speedJ", safety_epoch); - if (!safety_ready.ok()) { - return safety_ready; - } - const auto rpc_client = sdk_->rpc_client; - const auto motion_state = sdk_->motion_state; - const auto safety_monitor = sdk_->safety_monitor; - const aubo_internal::SafetyPermit safety_permit{safety_epoch}; - const auto motion = motion_state->begin( - aubo_internal::MotionKind::Joint, true); - if (!motion.started()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] speedJ rejected: " + - std::string(motionStartFailure(motion.status)) + - ", id=" + id_); - } - busy_.store(true); - MotionOwnerGuard motion_owner{ - *motion_state, busy_, motion.token}; + const auto ready = ensureConnected_("speedJ"); + if (!ready.ok()) { + return ready; + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; + try { Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - rpc_client, "speedJ", interface_result); + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedJ", interface_result); if (!interface_result.ok()) { return interface_result; } @@ -2457,33 +433,14 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.5; const double resolved_duration = duration > 0.0 ? duration : 100.0; - motion_owner.requireExplicitSettlement(); - submit_lock.unlock(); - if (motion_state->cancelled(motion.token) || - !validateSafetyPermit(safety_monitor, safety_permit)) { - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] speedJ cancelled before submission"); - } const int ret = robot_interface->getMotionControl()->speedJoint( velocity.velocity, resolved_acceleration, resolved_duration); - if (motion_state->cancelled(motion.token) || - !validateSafetyPermit(safety_monitor, safety_permit)) { - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] speedJ cancelled by stopMotion or hardware safety event"); - } if (ret != 0) { - motion_owner.settle(); return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] speedJ failed: ret=" + std::to_string(ret)); } - motion_owner.retainKind(); - motion_owner.settle(); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedJ failed: ") + e.what()); @@ -2492,143 +449,63 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration Result AuboArm::stopJ(double acceleration) { - return stopMotion_(MotionStopKind::Joint, acceleration); + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return Result::success(); + } + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopJ", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + const double resolved_acceleration = acceleration > 0.0 ? acceleration : 31.0; + const int ret = robot_interface->getMotionControl()->stopJoint(resolved_acceleration); + busy_.store(false); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] stopJ failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopJ failed: ") + e.what()); + } } Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame) { (void)frame; - try { - std::unique_lock submit_lock(mutex_); - const auto locked_ready = ensureConnected_("moveL"); - if (!locked_ready.ok()) { - return locked_ready; - } - std::uint64_t safety_epoch = 0; - const auto safety_ready = ensureMotionReady_( - "moveL", safety_epoch); - if (!safety_ready.ok()) { - return safety_ready; - } - const auto rpc_client = sdk_->rpc_client; - const auto motion_state = sdk_->motion_state; - const auto safety_monitor = sdk_->safety_monitor; - const aubo_internal::SafetyPermit safety_permit{safety_epoch}; - const auto motion = motion_state->begin( - aubo_internal::MotionKind::Linear); - if (!motion.started()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] moveL rejected: " + - std::string(motionStartFailure(motion.status)) + - ", id=" + id_); - } - busy_.store(true); - MotionOwnerGuard motion_owner{ - *motion_state, busy_, motion.token}; + const auto ready = ensureConnected_("moveL"); + if (!ready.ok()) { + return ready; + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; - const auto robot_names = rpc_client->getRobotNames(); + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); if (robot_names.empty()) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); } - auto robot_interface = rpc_client->getRobotInterface(robot_names.front()); + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); if (!robot_interface) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } - auto motion_control = robot_interface->getMotionControl(); - motion_control->setSpeedFraction(speed_scaling_); + robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); std::vector tcp_offset(6, 0.0); robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; - motion_owner.requireExplicitSettlement(); - if (cancellationRequested(options.cancellation_requested) || - !validateSafetyPermit(safety_monitor, safety_permit)) { - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] moveL cancelled before submission"); - } - const int ret = motion_control->moveLine( + robot_interface->getMotionControl()->moveLine( pose, options.acceleration > 0.0 ? options.acceleration : 0.5, options.velocity > 0.0 ? options.velocity : 0.25, options.blend_radius, 0); - if (ret == arcs::common_interface::AUBO_OK) { - submit_lock.unlock(); - } else { - motion_owner.settle(); - } - const auto outcome = aubo_internal::resolveMotionCommand( - ret, - arcs::common_interface::AUBO_OK, - arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface, - motion_state, - safety_monitor, - safety_permit, - cancellation_requested = options.cancellation_requested, - token = motion.token]() { - return waitArrival( - robot_interface, - [motion_state, - safety_monitor, - safety_permit, - cancellation_requested, - token]() { - return cancellationRequested( - cancellation_requested) || - motion_state->cancelled(token) || - !validateSafetyPermit( - safety_monitor, safety_permit); - }); - }); - if (cancellationRequested(options.cancellation_requested)) { - if (ret == arcs::common_interface::AUBO_OK) { - if (outcome == - aubo_internal::MotionCommandOutcome::CompletedAfterMotion) { - motion_owner.clearOnFinish(); - } else { - // Keep the accepted linear kind until a typed Stop confirms - // that the controller is idle. - motion_owner.retainKind(); - } - } - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] moveL cancelled by its caller"); - } - if (!validateSafetyPermit(safety_monitor, safety_permit)) { - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] moveL cancelled by hardware safety event"); - } - switch (outcome) { - case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: - CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret=" - << ret << " (" - << arcs::common_interface::returnValue2Str(ret) << ")"; - return Result::success(); - case aubo_internal::MotionCommandOutcome::CompletedAfterMotion: - motion_owner.clearOnFinish(); - motion_owner.settle(); - return Result::success(); - case aubo_internal::MotionCommandOutcome::Cancelled: - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] moveL cancelled by stopMotion or hardware safety event"); - case aubo_internal::MotionCommandOutcome::SubmitFailed: - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] moveL failed: sdk ret=" + std::to_string(ret) + - " (" + arcs::common_interface::returnValue2Str(ret) + ")"); - case aubo_internal::MotionCommandOutcome::CompletionFailed: + if (waitArrival(robot_interface) != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL did not complete"); } - return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL failed: unknown outcome"); + return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveL failed: ") + e.what()); } @@ -2636,38 +513,18 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame) { - try { - std::unique_lock submit_lock(mutex_); - const auto locked_ready = ensureConnected_("speedL"); - if (!locked_ready.ok()) { - return locked_ready; - } - std::uint64_t safety_epoch = 0; - const auto safety_ready = ensureMotionReady_( - "speedL", safety_epoch); - if (!safety_ready.ok()) { - return safety_ready; - } - const auto rpc_client = sdk_->rpc_client; - const auto motion_state = sdk_->motion_state; - const auto safety_monitor = sdk_->safety_monitor; - const aubo_internal::SafetyPermit safety_permit{safety_epoch}; - const auto motion = motion_state->begin( - aubo_internal::MotionKind::Linear, true); - if (!motion.started()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] speedL rejected: " + - std::string(motionStartFailure(motion.status)) + - ", id=" + id_); - } - busy_.store(true); - MotionOwnerGuard motion_owner{ - *motion_state, busy_, motion.token}; + const auto ready = ensureConnected_("speedL"); + if (!ready.ok()) { + return ready; + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; + try { Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - rpc_client, "speedL", interface_result); + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedL", interface_result); if (!interface_result.ok()) { return interface_result; } @@ -2687,10 +544,8 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d tool_frame[0] = 0.0; tool_frame[1] = 0.0; tool_frame[2] = 0.0; - line_speed = rpc_client->getMath()->poseTrans( - tool_frame, line_speed); - angular_speed = rpc_client->getMath()->poseTrans( - tool_frame, angular_speed); + line_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, line_speed); + angular_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, angular_speed); } else if (frame == FrameType::User) { return Result::failure(ArmErrorCode::UnsupportedCommand, "[AuboArm] speedL User frame requires a configured user coordinate frame"); @@ -2707,33 +562,14 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.2; const double resolved_duration = duration > 0.0 ? duration : 100.0; - motion_owner.requireExplicitSettlement(); - submit_lock.unlock(); - if (motion_state->cancelled(motion.token) || - !validateSafetyPermit(safety_monitor, safety_permit)) { - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] speedL cancelled before submission"); - } const int ret = robot_interface->getMotionControl()->speedLine( speed, resolved_acceleration, resolved_duration); - if (motion_state->cancelled(motion.token) || - !validateSafetyPermit(safety_monitor, safety_permit)) { - motion_owner.settle(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] speedL cancelled by stopMotion or hardware safety event"); - } if (ret != 0) { - motion_owner.settle(); return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] speedL failed: ret=" + std::to_string(ret)); } - motion_owner.retainKind(); - motion_owner.settle(); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedL failed: ") + e.what()); @@ -2742,191 +578,44 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d Result AuboArm::stopL(std::optional acceleration) { - return stopMotion_( - MotionStopKind::Linear, - acceleration.has_value() ? *acceleration : 0.0); + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return Result::success(); + } + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopL", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + const double resolved_acceleration = + acceleration.has_value() && *acceleration > 0.0 ? *acceleration : 10.0; + const int ret = robot_interface->getMotionControl()->stopLine(resolved_acceleration, resolved_acceleration); + busy_.store(false); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] stopL failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopL failed: ") + e.what()); + } } Result AuboArm::stopMotion() { - return stopMotion_(MotionStopKind::Automatic, 0.0); -} - -Result AuboArm::stopMotion_( - const MotionStopKind requested_kind, - const double acceleration) -{ - if (!connected_.load()) { + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { return Result::success(); } - try { - std::unique_lock submit_lock(mutex_); - if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { return Result::success(); } - - const auto motion_state = sdk_->motion_state; - const auto monitor = sdk_->safety_monitor; - std::unique_lock command_rpc_lock; - if (monitor) { - monitor->automatic_recovery_suppressed.store(true); - command_rpc_lock = std::unique_lock( - monitor->command_rpc_mutex); - } - aubo_internal::MotionKind forced_kind = - aubo_internal::MotionKind::None; - if (requested_kind == MotionStopKind::Joint) { - forced_kind = aubo_internal::MotionKind::Joint; - } else if (requested_kind == MotionStopKind::Linear) { - forced_kind = aubo_internal::MotionKind::Linear; - } - - const auto stop_request = - motion_state->beginStop(forced_kind); - if (!stop_request.started()) { - busy_.store(true); - constexpr auto kExistingStopTimeout = - std::chrono::seconds(6); - const auto existing_result = - motion_state->waitForStopCompletion( - stop_request, - std::chrono::duration_cast( - kExistingStopTimeout)); - busy_.store(motion_state->busy()); - if (existing_result == - aubo_internal::StopWaitStatus::Completed) { - return Result::success(); - } - if (existing_result == - aubo_internal::StopWaitStatus::Timeout) { - return Result::failure( - ArmErrorCode::Timeout, - "[AuboArm] stopMotion failed: timeout waiting for the existing stop operation to complete"); - } - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] stopMotion failed: the existing stop operation could not confirm controller idle"); - } - busy_.store(true); - StopStateGuard stop_state_guard{ - *motion_state, busy_}; - - Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - sdk_->rpc_client, "stopMotion", interface_result); - if (!interface_result.ok()) { - return interface_result; - } - - auto motion_control = robot_interface->getMotionControl(); - auto robot_state = robot_interface->getRobotState(); - int last_exec_id = motion_control->getExecId(); - bool last_steady = robot_state->isSteady(); - const bool requires_vendor_stop = - stop_request.tracked_motion || - last_exec_id != -1 || - !last_steady; - - if (requires_vendor_stop && - stop_request.kind == aubo_internal::MotionKind::None) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] stopMotion failed: controller is moving but the " - "active direct-motion type is unknown"); - } - - const auto issue_vendor_stop = [&]() -> Result { - int ret = 0; - if (stop_request.kind == aubo_internal::MotionKind::Joint) { - const double resolved_acceleration = - acceleration > 0.0 ? acceleration : 31.0; - ret = motion_control->stopJoint(resolved_acceleration); - } else { - const double resolved_acceleration = - acceleration > 0.0 ? acceleration : 10.0; - ret = motion_control->stopLine( - resolved_acceleration, resolved_acceleration); - } - if (ret != arcs::common_interface::AUBO_OK) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] stopMotion failed: " + - std::string(motionKindName(stop_request.kind)) + - " stop sdk ret=" + std::to_string(ret) + - " (" + - arcs::common_interface::returnValue2Str(ret) + - ")"); - } - return Result::success(); - }; - - if (requires_vendor_stop) { - const auto stop_result = issue_vendor_stop(); - if (!stop_result.ok()) { - return stop_result; - } - } - - constexpr auto kStopTimeout = std::chrono::seconds(5); - constexpr auto kPollInterval = std::chrono::milliseconds(50); - constexpr int kStableSamples = 3; - const auto deadline = - std::chrono::steady_clock::now() + kStopTimeout; - int stable_samples = 0; - bool idle_since_stop = last_exec_id == -1 && last_steady; - bool owner_active = - motion_state->ownerActive(stop_request.active_token); - while (std::chrono::steady_clock::now() < deadline) { - last_exec_id = motion_control->getExecId(); - last_steady = robot_state->isSteady(); - owner_active = - motion_state->ownerActive(stop_request.active_token); - const bool physically_idle = - last_exec_id == -1 && last_steady; - if (!physically_idle && idle_since_stop) { - if (stop_request.kind == - aubo_internal::MotionKind::None) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] stopMotion failed: motion started " - "after an idle observation but its type is unknown"); - } - const auto stop_result = issue_vendor_stop(); - if (!stop_result.ok()) { - return stop_result; - } - idle_since_stop = false; - } - if (physically_idle && !owner_active) { - if (++stable_samples >= kStableSamples) { - break; - } - } else { - stable_samples = 0; - } - if (physically_idle) { - idle_since_stop = true; - } - std::this_thread::sleep_for(kPollInterval); - } - if (stable_samples < kStableSamples) { - return Result::failure( - ArmErrorCode::Timeout, - "[AuboArm] stopMotion failed: timeout waiting for " + - std::string(motionKindName(stop_request.kind)) + - " motion to stop, generation=" + - std::to_string(stop_request.active_token.generation) + - ", exec_id=" + std::to_string(last_exec_id) + - ", steady=" + (last_steady ? "true" : "false") + - ", owner_active=" + - (owner_active ? "true" : "false")); - } - if (!stop_state_guard.complete()) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] stopMotion failed: cancelled motion handler is still active"); + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (robot_interface) { + robot_interface->getMotionControl()->stopMove(true, true); } + busy_.store(false); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what()); @@ -2939,14 +628,6 @@ Result AuboArm::startServoMode(const ServoOptions& options) if (!ready.ok()) { return ready; } - std::uint64_t safety_epoch = 0; - const auto safety_ready = ensureMotionReady_( - "startServoMode", safety_epoch); - if (!safety_ready.ok()) { - return safety_ready; - } - const auto safety_monitor = sdk_->safety_monitor; - const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { Result interface_result; @@ -2954,11 +635,6 @@ Result AuboArm::startServoMode(const ServoOptions& options) if (!interface_result.ok()) { return interface_result; } - if (!validateSafetyPermit(safety_monitor, safety_permit)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] startServoMode cancelled by hardware safety"); - } const int ret = robot_interface->getMotionControl()->setServoModeSelect(kAuboServoMode); if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, @@ -2968,15 +644,8 @@ Result AuboArm::startServoMode(const ServoOptions& options) return Result::failure(ArmErrorCode::Timeout, "[AuboArm] startServoMode failed: timeout waiting for servo mode"); } - if (!validateSafetyPermit(safety_monitor, safety_permit)) { - (void)robot_interface->getMotionControl()->setServoModeSelect(0); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] startServoMode cancelled by hardware safety event"); - } servo_options_ = options; servo_mode_.store(true); - safety_monitor->servo_mode_select.store(kAuboServoMode); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -2994,13 +663,6 @@ Result AuboArm::servoJ(const JointPositionCommand& target) if (!ready.ok()) { return ready; } - std::uint64_t safety_epoch = 0; - const auto safety_ready = ensureMotionReady_("servoJ", safety_epoch); - if (!safety_ready.ok()) { - return safety_ready; - } - const auto safety_monitor = sdk_->safety_monitor; - const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { Result interface_result; @@ -3015,11 +677,6 @@ Result AuboArm::servoJ(const JointPositionCommand& target) } } const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008; - if (!validateSafetyPermit(safety_monitor, safety_permit)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] servoJ cancelled by hardware safety before submission"); - } const int ret = robot_interface->getMotionControl()->servoJoint( target.position, 0.0, @@ -3031,11 +688,6 @@ Result AuboArm::servoJ(const JointPositionCommand& target) return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] servoJ failed: ret=" + std::to_string(ret)); } - if (!validateSafetyPermit(safety_monitor, safety_permit)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] servoJ cancelled by hardware safety event"); - } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoJ failed: ") + e.what()); @@ -3048,13 +700,6 @@ Result AuboArm::servoL(const CartesianPose& target, FrameType frame) if (!ready.ok()) { return ready; } - std::uint64_t safety_epoch = 0; - const auto safety_ready = ensureMotionReady_("servoL", safety_epoch); - if (!safety_ready.ok()) { - return safety_ready; - } - const auto safety_monitor = sdk_->safety_monitor; - const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { Result interface_result; @@ -3083,11 +728,6 @@ Result AuboArm::servoL(const CartesianPose& target, FrameType frame) } const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008; - if (!validateSafetyPermit(safety_monitor, safety_permit)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] servoL cancelled by hardware safety before submission"); - } const int ret = robot_interface->getMotionControl()->servoCartesian( pose, 0.0, @@ -3099,11 +739,6 @@ Result AuboArm::servoL(const CartesianPose& target, FrameType frame) return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] servoL failed: ret=" + std::to_string(ret)); } - if (!validateSafetyPermit(safety_monitor, safety_permit)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] servoL cancelled by hardware safety event"); - } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoL failed: ") + e.what()); @@ -3136,9 +771,6 @@ Result AuboArm::stopServoMode() } const int ret = robot_interface->getMotionControl()->setServoModeSelect(0); servo_mode_.store(false); - if (sdk_->safety_monitor) { - sdk_->safety_monitor->servo_mode_select.store(0); - } if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] stopServoMode failed: ret=" + std::to_string(ret)); @@ -3156,7 +788,6 @@ Result AuboArm::stopServoMode() Result AuboArm::connect(const std::string& ip, const int port) { - std::lock_guard lock(mutex_); if (connected_.load()) { return Result::success(); } @@ -3167,7 +798,13 @@ Result AuboArm::connect(const std::string& ip, const int port) try { const int resolved_port = port > 0 ? port : 30004; auto sdk_state = std::make_unique(); - sdk_state->rpc_client = makeRpcClient(); + sdk_state->rpc_client = std::shared_ptr( + ::createRpcClient(), + [](arcs::aubo_sdk::RpcClient* client) { + if (client) { + ::destroyRpcClient(client); + } + }); if (!sdk_state->rpc_client) { return Result::failure(ArmErrorCode::ConnectionFailed, "[AuboArm] connect failed: create RPC client failed"); @@ -3203,46 +840,8 @@ Result AuboArm::connect(const std::string& ip, const int port) "[AuboArm] connect failed: robot name list is empty"); } - auto robot_interface = sdk_state->rpc_client->getRobotInterface( - robot_names.front()); - if (!robot_interface) { - sdk_state->rpc_client->logout(); - sdk_state->rpc_client->disconnect(); - return Result::failure(ArmErrorCode::ConnectionFailed, - "[AuboArm] connect failed: robot interface is null"); - } - - sdk_state->safety_monitor = - std::make_shared(); - sdk_state->safety_monitor->motion_state = - sdk_state->motion_state; - sdk_state->safety_monitor->arm_id = id_; - sdk_state->safety_monitor - ->auto_power_on_after_hardware_estop_release = - vendor_cfg_.auto_power_on_after_hardware_estop_release(); - sdk_state->safety_monitor->on_hardware_estop_auto_recovered = - [this] { - busy_.store(false); - servo_mode_.store(false); - }; - publishSafetySample( - sdk_state->safety_monitor, - robot_interface->getRobotState()->getSafetyModeType(), - robot_interface->getRobotState()->getRobotModeType(), - sdk_state->rpc_client->getRuntimeMachine()->getRuntimeState(), - robot_interface->getRobotConfig() - ->getRobotEmergencyStopSource(), - robot_interface->getMotionControl()->getServoModeSelect()); - ip_ = ip; port_ = resolved_port; - sdk_state->safety_monitor_thread = std::thread( - runSafetyMonitor, - sdk_state->safety_monitor, - ip_, - port_, - username_, - password_); sdk_ = std::move(sdk_state); connected_.store(true); return Result::success(); @@ -3256,40 +855,22 @@ Result AuboArm::connect(const std::string& ip, const int port) Result AuboArm::disconnect() { - std::lock_guard lock(mutex_); try { - connected_.store(false); - if (sdk_ && sdk_->safety_monitor) { - sdk_->safety_monitor->stop_requested.store(true); - sdk_->safety_monitor->wait_cv.notify_all(); - } - if (sdk_ && sdk_->safety_monitor_thread.joinable()) { - sdk_->safety_monitor_thread.join(); - } - const auto close_command_rpc = [this]() { - if (sdk_ && sdk_->rpc_client) { - if (sdk_->rpc_client->hasLogined()) { - sdk_->rpc_client->logout(); - } - if (sdk_->rpc_client->hasConnected()) { - sdk_->rpc_client->disconnect(); - } + if (sdk_ && sdk_->rpc_client) { + if (sdk_->rpc_client->hasLogined()) { + sdk_->rpc_client->logout(); + } + if (sdk_->rpc_client->hasConnected()) { + sdk_->rpc_client->disconnect(); } - }; - if (sdk_ && sdk_->safety_monitor) { - std::unique_lock command_rpc_lock( - sdk_->safety_monitor->command_rpc_mutex); - close_command_rpc(); - } else { - close_command_rpc(); } } catch (const std::exception& e) { CMVR_LOG(ERROR) << "[AuboArm] disconnect failed: " << e.what(); } sdk_.reset(); + connected_.store(false); busy_.store(false); servo_mode_.store(false); - emergency_stopped_.store(false); return Result::success(); } @@ -3299,234 +880,6 @@ Result AuboArm::shutdown() return disconnect(); } -Result AuboArm::clearFault() -{ - std::shared_ptr rpc_client; - std::shared_ptr monitor; - { - std::lock_guard lock(mutex_); - const auto ready = ensureConnected_("clearFault"); - if (!ready.ok()) { - return ready; - } - rpc_client = sdk_->rpc_client; - monitor = sdk_->safety_monitor; - } - - try { - if (!monitor) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] clearFault failed: hardware safety monitor is unavailable"); - } - std::unique_lock command_rpc_lock( - monitor->command_rpc_mutex); - Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - rpc_client, "clearFault", interface_result); - if (!interface_result.ok()) { - return interface_result; - } - refreshSafetySample(rpc_client, monitor, robot_interface); - auto snapshot = monitor->safety_state->snapshot(); - const std::uint64_t expected_safety_epoch = snapshot.epoch; - if (!snapshot.latched && - aubo_internal::isMotionSafe(snapshot.observed) && - monitor->robot_mode.load() != - static_cast(RobotModeType::Error)) { - return Result::success(); - } - if (!snapshot.latched && - monitor->robot_mode.load() == - static_cast(RobotModeType::Error)) { - return Result::failure( - ArmErrorCode::RobotInFault, - "[AuboArm] clearFault rejected: RobotMode is Error even though the safety mode is Normal/Reduced"); - } - if (monitor->emergency_stop_source.load() != 0) { - return Result::failure( - ArmErrorCode::RobotInEmergencyStop, - "[AuboArm] clearFault rejected: hardware emergency-stop input is still active"); - } - - if (aubo_internal::needsProtectiveUnlock(snapshot.observed) || - aubo_internal::needsProtectiveUnlock( - snapshot.latched_reason)) { - command_rpc_lock.unlock(); - return unlockProtectiveStop_(expected_safety_epoch); - } - - if (aubo_internal::isMotionSafe(snapshot.observed)) { - if (monitor->robot_mode.load() == - static_cast(RobotModeType::Running)) { - command_rpc_lock.unlock(); - return completeSafetyRecovery_( - "clearFault", expected_safety_epoch); - } - return Result::failure( - ArmErrorCode::RobotNotPowered, - "[AuboArm] safety condition is clear, but torqueOn is required to verify the old queue and complete recovery"); - } - - if (!aubo_internal::needsInterfaceBoardRestart( - snapshot.observed)) { - const std::string guidance = - snapshot.observed == - aubo_internal::SafetyCondition::SafeguardStop - ? "clear the external safety IO" - : (snapshot.observed == - aubo_internal::SafetyCondition::Recovery - ? "manually move the arm inside its safety limits" - : "restore a valid controller safety state"); - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] clearFault rejected for " + - std::string(safetyConditionName(snapshot.observed)) + - ": " + guidance); - } - - const int ret = - robot_interface->getRobotManage()->restartInterfaceBoard(); - if (ret != arcs::common_interface::AUBO_OK) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] clearFault failed: restartInterfaceBoard ret=" + - std::to_string(ret)); - } - - const auto deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(10); - while (std::chrono::steady_clock::now() < deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - refreshSafetySample(rpc_client, monitor, robot_interface); - snapshot = monitor->safety_state->snapshot(); - if (aubo_internal::isMotionSafe(snapshot.observed)) { - break; - } - } - if (!aubo_internal::isMotionSafe(snapshot.observed)) { - return Result::failure( - ArmErrorCode::Timeout, - "[AuboArm] clearFault failed: timeout waiting for a safe controller state"); - } - if (monitor->robot_mode.load() == - static_cast(RobotModeType::Running)) { - command_rpc_lock.unlock(); - return completeSafetyRecovery_( - "clearFault", expected_safety_epoch); - } - return Result::failure( - ArmErrorCode::RobotNotPowered, - "[AuboArm] controller fault was reset, but the safety latch remains until torqueOn verifies an empty queue in Running mode"); - } catch (const std::exception& e) { - return Result::failure( - ArmErrorCode::CommandFailed, - std::string("[AuboArm] clearFault failed: ") + e.what()); - } -} - -Result AuboArm::unlockProtectiveStop() -{ - return unlockProtectiveStop_(std::nullopt); -} - -Result AuboArm::unlockProtectiveStop_( - const std::optional expected_safety_epoch) -{ - std::shared_ptr rpc_client; - std::shared_ptr monitor; - { - std::lock_guard lock(mutex_); - const auto ready = ensureConnected_("unlockProtectiveStop"); - if (!ready.ok()) { - return ready; - } - rpc_client = sdk_->rpc_client; - monitor = sdk_->safety_monitor; - } - - try { - if (!monitor) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] unlockProtectiveStop failed: hardware safety monitor is unavailable"); - } - std::unique_lock command_rpc_lock( - monitor->command_rpc_mutex); - Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - rpc_client, "unlockProtectiveStop", interface_result); - if (!interface_result.ok()) { - return interface_result; - } - refreshSafetySample(rpc_client, monitor, robot_interface); - auto snapshot = monitor->safety_state->snapshot(); - const std::uint64_t recovery_epoch = - expected_safety_epoch.value_or(snapshot.epoch); - if (snapshot.epoch != recovery_epoch) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] unlockProtectiveStop rejected: a newer safety event superseded this recovery request"); - } - if (!snapshot.latched && - aubo_internal::isMotionSafe(snapshot.observed)) { - return Result::success(); - } - if (monitor->emergency_stop_source.load() != 0) { - return Result::failure( - ArmErrorCode::RobotInEmergencyStop, - "[AuboArm] unlockProtectiveStop rejected: hardware emergency-stop input is active"); - } - - if (aubo_internal::needsProtectiveUnlock( - snapshot.observed)) { - const int ret = robot_interface->getRobotManage() - ->setUnlockProtectiveStop(); - if (ret != arcs::common_interface::AUBO_OK) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] unlockProtectiveStop failed: sdk ret=" + - std::to_string(ret)); - } - } else if (!aubo_internal::isMotionSafe(snapshot.observed)) { - return Result::failure( - ArmErrorCode::RobotInProtectiveStop, - "[AuboArm] unlockProtectiveStop rejected: current safety state is " + - std::string(safetyConditionName(snapshot.observed))); - } - - const auto deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(5); - while (std::chrono::steady_clock::now() < deadline) { - refreshSafetySample(rpc_client, monitor, robot_interface); - snapshot = monitor->safety_state->snapshot(); - if (aubo_internal::isMotionSafe(snapshot.observed)) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - if (!aubo_internal::isMotionSafe(snapshot.observed)) { - return Result::failure( - ArmErrorCode::Timeout, - "[AuboArm] unlockProtectiveStop failed: safety mode did not return to Normal/Reduced"); - } - if (monitor->robot_mode.load() != - static_cast(RobotModeType::Running)) { - return Result::failure( - ArmErrorCode::RobotNotPowered, - "[AuboArm] protective stop was unlocked, but torqueOn is required to complete safety recovery"); - } - command_rpc_lock.unlock(); - return completeSafetyRecovery_( - "unlockProtectiveStop", recovery_epoch); - } catch (const std::exception& e) { - return Result::failure( - ArmErrorCode::CommandFailed, - std::string("[AuboArm] unlockProtectiveStop failed: ") + - e.what()); - } -} - Result AuboArm::loadProgram(const std::string& program_name) { if (program_name.empty()) { @@ -3555,33 +908,12 @@ Result AuboArm::playProgram() if (!ready.ok()) { return ready; } - std::uint64_t safety_epoch = 0; - const auto safety_ready = ensureMotionReady_( - "playProgram", safety_epoch); - if (!safety_ready.ok()) { - return safety_ready; - } - const auto safety_monitor = sdk_->safety_monitor; - const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { - if (!validateSafetyPermit(safety_monitor, safety_permit)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] playProgram cancelled by hardware safety before submission"); - } const int ret = sdk_->rpc_client->getRuntimeMachine()->runProgram(); if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] playProgram failed: ret=" + std::to_string(ret)); } - safety_monitor->runtime_state.store( - static_cast(RuntimeState::Running)); - if (!validateSafetyPermit(safety_monitor, safety_permit)) { - (void)sdk_->rpc_client->getRuntimeMachine()->abort(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] playProgram cancelled by hardware safety event"); - } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -3601,10 +933,6 @@ Result AuboArm::pauseProgram() return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] pauseProgram failed: ret=" + std::to_string(ret)); } - if (sdk_->safety_monitor) { - sdk_->safety_monitor->runtime_state.store( - static_cast(RuntimeState::Paused)); - } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -3620,15 +948,11 @@ Result AuboArm::stopProgram() } try { const int ret = sdk_->rpc_client->getRuntimeMachine()->abort(); + busy_.store(false); if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] stopProgram failed: ret=" + std::to_string(ret)); } - if (sdk_->safety_monitor) { - sdk_->safety_monitor->runtime_state.store( - static_cast(RuntimeState::Stopped)); - sdk_->safety_monitor->runtime_abort_required.store(false); - } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -3725,119 +1049,6 @@ bool AuboArm::validDof_(const std::size_t size, std::string& error) const return true; } -Result AuboArm::completeSafetyRecovery_( - const std::string& context, - const std::uint64_t expected_safety_epoch) -{ - std::shared_ptr rpc_client; - std::shared_ptr monitor; - { - std::lock_guard lock(mutex_); - const auto ready = ensureConnected_(context); - if (!ready.ok()) { - return ready; - } - rpc_client = sdk_->rpc_client; - monitor = sdk_->safety_monitor; - } - - try { - if (!monitor) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] " + context + - " failed: hardware safety monitor is unavailable"); - } - std::unique_lock command_rpc_lock( - monitor->command_rpc_mutex); - Result interface_result; - auto robot_interface = getPrimaryRobotInterface( - rpc_client, context, interface_result); - if (!interface_result.ok()) { - return interface_result; - } - refreshSafetySample(rpc_client, monitor, robot_interface); - if (monitor->emergency_stop_source.load() != 0) { - return Result::failure( - ArmErrorCode::RobotInEmergencyStop, - "[AuboArm] " + context + - " rejected: hardware emergency-stop input is active"); - } - - const auto snapshot = monitor->safety_state->snapshot(); - if (snapshot.epoch != expected_safety_epoch) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] " + context + - " rejected: a newer safety event superseded this recovery request"); - } - if (!snapshot.latched) { - return aubo_internal::isMotionSafe(snapshot.observed) - ? Result::success() - : Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] " + context + - " failed: safety state is " + - safetyConditionName(snapshot.observed)); - } - if (!aubo_internal::isMotionSafe(snapshot.observed)) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] " + context + - " rejected: hardware safety state is " + - safetyConditionName(snapshot.observed)); - } - if (monitor->robot_mode.load() != - static_cast(RobotModeType::Running)) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] " + context + - " rejected: robot must be Running before the safety latch can be cleared"); - } - - const auto recovery_token = - monitor->safety_state->beginRecovery( - expected_safety_epoch); - if (!recovery_token.has_value()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] " + context + - " rejected: another recovery is active or the safety state changed"); - } - SafetyRecoveryGuard recovery_guard{ - monitor->safety_state, *recovery_token}; - cancelForSafetyTransition(monitor); - if (!enforceControllerTermination(rpc_client, monitor)) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] " + context + - " failed: old motion/program could not be terminated and verified"); - } - - refreshSafetySample(rpc_client, monitor, robot_interface); - const bool robot_running = - monitor->robot_mode.load() == - static_cast(RobotModeType::Running); - if (!recovery_guard.complete( - robot_running, - true, - monitor->cancellation_confirmed.load())) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[AuboArm] " + context + - " failed: safety state changed while recovery was being verified"); - } - emergency_stopped_.store(false); - servo_mode_.store(false); - return Result::success(); - } catch (const std::exception& e) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[AuboArm] " + context + - " failed during safety recovery: " + e.what()); - } -} - Result AuboArm::ensureConnected_(const std::string& context) const { if (!connected_.load()) { @@ -3851,98 +1062,4 @@ Result AuboArm::ensureConnected_(const std::string& context) const return Result::success(); } -Result AuboArm::ensureMotionReady_( - const std::string& context, - std::uint64_t& safety_epoch) const -{ - const auto connected = ensureConnected_(context); - if (!connected.ok()) { - return connected; - } - if (!sdk_->safety_monitor) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] " + context + - " rejected: hardware safety monitor is unavailable"); - } - - const auto monitor = sdk_->safety_monitor; - if (!safetySampleFresh(monitor)) { - publishSafetyUnavailable(monitor, "sample is stale"); - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] " + context + - " rejected: hardware safety state is unavailable or stale"); - } - - if (monitor->emergency_stop_source.load() != 0) { - const auto previous = monitor->safety_state->snapshot(); - monitor->safety_state->observe( - aubo_internal::SafetyCondition::RobotEmergencyStop); - if (!previous.latched || - previous.observed != - aubo_internal::SafetyCondition::RobotEmergencyStop) { - cancelForSafetyTransition(monitor); - } - } - - const auto snapshot = monitor->safety_state->snapshot(); - const auto condition = snapshot.latched - ? snapshot.latched_reason - : snapshot.observed; - if (snapshot.latched || - !aubo_internal::isMotionSafe(snapshot.observed)) { - ArmErrorCode code = ArmErrorCode::RobotNotReady; - if (condition == - aubo_internal::SafetyCondition::RobotEmergencyStop || - condition == - aubo_internal::SafetyCondition::SystemEmergencyStop || - condition == - aubo_internal::SafetyCondition::SoftwareEmergencyStop) { - code = ArmErrorCode::RobotInEmergencyStop; - } else if ( - condition == aubo_internal::SafetyCondition::ProtectiveStop || - condition == aubo_internal::SafetyCondition::SafeguardStop) { - code = ArmErrorCode::RobotInProtectiveStop; - } else if ( - condition == aubo_internal::SafetyCondition::Fault || - condition == aubo_internal::SafetyCondition::Violation) { - code = ArmErrorCode::RobotInFault; - } - std::string recovery_instruction = - "; clear the hardware condition and perform explicit recovery"; - if (condition == - aubo_internal::SafetyCondition::RobotEmergencyStop && - monitor->auto_power_on_after_hardware_estop_release && - !monitor->automatic_recovery_suppressed.load()) { - recovery_instruction = - "; release the physical emergency stop and wait for " - "automatic power-on recovery"; - } - return Result::failure( - code, - "[AuboArm] " + context + - " rejected: hardware safety latch is " + - safetyConditionName(condition) + recovery_instruction); - } - - if (monitor->robot_mode.load() != - static_cast(RobotModeType::Running)) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] " + context + - " rejected: robot is not in Running mode"); - } - - const auto permit = monitor->safety_state->tryPermit(); - if (!permit.has_value()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[AuboArm] " + context + - " rejected: no valid hardware safety permit"); - } - safety_epoch = permit->epoch; - return Result::success(); -} - } // namespace cmvr::device diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index f7e9061e..a4d4dd6f 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -2,8 +2,6 @@ #define CMVR_ES_AUBO_ARM_H #include -#include -#include #include #include #include @@ -23,8 +21,6 @@ public: std::string typeName() const override { return "AuboARM"; } bool init() override; bool stop() override; - bool executeJsonCommand(const std::string& request_json, - std::string& response_json) override; RobotModel getRobotModel() const override { return model_; } std::size_t getDof() const override { return model_.dof; } @@ -32,17 +28,14 @@ public: JointGroupState getJointState() const override; CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; - SafetyMode getSafetyMode() const override; - ControlMode getControlMode() const override; - bool supportsActionQueueMotion() const noexcept override { return true; } + SafetyMode getSafetyMode() const override { return SafetyMode::Normal; } + ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; } Result torqueOn() override; - Result torqueOn( - const std::function& cancellation_requested) override; Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; - Result protectiveStop() override; + Result protectiveStop() override { return emergencyStop(); } Result recoverProtectiveStop( const JointTrajectory&, const MotionOptions&) override @@ -53,9 +46,9 @@ public: } Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } - bool isProtectiveStopped() const override; - bool isEmergencyStopped() const override; - bool isFault() const override; + bool isProtectiveStopped() const override { return false; } + bool isEmergencyStopped() const override { return emergency_stopped_; } + bool isFault() const override { return false; } Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override; Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override; @@ -79,8 +72,8 @@ public: Result powerOff() override { return torqueOff(); } Result brakeRelease() override { return torqueOn(); } Result shutdown() override; - Result clearFault() override; - Result unlockProtectiveStop() override; + Result clearFault() override { return Result::success(); } + Result unlockProtectiveStop() override { return Result::success(); } Result loadProgram(const std::string& program_name) override; Result playProgram() override; Result pauseProgram() override; @@ -93,27 +86,16 @@ public: CartesianPose fk(const std::string& base_link, const std::string& ee_link) override; CartesianPose fk(bool is_tcp = true) override; CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; } - bool busy() const override; + bool busy() const override { return busy_.load(); } private: - enum class MotionStopKind { - Automatic, - Joint, - Linear, - }; - Result unsupported_(const std::string& name) const; bool validDof_(std::size_t size, std::string& error) const; Result ensureConnected_(const std::string& context) const; - Result ensureMotionReady_(const std::string& context, - std::uint64_t& safety_epoch) const; - Result completeSafetyRecovery_(const std::string& context, - std::uint64_t expected_safety_epoch); - Result unlockProtectiveStop_( - std::optional expected_safety_epoch); - Result stopMotion_(MotionStopKind kind, double acceleration); +#if defined(CMVR_HAS_AUBO_SDK) struct SdkState; +#endif private: config::RobotArmConfig cfg_; @@ -128,10 +110,12 @@ private: std::atomic connected_{false}; std::atomic busy_{false}; std::atomic servo_mode_{false}; - std::atomic emergency_stopped_{false}; + bool emergency_stopped_{false}; mutable std::mutex mutex_; +#if defined(CMVR_HAS_AUBO_SDK) std::unique_ptr sdk_; +#endif }; } // namespace cmvr::device diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h deleted file mode 100644 index d1c92fb0..00000000 --- a/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef CMVR_ES_AUBO_MOTION_RESULT_H -#define CMVR_ES_AUBO_MOTION_RESULT_H - -namespace cmvr::device::aubo_internal { - -enum class MotionCommandOutcome { - CompletedWithoutMotion, - CompletedAfterMotion, - Cancelled, - SubmitFailed, - CompletionFailed, -}; - -enum class MotionWaitResult { - Completed, - Cancelled, - Failed, -}; - -template -MotionCommandOutcome resolveMotionCommand( - const int return_code, - const int success_code, - const int request_ignore_code, - WaitForCompletion&& wait_for_completion) -{ - if (return_code == request_ignore_code) { - return MotionCommandOutcome::CompletedWithoutMotion; - } - if (return_code != success_code) { - return MotionCommandOutcome::SubmitFailed; - } - const auto wait_result = wait_for_completion(); - if (wait_result == MotionWaitResult::Cancelled) { - return MotionCommandOutcome::Cancelled; - } - if (wait_result != MotionWaitResult::Completed) { - return MotionCommandOutcome::CompletionFailed; - } - return MotionCommandOutcome::CompletedAfterMotion; -} - -} // namespace cmvr::device::aubo_internal - -#endif // CMVR_ES_AUBO_MOTION_RESULT_H diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h deleted file mode 100644 index 84c84367..00000000 --- a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h +++ /dev/null @@ -1,369 +0,0 @@ -#ifndef CMVR_ES_AUBO_MOTION_STATE_H -#define CMVR_ES_AUBO_MOTION_STATE_H - -#include -#include -#include -#include -#include -#include - -namespace cmvr::device::aubo_internal { - -enum class MotionKind { - None, - Joint, - Linear, -}; - -struct MotionToken { - std::uint64_t generation{0}; - MotionKind kind{MotionKind::None}; - - bool valid() const noexcept - { - return generation != 0 && kind != MotionKind::None; - } -}; - -enum class MotionStartStatus { - Started, - Invalid, - Busy, - Stopping, - Blocked, -}; - -struct MotionStartResult { - MotionStartStatus status{MotionStartStatus::Busy}; - MotionToken token; - - bool started() const noexcept - { - return status == MotionStartStatus::Started; - } -}; - -enum class MotionFinishMode { - RestorePrevious, - Clear, - Retain, -}; - -enum class StopStartStatus { - Started, - AlreadyStopping, -}; - -enum class StopWaitStatus { - Completed, - Failed, - Timeout, -}; - -class StopCompletion final { -public: - StopWaitStatus waitFor(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - if (!cv_.wait_for(lock, timeout, [this]() { return completed_; })) { - return StopWaitStatus::Timeout; - } - return succeeded_ - ? StopWaitStatus::Completed - : StopWaitStatus::Failed; - } - -private: - friend class MotionState; - - void finish(const bool succeeded) - { - { - std::lock_guard lock(mutex_); - succeeded_ = succeeded; - completed_ = true; - } - cv_.notify_all(); - } - - std::mutex mutex_; - std::condition_variable cv_; - bool completed_{false}; - bool succeeded_{false}; -}; - -struct StopRequest { - StopStartStatus status{StopStartStatus::AlreadyStopping}; - MotionKind kind{MotionKind::None}; - MotionToken active_token; - bool tracked_motion{false}; - std::shared_ptr completion; - - bool started() const noexcept - { - return status == StopStartStatus::Started; - } -}; - -struct SafetyCancelResult { - MotionKind kind{MotionKind::None}; - MotionToken active_token; - bool tracked_motion{false}; -}; - -// Tracks one direct AUBO motion owner. MoveJ/MoveL submissions are serialized -// through the vendor call. Speed calls release the outer mutex before their -// potentially blocking SDK call, so the generation cancellation below also -// closes the stop-vs-speed-submission race. -class MotionState final { -public: - MotionStartResult begin( - const MotionKind kind, - const bool replace_retained_same_kind = false) - { - std::lock_guard lock(mutex_); - if (kind == MotionKind::None) { - return {MotionStartStatus::Invalid, {}}; - } - if (stop_in_progress_) { - return {MotionStartStatus::Stopping, {}}; - } - if (blocked_) { - return {MotionStartStatus::Blocked, {}}; - } - if (owner_active_) { - return {MotionStartStatus::Busy, {}}; - } - if (last_kind_ != MotionKind::None && - (!replace_retained_same_kind || last_kind_ != kind)) { - return {MotionStartStatus::Busy, {}}; - } - - MotionToken token{++next_generation_, kind}; - owner_active_ = true; - active_token_ = token; - previous_kind_ = last_kind_; - return {MotionStartStatus::Started, token}; - } - - void finish( - const MotionToken& token, - const MotionFinishMode mode = MotionFinishMode::RestorePrevious) - { - std::lock_guard lock(mutex_); - if (!owner_active_ || - active_token_.generation != token.generation) { - return; - } - - owner_active_ = false; - active_token_ = {}; - if (!stop_in_progress_ && !blocked_) { - if (mode == MotionFinishMode::Retain) { - last_kind_ = token.kind; - } else if (mode == MotionFinishMode::Clear) { - last_kind_ = MotionKind::None; - } else { - last_kind_ = previous_kind_; - } - } - previous_kind_ = MotionKind::None; - owner_finished_cv_.notify_all(); - } - - void failMotion(const MotionToken& token) - { - std::lock_guard lock(mutex_); - if (!owner_active_ || - active_token_.generation != token.generation) { - return; - } - owner_active_ = false; - active_token_ = {}; - last_kind_ = token.kind; - previous_kind_ = MotionKind::None; - blocked_ = true; - owner_finished_cv_.notify_all(); - } - - StopRequest beginStop( - const MotionKind requested_kind = MotionKind::None) - { - std::lock_guard lock(mutex_); - if (stop_in_progress_) { - return { - StopStartStatus::AlreadyStopping, - MotionKind::None, - {}, - false, - active_stop_completion_}; - } - - auto completion = std::make_shared(); - stop_in_progress_ = true; - active_stop_completion_ = completion; - const MotionToken active = owner_active_ - ? active_token_ - : MotionToken{}; - // A successful speedJoint/speedLine call may keep the controller in - // velocity mode after the SDK function returns, even when the target - // velocity is zero and the robot currently reports steady. Retain that - // motion kind until a typed stop has been acknowledged. - const bool tracked_motion = - active.valid() || last_kind_ != MotionKind::None; - if (active.valid()) { - cancelled_generation_ = std::max( - cancelled_generation_, active.generation); - } - MotionKind kind = MotionKind::None; - if (active.valid()) { - kind = active.kind; - } else if (last_kind_ != MotionKind::None) { - kind = last_kind_; - } else if (requested_kind != MotionKind::None) { - kind = requested_kind; - } else { - kind = last_kind_; - } - if (kind != MotionKind::None) { - last_kind_ = kind; - } - return { - StopStartStatus::Started, - kind, - active, - tracked_motion, - completion}; - } - - SafetyCancelResult cancelActiveForSafety() - { - std::lock_guard lock(mutex_); - const MotionToken active = owner_active_ - ? active_token_ - : MotionToken{}; - if (active.valid()) { - cancelled_generation_ = std::max( - cancelled_generation_, active.generation); - } - - const MotionKind kind = active.valid() - ? active.kind - : last_kind_; - if (kind != MotionKind::None) { - last_kind_ = kind; - } - // This block is intentionally independent of stop_in_progress_. The - // monitor may observe the safety event while a software Stop owns the - // stop transaction; either way no new motion may enter. - blocked_ = true; - owner_finished_cv_.notify_all(); - return {kind, active, active.valid() || kind != MotionKind::None}; - } - - bool cancelled(const MotionToken& token) const - { - std::lock_guard lock(mutex_); - return token.valid() && - token.generation <= cancelled_generation_; - } - - bool waitForOwnerExit( - const MotionToken& token, - const std::chrono::milliseconds timeout) - { - if (!token.valid()) { - return true; - } - std::unique_lock lock(mutex_); - return owner_finished_cv_.wait_for( - lock, - timeout, - [this, &token]() { - return !owner_active_ || - active_token_.generation != token.generation; - }); - } - - bool ownerActive(const MotionToken& token) const - { - if (!token.valid()) { - return false; - } - std::lock_guard lock(mutex_); - return owner_active_ && - active_token_.generation == token.generation; - } - - StopWaitStatus waitForStopCompletion( - const StopRequest& request, - const std::chrono::milliseconds timeout) const - { - if (!request.completion) { - return StopWaitStatus::Failed; - } - return request.completion->waitFor(timeout); - } - - bool completeStop() - { - std::shared_ptr completion; - { - std::lock_guard lock(mutex_); - if (owner_active_) { - return false; - } - stop_in_progress_ = false; - blocked_ = false; - active_token_ = {}; - last_kind_ = MotionKind::None; - previous_kind_ = MotionKind::None; - completion = std::move(active_stop_completion_); - owner_finished_cv_.notify_all(); - } - if (completion) { - completion->finish(true); - } - return true; - } - - void failStop() - { - std::shared_ptr completion; - { - std::lock_guard lock(mutex_); - stop_in_progress_ = false; - blocked_ = true; - completion = std::move(active_stop_completion_); - owner_finished_cv_.notify_all(); - } - if (completion) { - completion->finish(false); - } - } - - bool busy() const - { - std::lock_guard lock(mutex_); - return owner_active_ || stop_in_progress_ || blocked_ || - last_kind_ != MotionKind::None; - } - -private: - mutable std::mutex mutex_; - std::condition_variable owner_finished_cv_; - std::uint64_t next_generation_{0}; - std::uint64_t cancelled_generation_{0}; - MotionToken active_token_; - MotionKind last_kind_{MotionKind::None}; - MotionKind previous_kind_{MotionKind::None}; - std::shared_ptr active_stop_completion_; - bool owner_active_{false}; - bool stop_in_progress_{false}; - bool blocked_{false}; -}; - -} // namespace cmvr::device::aubo_internal - -#endif // CMVR_ES_AUBO_MOTION_STATE_H diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h deleted file mode 100644 index b11fb94d..00000000 --- a/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h +++ /dev/null @@ -1,240 +0,0 @@ -#ifndef CMVR_ES_AUBO_SAFETY_STATE_H -#define CMVR_ES_AUBO_SAFETY_STATE_H - -#include -#include -#include - -namespace cmvr::device::aubo_internal { - -// This is deliberately richer than RobotArm::SafetyMode. Recovery and -// Violation have no lossless public mapping, but both must remain fail-closed. -enum class SafetyCondition { - Unknown, - Normal, - Reduced, - Recovery, - Violation, - ProtectiveStop, - SafeguardStop, - SystemEmergencyStop, - RobotEmergencyStop, - SoftwareEmergencyStop, - Fault, -}; - -inline bool isMotionSafe(const SafetyCondition condition) noexcept -{ - return condition == SafetyCondition::Normal || - condition == SafetyCondition::Reduced; -} - -inline SafetyCondition effectiveSafetyCondition( - const SafetyCondition reported_condition, - const int robot_emergency_stop_source) noexcept -{ - if (robot_emergency_stop_source < 0) { - return SafetyCondition::Unknown; - } - if (robot_emergency_stop_source != 0) { - return SafetyCondition::RobotEmergencyStop; - } - return reported_condition; -} - -inline bool needsProtectiveUnlock( - const SafetyCondition condition) noexcept -{ - return condition == SafetyCondition::ProtectiveStop || - condition == SafetyCondition::Violation; -} - -inline bool needsInterfaceBoardRestart( - const SafetyCondition condition) noexcept -{ - return condition == SafetyCondition::SystemEmergencyStop || - condition == SafetyCondition::RobotEmergencyStop || - condition == SafetyCondition::Fault; -} - -struct SafetyPermit { - std::uint64_t epoch{0}; - - bool valid() const noexcept { return epoch != 0; } -}; - -struct RecoveryToken { - std::uint64_t epoch{0}; - - bool valid() const noexcept { return epoch != 0; } -}; - -struct SafetySnapshot { - SafetyCondition observed{SafetyCondition::Unknown}; - SafetyCondition latched_reason{SafetyCondition::Unknown}; - std::uint64_t epoch{0}; - bool latched{false}; - bool recovery_in_progress{false}; - bool software_emergency_stop_latched{false}; -}; - -inline bool shouldAutoRecoverHardwareEmergencyStop( - const SafetySnapshot& snapshot, - const bool hardware_emergency_stop_was_observed, - const int current_emergency_stop_source, - const bool auto_power_on_enabled, - const bool automatic_recovery_suppressed) noexcept -{ - return auto_power_on_enabled && !automatic_recovery_suppressed && - hardware_emergency_stop_was_observed && snapshot.latched && - !snapshot.recovery_in_progress && - !snapshot.software_emergency_stop_latched && - snapshot.latched_reason == SafetyCondition::RobotEmergencyStop && - isMotionSafe(snapshot.observed) && - current_emergency_stop_source == 0; -} - -// Hardware safety is an event, not a level. Once an unsafe state has been -// observed, returning to Normal only changes the observed level. A separate, -// explicit recovery must prove that the old controller operation has been -// cancelled before new motion permits can be issued. -class SafetyState final { -public: - SafetyState() = default; - - void observe(const SafetyCondition condition) - { - std::lock_guard lock(mutex_); - const bool changed = observed_ != condition; - observed_ = condition; - if (condition == SafetyCondition::SoftwareEmergencyStop) { - software_emergency_stop_latched_ = true; - } - if (isMotionSafe(condition)) { - return; - } - - if (!latched_ || recovery_in_progress_ || changed) { - ++epoch_; - } - latched_ = true; - recovery_in_progress_ = false; - // A physical E-stop sample can continue arriving after a software - // E-stop request. Keep the software stop independently latched so a - // later physical-input release can never clear it automatically. - latched_reason_ = software_emergency_stop_latched_ - ? SafetyCondition::SoftwareEmergencyStop - : condition; - } - - std::optional tryPermit() const - { - std::lock_guard lock(mutex_); - if (latched_ || !isMotionSafe(observed_)) { - return std::nullopt; - } - return SafetyPermit{epoch_}; - } - - bool validate(const SafetyPermit permit) const - { - std::lock_guard lock(mutex_); - return permit.valid() && permit.epoch == epoch_ && !latched_ && - isMotionSafe(observed_); - } - - std::optional beginRecovery( - const std::uint64_t expected_epoch) - { - std::lock_guard lock(mutex_); - if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ || - recovery_in_progress_ || - !isMotionSafe(observed_)) { - return std::nullopt; - } - recovery_in_progress_ = true; - return RecoveryToken{epoch_}; - } - - bool completeRecovery( - const RecoveryToken token, - const bool robot_running, - const bool controller_idle, - const bool cancellation_confirmed) - { - std::lock_guard lock(mutex_); - if (!token.valid() || token.epoch != epoch_ || !latched_ || - !recovery_in_progress_ || !isMotionSafe(observed_) || - !robot_running || !controller_idle || - !cancellation_confirmed) { - return false; - } - - latched_ = false; - recovery_in_progress_ = false; - latched_reason_ = SafetyCondition::Unknown; - software_emergency_stop_latched_ = false; - ++epoch_; - return true; - } - - // The caller may clear the physical E-stop latch only after it has powered - // the controller, released the brakes, and then re-confirmed an empty, - // steady controller in Running mode. This never authorizes replaying the - // old target, runtime program, or servo session. - bool completeHardwareEmergencyStopRecovery( - const RecoveryToken token, - const bool robot_running, - const bool controller_idle, - const bool cancellation_confirmed) - { - std::lock_guard lock(mutex_); - if (!token.valid() || token.epoch != epoch_ || !latched_ || - !recovery_in_progress_ || - software_emergency_stop_latched_ || - latched_reason_ != SafetyCondition::RobotEmergencyStop || - !isMotionSafe(observed_) || !robot_running || !controller_idle || - !cancellation_confirmed) { - return false; - } - - latched_ = false; - recovery_in_progress_ = false; - latched_reason_ = SafetyCondition::Unknown; - ++epoch_; - return true; - } - - void failRecovery(const RecoveryToken token) - { - std::lock_guard lock(mutex_); - if (token.valid() && token.epoch == epoch_) { - recovery_in_progress_ = false; - } - } - - SafetySnapshot snapshot() const - { - std::lock_guard lock(mutex_); - return { - observed_, - latched_reason_, - epoch_, - latched_, - recovery_in_progress_, - software_emergency_stop_latched_}; - } - -private: - mutable std::mutex mutex_; - SafetyCondition observed_{SafetyCondition::Unknown}; - SafetyCondition latched_reason_{SafetyCondition::Unknown}; - std::uint64_t epoch_{1}; - bool latched_{false}; - bool recovery_in_progress_{false}; - bool software_emergency_stop_latched_{false}; -}; - -} // namespace cmvr::device::aubo_internal - -#endif // CMVR_ES_AUBO_SAFETY_STATE_H diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h b/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h deleted file mode 100644 index 4f2f1365..00000000 --- a/cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef CMVR_ES_AUBO_TORQUE_ON_RESULT_H -#define CMVR_ES_AUBO_TORQUE_ON_RESULT_H - -#include -#include -#include - -#include "common/types/arm/arm_types.h" - -namespace cmvr::device::aubo_internal { - -inline Result preservePrimaryTorqueOnFailure( - Result primary_failure, - const std::optional& cancellation_outcome) -{ - if (!cancellation_outcome.has_value() || - cancellation_outcome->message.empty()) { - return primary_failure; - } - - if (!primary_failure.message.empty()) { - primary_failure.message += "; "; - } - primary_failure.message += - "cancellation handling: " + cancellation_outcome->message; - return primary_failure; -} - -template -std::optional runTorqueOnControllerMutation( - const int success_code, - Mutation&& mutation, - FailureResult&& failure_result, - CancellationOutcome&& cancellation_outcome) -{ - const int return_code = std::forward(mutation)(); - if (return_code != success_code) { - auto primary_failure = - std::forward(failure_result)(return_code); - return preservePrimaryTorqueOnFailure( - std::move(primary_failure), - std::forward(cancellation_outcome)()); - } - return std::forward(cancellation_outcome)(); -} - -} // namespace cmvr::device::aubo_internal - -#endif // CMVR_ES_AUBO_TORQUE_ON_RESULT_H diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_json_command_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_json_command_test.cpp deleted file mode 100644 index d31683f6..00000000 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_json_command_test.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "devices/arm/aubo_arm/aubo_arm.h" - -#include -#include - -namespace { - -#define CHECK_TRUE(condition) \ - do { \ - if (!(condition)) { \ - std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ - << #condition << std::endl; \ - return 1; \ - } \ - } while (false) - -bool contains(const std::string& value, const std::string& expected) -{ - return value.find(expected) != std::string::npos; -} - -cmvr::config::RobotArmConfig makeConfig() -{ - cmvr::config::RobotArmConfig config; - config.set_id("aubo_arm_json_test"); - auto* vendor = config.mutable_vendor(); - vendor->set_brand(cmvr::config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM); - vendor->set_model("AuboTest"); - vendor->set_dof(6); - return config; -} - -} // namespace - -int main() -{ - cmvr::device::AuboArm arm(makeConfig()); - cmvr::device::AbstractDevice* device = &arm; - std::string response; - - CHECK_TRUE(!device->executeJsonCommand("{", response)); - CHECK_TRUE(contains(response, R"("error_code":"invalid_json")")); - - CHECK_TRUE(!device->executeJsonCommand("[]", response)); - CHECK_TRUE(contains(response, R"("error_code":"invalid_json")")); - - CHECK_TRUE(!device->executeJsonCommand( - R"({"command":"ptz","operation":"get_di","index":0})", response)); - CHECK_TRUE(contains(response, R"("error_code":"unsupported_command")")); - - CHECK_TRUE(!device->executeJsonCommand( - R"({"command":"cabinet_io","operation":"get_ai","index":0})", response)); - CHECK_TRUE(contains(response, R"("error_code":"invalid_operation")")); - - CHECK_TRUE(!device->executeJsonCommand( - R"({"command":"cabinet_io","operation":"get_di","index":-1})", response)); - CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")")); - - CHECK_TRUE(!device->executeJsonCommand( - R"({"command":"cabinet_io","operation":"set_do","index":0,"value":1})", - response)); - CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")")); - - CHECK_TRUE(!device->executeJsonCommand( - R"({"command":"cabinet_io","operation":"set_do","index":0})", response)); - CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")")); - - CHECK_TRUE(!device->executeJsonCommand( - R"({"command":"cabinet_io","operation":"get_di","index":0})", response)); - CHECK_TRUE(contains(response, R"("error_code":"not_connected")")); - - CHECK_TRUE(!device->executeJsonCommand( - R"({"command":"cabinet_io","operation":"get_do","index":0})", response)); - CHECK_TRUE(contains(response, R"("operation":"get_do")")); - CHECK_TRUE(contains(response, R"("error_code":"not_connected")")); - - CHECK_TRUE(!device->executeJsonCommand( - R"({"command":"cabinet_io","operation":"set_do","index":0,"value":true})", - response)); - CHECK_TRUE(contains(response, R"("operation":"set_do")")); - CHECK_TRUE(contains(response, R"("error_code":"not_connected")")); - - return 0; -} diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp deleted file mode 100644 index 57bca235..00000000 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp +++ /dev/null @@ -1,158 +0,0 @@ -#include "devices/arm/aubo_arm/aubo_motion_result.h" -#include "devices/arm/aubo_arm/aubo_torque_on_result.h" - -#include -#include -#include -#include - -namespace { - -#define CHECK_TRUE(condition) \ - do { \ - if (!(condition)) { \ - std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ - << #condition << std::endl; \ - return 1; \ - } \ - } while (false) - -} // namespace - -int main() -{ - using cmvr::device::aubo_internal::MotionCommandOutcome; - using cmvr::device::aubo_internal::MotionWaitResult; - using cmvr::device::aubo_internal::preservePrimaryTorqueOnFailure; - using cmvr::device::aubo_internal::resolveMotionCommand; - using cmvr::device::aubo_internal::runTorqueOnControllerMutation; - - constexpr int success_code = 0; - constexpr int request_ignore_code = 13; - - int wait_calls = 0; - const auto wait_succeeded = [&wait_calls]() { - ++wait_calls; - return MotionWaitResult::Completed; - }; - CHECK_TRUE(resolveMotionCommand( - success_code, - success_code, - request_ignore_code, - wait_succeeded) == MotionCommandOutcome::CompletedAfterMotion); - CHECK_TRUE(wait_calls == 1); - - wait_calls = 0; - CHECK_TRUE(resolveMotionCommand( - request_ignore_code, - success_code, - request_ignore_code, - wait_succeeded) == MotionCommandOutcome::CompletedWithoutMotion); - CHECK_TRUE(wait_calls == 0); - - const int submit_failures[] = {1, 2, 3, -request_ignore_code}; - for (const int return_code : submit_failures) { - wait_calls = 0; - CHECK_TRUE(resolveMotionCommand( - return_code, - success_code, - request_ignore_code, - wait_succeeded) == MotionCommandOutcome::SubmitFailed); - CHECK_TRUE(wait_calls == 0); - } - - wait_calls = 0; - const auto wait_failed = [&wait_calls]() { - ++wait_calls; - return MotionWaitResult::Failed; - }; - CHECK_TRUE(resolveMotionCommand( - success_code, - success_code, - request_ignore_code, - wait_failed) == MotionCommandOutcome::CompletionFailed); - CHECK_TRUE(wait_calls == 1); - - wait_calls = 0; - const auto wait_cancelled = [&wait_calls]() { - ++wait_calls; - return MotionWaitResult::Cancelled; - }; - CHECK_TRUE(resolveMotionCommand( - success_code, - success_code, - request_ignore_code, - wait_cancelled) == MotionCommandOutcome::Cancelled); - CHECK_TRUE(wait_calls == 1); - - using cmvr::device::ArmErrorCode; - using cmvr::device::Result; - std::vector mutation_trace; - const auto failed_mutation = runTorqueOnControllerMutation( - success_code, - [&mutation_trace] { - mutation_trace.push_back("mutation"); - return 42; - }, - [&mutation_trace](const int return_code) { - mutation_trace.push_back("primary-failure"); - return Result::failure( - ArmErrorCode::CommandFailed, - "vendor failure ret=" + std::to_string(return_code)); - }, - [&mutation_trace]() -> std::optional { - mutation_trace.push_back("cancellation-cleanup"); - return Result::failure( - ArmErrorCode::CommandRejected, - "controller safety termination confirmed"); - }); - CHECK_TRUE(failed_mutation.has_value()); - CHECK_TRUE(failed_mutation->code == ArmErrorCode::CommandFailed); - CHECK_TRUE( - failed_mutation->message.find("vendor failure ret=42") != - std::string::npos); - CHECK_TRUE( - failed_mutation->message.find( - "controller safety termination confirmed") != - std::string::npos); - CHECK_TRUE(mutation_trace.size() == 3); - CHECK_TRUE(mutation_trace[0] == "mutation"); - CHECK_TRUE(mutation_trace[1] == "primary-failure"); - CHECK_TRUE(mutation_trace[2] == "cancellation-cleanup"); - - const auto cleanup_failed = preservePrimaryTorqueOnFailure( - Result::failure( - ArmErrorCode::CommandFailed, - "vendor exception"), - std::optional{Result::failure( - ArmErrorCode::CommandFailed, - "safety termination failed")}); - CHECK_TRUE(cleanup_failed.code == ArmErrorCode::CommandFailed); - CHECK_TRUE( - cleanup_failed.message.find("vendor exception") != - std::string::npos); - CHECK_TRUE( - cleanup_failed.message.find("safety termination failed") != - std::string::npos); - - int successful_cancellation_checks = 0; - const auto cancelled_after_success = runTorqueOnControllerMutation( - success_code, - [] { return 0; }, - [](const int) { - return Result::failure( - ArmErrorCode::CommandFailed, "must not be used"); - }, - [&successful_cancellation_checks]() -> std::optional { - ++successful_cancellation_checks; - return Result::failure( - ArmErrorCode::CommandRejected, - "cancelled after successful mutation"); - }); - CHECK_TRUE(cancelled_after_success.has_value()); - CHECK_TRUE( - cancelled_after_success->code == ArmErrorCode::CommandRejected); - CHECK_TRUE(successful_cancellation_checks == 1); - - return 0; -} diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp deleted file mode 100644 index 6f68937d..00000000 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp +++ /dev/null @@ -1,213 +0,0 @@ -#include "devices/arm/aubo_arm/aubo_motion_state.h" - -#include -#include -#include -#include - -namespace { - -#define CHECK_TRUE(condition) \ - do { \ - if (!(condition)) { \ - std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ - << #condition << std::endl; \ - return 1; \ - } \ - } while (false) - -} // namespace - -int main() -{ - using namespace cmvr::device::aubo_internal; - - MotionState state; - CHECK_TRUE(state.begin(MotionKind::None).status == - MotionStartStatus::Invalid); - const auto joint = state.begin(MotionKind::Joint); - CHECK_TRUE(joint.started()); - CHECK_TRUE(state.busy()); - CHECK_TRUE(state.begin(MotionKind::Linear).status == - MotionStartStatus::Busy); - - const auto stop_joint = state.beginStop(); - CHECK_TRUE(stop_joint.started()); - CHECK_TRUE(stop_joint.kind == MotionKind::Joint); - CHECK_TRUE(stop_joint.active_token.generation == - joint.token.generation); - CHECK_TRUE(stop_joint.tracked_motion); - CHECK_TRUE(state.cancelled(joint.token)); - const auto joined_stop_joint = state.beginStop(); - CHECK_TRUE(joined_stop_joint.status == - StopStartStatus::AlreadyStopping); - CHECK_TRUE( - state.waitForStopCompletion( - joined_stop_joint, std::chrono::milliseconds(1)) == - StopWaitStatus::Timeout); - CHECK_TRUE(state.begin(MotionKind::Linear).status == - MotionStartStatus::Stopping); - CHECK_TRUE(!state.waitForOwnerExit( - joint.token, std::chrono::milliseconds(1))); - CHECK_TRUE(!state.completeStop()); - - state.finish(joint.token); - CHECK_TRUE(state.waitForOwnerExit( - joint.token, std::chrono::milliseconds(1))); - CHECK_TRUE(state.completeStop()); - CHECK_TRUE( - state.waitForStopCompletion( - joined_stop_joint, std::chrono::milliseconds(1)) == - StopWaitStatus::Completed); - CHECK_TRUE(!state.busy()); - - const auto linear = state.begin(MotionKind::Linear); - CHECK_TRUE(linear.started()); - CHECK_TRUE(!state.cancelled(linear.token)); - // A delayed guard from the cancelled command must not release a newer one. - state.finish(joint.token); - CHECK_TRUE(state.busy()); - state.finish(linear.token, MotionFinishMode::Clear); - CHECK_TRUE(!state.busy()); - - const auto speed_joint = state.begin(MotionKind::Joint); - CHECK_TRUE(speed_joint.started()); - state.finish(speed_joint.token, MotionFinishMode::Retain); - CHECK_TRUE(state.busy()); - CHECK_TRUE(state.begin(MotionKind::Linear).status == - MotionStartStatus::Busy); - const auto rejected_speed_update = - state.begin(MotionKind::Joint, true); - CHECK_TRUE(rejected_speed_update.started()); - state.finish( - rejected_speed_update.token, - MotionFinishMode::RestorePrevious); - const auto stop_speed = state.beginStop(); - CHECK_TRUE(stop_speed.started()); - CHECK_TRUE(stop_speed.kind == MotionKind::Joint); - CHECK_TRUE(stop_speed.tracked_motion); - CHECK_TRUE(state.completeStop()); - CHECK_TRUE(!state.busy()); - - const auto idle_stop = state.beginStop(); - CHECK_TRUE(idle_stop.kind == MotionKind::None); - CHECK_TRUE(!idle_stop.tracked_motion); - const auto joined_idle_stop = state.beginStop(); - CHECK_TRUE(joined_idle_stop.status == - StopStartStatus::AlreadyStopping); - state.failStop(); - CHECK_TRUE( - state.waitForStopCompletion( - joined_idle_stop, std::chrono::milliseconds(1)) == - StopWaitStatus::Failed); - const auto retry_idle_stop = state.beginStop(); - CHECK_TRUE(retry_idle_stop.kind == MotionKind::None); - CHECK_TRUE(!retry_idle_stop.tracked_motion); - CHECK_TRUE(state.completeStop()); - - const auto mismatched_stop_motion = state.begin(MotionKind::Linear); - CHECK_TRUE(mismatched_stop_motion.started()); - const auto mismatched_stop = state.beginStop(MotionKind::Joint); - CHECK_TRUE(mismatched_stop.kind == MotionKind::Linear); - state.finish(mismatched_stop_motion.token); - CHECK_TRUE(state.completeStop()); - - const auto uncertain_motion = state.begin(MotionKind::Joint); - CHECK_TRUE(uncertain_motion.started()); - state.failMotion(uncertain_motion.token); - CHECK_TRUE(state.busy()); - CHECK_TRUE(state.begin(MotionKind::Linear).status == - MotionStartStatus::Blocked); - const auto stop_uncertain = state.beginStop(); - CHECK_TRUE(stop_uncertain.kind == MotionKind::Joint); - CHECK_TRUE(stop_uncertain.tracked_motion); - CHECK_TRUE(state.completeStop()); - - const auto failed_stop_motion = state.begin(MotionKind::Linear); - CHECK_TRUE(failed_stop_motion.started()); - const auto failed_stop = state.beginStop(); - CHECK_TRUE(failed_stop.kind == MotionKind::Linear); - state.failStop(); - CHECK_TRUE(state.busy()); - CHECK_TRUE(state.begin(MotionKind::Joint).status == - MotionStartStatus::Blocked); - state.finish(failed_stop_motion.token); - - const auto retry = state.beginStop(); - CHECK_TRUE(retry.started()); - CHECK_TRUE(retry.kind == MotionKind::Linear); - CHECK_TRUE(state.completeStop()); - const auto recovered = state.begin(MotionKind::Joint); - CHECK_TRUE(recovered.started()); - state.finish(recovered.token, MotionFinishMode::Clear); - - const auto safety_motion = state.begin(MotionKind::Linear); - CHECK_TRUE(safety_motion.started()); - const auto safety_cancel = state.cancelActiveForSafety(); - CHECK_TRUE(safety_cancel.kind == MotionKind::Linear); - CHECK_TRUE(safety_cancel.tracked_motion); - CHECK_TRUE(state.cancelled(safety_motion.token)); - CHECK_TRUE(state.begin(MotionKind::Joint).status == - MotionStartStatus::Blocked); - state.finish(safety_motion.token); - const auto safety_stop = state.beginStop(); - CHECK_TRUE(safety_stop.started()); - CHECK_TRUE(safety_stop.kind == MotionKind::Linear); - CHECK_TRUE(state.completeStop()); - - const auto retained_speed = state.begin(MotionKind::Joint); - CHECK_TRUE(retained_speed.started()); - state.finish(retained_speed.token, MotionFinishMode::Retain); - const auto retained_cancel = state.cancelActiveForSafety(); - CHECK_TRUE(retained_cancel.kind == MotionKind::Joint); - CHECK_TRUE(retained_cancel.tracked_motion); - CHECK_TRUE(!retained_cancel.active_token.valid()); - CHECK_TRUE(state.begin(MotionKind::Linear).status == - MotionStartStatus::Blocked); - const auto retained_stop = state.beginStop(); - CHECK_TRUE(retained_stop.started()); - CHECK_TRUE(retained_stop.kind == MotionKind::Joint); - CHECK_TRUE(retained_stop.tracked_motion); - CHECK_TRUE(state.completeStop()); - - // A waiter keeps the completion for the stop it joined even when another - // stop starts and finishes before the waiter is scheduled again. - const auto concurrent_first_stop = state.beginStop(); - CHECK_TRUE(concurrent_first_stop.started()); - const auto concurrent_first_join = state.beginStop(); - CHECK_TRUE(concurrent_first_join.status == - StopStartStatus::AlreadyStopping); - std::atomic waiter_entered{false}; - std::atomic first_wait_result{ - StopWaitStatus::Timeout}; - std::thread first_waiter([&]() { - waiter_entered.store(true, std::memory_order_release); - first_wait_result.store( - state.waitForStopCompletion( - concurrent_first_join, - std::chrono::milliseconds(500)), - std::memory_order_release); - }); - while (!waiter_entered.load(std::memory_order_acquire)) { - std::this_thread::yield(); - } - - const bool concurrent_first_completed = state.completeStop(); - const auto concurrent_second_stop = state.beginStop(); - const auto concurrent_second_join = state.beginStop(); - state.failStop(); - first_waiter.join(); - - CHECK_TRUE(concurrent_first_completed); - CHECK_TRUE(concurrent_second_stop.started()); - CHECK_TRUE(concurrent_second_join.status == - StopStartStatus::AlreadyStopping); - CHECK_TRUE(first_wait_result.load(std::memory_order_acquire) == - StopWaitStatus::Completed); - CHECK_TRUE( - state.waitForStopCompletion( - concurrent_second_join, std::chrono::milliseconds(1)) == - StopWaitStatus::Failed); - - return 0; -} diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp deleted file mode 100644 index 716d2b00..00000000 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp +++ /dev/null @@ -1,136 +0,0 @@ -#include "devices/arm/aubo_arm/aubo_safety_state.h" - -#include - -namespace { - -#define CHECK_TRUE(condition) \ - do { \ - if (!(condition)) { \ - std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ - << #condition << std::endl; \ - return 1; \ - } \ - } while (false) - -} // namespace - -int main() -{ - using namespace cmvr::device::aubo_internal; - - SafetyState state; - CHECK_TRUE(!state.tryPermit().has_value()); - - state.observe(SafetyCondition::Normal); - const auto initial_permit = state.tryPermit(); - CHECK_TRUE(initial_permit.has_value()); - CHECK_TRUE(state.validate(*initial_permit)); - - CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, 1) == - SafetyCondition::RobotEmergencyStop); - CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, -1) == - SafetyCondition::Unknown); - CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Reduced, 0) == - SafetyCondition::Reduced); - CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::ProtectiveStop)); - CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::Violation)); - CHECK_TRUE(!needsProtectiveUnlock(SafetyCondition::SafeguardStop)); - CHECK_TRUE(needsInterfaceBoardRestart( - SafetyCondition::RobotEmergencyStop)); - CHECK_TRUE(needsInterfaceBoardRestart( - SafetyCondition::SystemEmergencyStop)); - CHECK_TRUE(needsInterfaceBoardRestart(SafetyCondition::Fault)); - CHECK_TRUE(!needsInterfaceBoardRestart(SafetyCondition::Recovery)); - - state.observe(SafetyCondition::RobotEmergencyStop); - CHECK_TRUE(!state.validate(*initial_permit)); - CHECK_TRUE(state.snapshot().latched); - CHECK_TRUE(!state.beginRecovery(state.snapshot().epoch).has_value()); - - // The observed level alone does not unlock motion. The monitor must first - // prove the old controller operation is fully quiescent. - state.observe(SafetyCondition::Normal); - CHECK_TRUE(state.snapshot().latched); - CHECK_TRUE(!state.tryPermit().has_value()); - CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop( - state.snapshot(), false, 0, true, false)); - CHECK_TRUE(shouldAutoRecoverHardwareEmergencyStop( - state.snapshot(), true, 0, true, false)); - CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop( - state.snapshot(), true, 0, false, false)); - CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop( - state.snapshot(), true, 0, true, true)); - - const auto recovery = state.beginRecovery(state.snapshot().epoch); - CHECK_TRUE(recovery.has_value()); - CHECK_TRUE(!state.completeRecovery(*recovery, true, true, false)); - state.failRecovery(*recovery); - - const auto retry = state.beginRecovery(state.snapshot().epoch); - CHECK_TRUE(retry.has_value()); - // Automatic release is not committed at Idle/PowerOn. The controller - // must have completed startup and reached Running first. - CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery( - *retry, false, true, true)); - CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery( - *retry, true, false, true)); - CHECK_TRUE(state.completeHardwareEmergencyStopRecovery( - *retry, true, true, true)); - const auto recovered_permit = state.tryPermit(); - CHECK_TRUE(recovered_permit.has_value()); - CHECK_TRUE(state.validate(*recovered_permit)); - - // Releasing a real E-stop must never clear a software-triggered stop that - // was latched while the hardware input was active. - state.observe(SafetyCondition::RobotEmergencyStop); - state.observe(SafetyCondition::SoftwareEmergencyStop); - // The hardware monitor continues publishing the physical E-stop level - // until the switch is released. It must not overwrite the software latch. - state.observe(SafetyCondition::RobotEmergencyStop); - state.observe(SafetyCondition::Normal); - CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop( - state.snapshot(), true, 0, true, false)); - CHECK_TRUE(state.snapshot().software_emergency_stop_latched); - CHECK_TRUE(state.snapshot().latched_reason == - SafetyCondition::SoftwareEmergencyStop); - const auto software_recovery = state.beginRecovery( - state.snapshot().epoch); - CHECK_TRUE(software_recovery.has_value()); - CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery( - *software_recovery, true, true, true)); - state.failRecovery(*software_recovery); - const auto explicit_software_recovery = state.beginRecovery( - state.snapshot().epoch); - CHECK_TRUE(explicit_software_recovery.has_value()); - CHECK_TRUE(state.completeRecovery( - *explicit_software_recovery, true, true, true)); - CHECK_TRUE(!state.snapshot().software_emergency_stop_latched); - - // A new safety event invalidates an in-flight recovery token. - state.observe(SafetyCondition::ProtectiveStop); - state.observe(SafetyCondition::Reduced); - const auto stale_recovery = state.beginRecovery( - state.snapshot().epoch); - CHECK_TRUE(stale_recovery.has_value()); - CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery( - *stale_recovery, true, true, true)); - state.failRecovery(*stale_recovery); - const auto explicit_recovery = state.beginRecovery( - state.snapshot().epoch); - CHECK_TRUE(explicit_recovery.has_value()); - state.observe(SafetyCondition::SafeguardStop); - state.observe(SafetyCondition::Normal); - CHECK_TRUE(!state.completeRecovery( - *explicit_recovery, true, true, true)); - CHECK_TRUE(state.snapshot().latched); - - // An old API call must not begin recovery for a newer safety event. - const auto stale_epoch = state.snapshot().epoch; - state.observe(SafetyCondition::RobotEmergencyStop); - state.observe(SafetyCondition::Normal); - CHECK_TRUE(!state.beginRecovery(stale_epoch).has_value()); - CHECK_TRUE(!state.snapshot().recovery_in_progress); - - return 0; -} diff --git a/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt b/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt index 6001a5d7..617fab7d 100644 --- a/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt @@ -1,7 +1,5 @@ add_library(huayan_arm SHARED huayan_arm.cpp) -find_package(Threads REQUIRED) - set(HUAYAN_ARM_SDK_DIR ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/huayan_arm/v1.0) target_include_directories(huayan_arm @@ -22,56 +20,9 @@ target_link_libraries(huayan_arm PRIVATE HR_Pro glog - Threads::Threads ) add_library(cmvr_es::device::huayan_arm ALIAS huayan_arm) install(TARGETS huayan_arm LIBRARY DESTINATION lib) -install(FILES ${HUAYAN_ARM_SDK_DIR}/lib/libHR_Pro.so DESTINATION lib) - -if(BUILD_TESTING) - add_executable(huayan_lifecycle_state_test - tests/huayan_lifecycle_state_test.cpp - ) - target_include_directories(huayan_lifecycle_state_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(huayan_lifecycle_state_test - PRIVATE - Threads::Threads - ) - add_test( - NAME huayan_lifecycle_state_test - COMMAND huayan_lifecycle_state_test - ) - set_tests_properties(huayan_lifecycle_state_test PROPERTIES TIMEOUT 10) - - if(UNIX AND NOT APPLE) - add_executable(huayan_arm_sdk_test - tests/huayan_arm_sdk_test.cpp - ) - target_include_directories(huayan_arm_sdk_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${HUAYAN_ARM_SDK_DIR}/include - ) - target_link_libraries(huayan_arm_sdk_test - PRIVATE - cmvr_es::device::huayan_arm - Threads::Threads - ) - # Export the fake HRIF_* definitions so libhuayan_arm resolves its SDK - # calls to the deterministic test controller instead of real hardware. - target_link_options(huayan_arm_sdk_test PRIVATE -Wl,--export-dynamic) - add_test( - NAME huayan_arm_sdk_test - COMMAND huayan_arm_sdk_test - ) - set_tests_properties(huayan_arm_sdk_test PROPERTIES - TIMEOUT 20 - ENVIRONMENT "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - endif() -endif() +install(FILES ${HUAYAN_ARM_SDK_DIR}/lib/libHR_Pro.so DESTINATION lib) \ No newline at end of file diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index 90c0e627..74ef039f 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp @@ -2,52 +2,20 @@ #include #include -#include #include -#include -#include #include -#include -#include "common/base/logging/logger.h" #include "huayan_arm/v1.0/include/HR_Pro.h" +#include "common/base/logging/logger.h" namespace cmvr::device { namespace { -using huayan_internal::MotionFinishMode; -using huayan_internal::MotionKind; -using huayan_internal::MotionStartStatus; -using huayan_internal::SafetyCondition; - constexpr double kPi = 3.14159265358979323846; constexpr double kDefaultMoveJVelocityDeg = 30.0; constexpr double kDefaultMoveJAccelerationDeg = 60.0; constexpr double kDefaultMoveLVelocityMm = 100.0; constexpr double kDefaultMoveLAccelerationMm = 200.0; -constexpr double kJointTargetToleranceRad = 0.002; -constexpr double kTcpPositionToleranceM = 0.0005; -constexpr double kTcpRotationToleranceRad = 0.003; -constexpr double kIdleVelocityToleranceRad = 0.01; -constexpr auto kSafetyPollPeriod = std::chrono::milliseconds(50); -constexpr auto kControllerStopTimeout = std::chrono::milliseconds(3000); -constexpr auto kOwnerExitTimeout = std::chrono::milliseconds(3000); -constexpr auto kCompletionCorrelationGrace = std::chrono::milliseconds(250); - -bool cancellationRequested( - const std::function& cancellation_requested) noexcept -{ - if (!cancellation_requested) { - return false; - } - try { - return cancellation_requested(); - } catch (...) { - // Cancellation sources are part of the motion-admission safety gate. - // Treat an exception as cancellation instead of admitting new motion. - return true; - } -} double radToDeg(const double value) { @@ -69,18 +37,6 @@ double mmToMeters(const double value) return value / 1000.0; } -double angularDistance(const double lhs, const double rhs) -{ - return std::abs(std::remainder(lhs - rhs, 2.0 * kPi)); -} - -std::int64_t monotonicNowNs() -{ - return std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()) - .count(); -} - std::vector defaultJointNames(const std::size_t dof) { std::vector names; @@ -91,13 +47,11 @@ std::vector defaultJointNames(const std::size_t dof) return names; } -std::array toSix( - const std::vector& values, - const double fill = 0.0) +std::array toSix(const std::vector& values, const double fill = 0.0) { std::array out{fill, fill, fill, fill, fill, fill}; - const auto count = std::min(out.size(), values.size()); - for (std::size_t i = 0; i < count; ++i) { + const auto n = std::min(out.size(), values.size()); + for (std::size_t i = 0; i < n; ++i) { out[i] = values[i]; } return out; @@ -105,13 +59,12 @@ std::array toSix( std::vector poseToHrCoord(const CartesianPose& pose) { - return { - metersToMm(pose.x), - metersToMm(pose.y), - metersToMm(pose.z), - radToDeg(pose.rx), - radToDeg(pose.ry), - radToDeg(pose.rz)}; + return {metersToMm(pose.x), + metersToMm(pose.y), + metersToMm(pose.z), + radToDeg(pose.rx), + radToDeg(pose.ry), + radToDeg(pose.rz)}; } std::vector zeroHrFrame() @@ -119,58 +72,8 @@ std::vector zeroHrFrame() return {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; } -SafetyMode safetyModeFromCondition(const SafetyCondition condition) -{ - switch (condition) { - case SafetyCondition::Normal: - return SafetyMode::Normal; - case SafetyCondition::EmergencyStop: - case SafetyCondition::SoftwareEmergencyStop: - return SafetyMode::EmergencyStop; - case SafetyCondition::SafeguardStop: - return SafetyMode::SafeguardStop; - case SafetyCondition::SoftwareProtectiveStop: - return SafetyMode::ProtectiveStop; - case SafetyCondition::RobotFault: - case SafetyCondition::EmergencySignalFault: - case SafetyCondition::SafeguardSignalFault: - return SafetyMode::Fault; - case SafetyCondition::Unknown: - return SafetyMode::Unknown; - } - return SafetyMode::Unknown; -} - -bool isEmergencyCondition(const SafetyCondition condition) -{ - return condition == SafetyCondition::EmergencyStop || - condition == SafetyCondition::SoftwareEmergencyStop || - condition == SafetyCondition::EmergencySignalFault; -} - -bool isProtectiveCondition(const SafetyCondition condition) -{ - return condition == SafetyCondition::SafeguardStop || - condition == SafetyCondition::SoftwareProtectiveStop || - condition == SafetyCondition::SafeguardSignalFault; -} - } // namespace -struct HuayanRobot::RuntimeState { - huayan_internal::MotionState motion; - huayan_internal::SafetyState safety; - std::atomic monitor_running{true}; - std::atomic program_active{false}; - std::atomic termination_confirmed{false}; - std::atomic last_valid_sample_ns{0}; - std::atomic speed_completion_not_before_ns{0}; - mutable std::recursive_mutex termination_mutex; - mutable std::recursive_mutex submission_mutex; - mutable std::mutex wait_mutex; - std::condition_variable wait_cv; -}; - HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg) : cfg_(cfg) { @@ -181,24 +84,14 @@ HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg) ip_ = vendor_cfg_.ip(); port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 10003; - tcp_name_ = vendor_cfg_.tool_frame().empty() - ? "TCP" - : vendor_cfg_.tool_frame(); - ucs_name_ = vendor_cfg_.base_frame().empty() - ? "Base" - : vendor_cfg_.base_frame(); + tcp_name_ = vendor_cfg_.tool_frame().empty() ? "TCP" : vendor_cfg_.tool_frame(); + ucs_name_ = vendor_cfg_.base_frame().empty() ? "Base" : vendor_cfg_.base_frame(); - const auto dof = vendor_cfg_.dof() > 0 - ? static_cast(vendor_cfg_.dof()) - : 6U; - model_.name = vendor_cfg_.model().empty() - ? "HuayanRobot" - : vendor_cfg_.model(); + const auto dof = vendor_cfg_.dof() > 0 ? static_cast(vendor_cfg_.dof()) : 6U; + model_.name = vendor_cfg_.model().empty() ? "HuayanRobot" : vendor_cfg_.model(); model_.manufacturer = "Huayan"; model_.dof = dof; - model_.joint_names.assign( - vendor_cfg_.joint_names().begin(), - vendor_cfg_.joint_names().end()); + model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end()); if (model_.joint_names.empty()) { model_.joint_names = defaultJointNames(dof); } @@ -210,23 +103,7 @@ HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg) HuayanRobot::~HuayanRobot() { - const auto result = disconnect(); - if (result.ok()) { - return; - } - CMVR_LOG(ERROR) << "[HuayanRobot] destructor forced a best-effort disconnect " - << "after safe disconnect failed: " << result.message; - const auto runtime = runtimeSnapshot_(); - if (runtime) { - runtime->monitor_running.store(false); - runtime->wait_cv.notify_all(); - } - if (safety_monitor_thread_.joinable()) { - safety_monitor_thread_.join(); - } - std::lock_guard sdk_lock(sdk_mutex_); - (void)HRIF_DisConnect(box_id_); - connected_.store(false); + (void)disconnect(); } bool HuayanRobot::init() @@ -240,12 +117,7 @@ bool HuayanRobot::init() CMVR_LOG(ERROR) << "[HuayanRobot] init failed: " << result.message; return false; } - const auto scaling_result = setSpeedScaling(1.0); - if (!scaling_result.ok()) { - CMVR_LOG(ERROR) << "[HuayanRobot] set speed scaling failed: " - << scaling_result.message; - return false; - } + setSpeedScaling(1); return true; } @@ -257,48 +129,20 @@ bool HuayanRobot::stop() ArmState HuayanRobot::getRobotState() const { const auto hr_state = readHrState_(); - const auto runtime = runtimeSnapshot_(); - const auto safety = runtime - ? runtime->safety.snapshot() - : huayan_internal::SafetySnapshot{}; - const auto motion = runtime - ? runtime->motion.snapshot() - : huayan_internal::MotionSnapshot{}; ArmState state; state.connected = isConnected(); - state.powered_on = hr_state.valid && hr_state.electrified != 0; - state.brake_released = hr_state.valid && hr_state.brake != 0; - state.moving = hr_state.valid && hr_state.moving != 0; - state.program_running = runtime && runtime->program_active.load(); - state.protective_stopped = safety.latched && - isProtectiveCondition(safety.latched_reason); - state.emergency_stopped = safety.latched && - isEmergencyCondition(safety.latched_reason); - state.fault = !hr_state.valid || hr_state.error != 0 || - (safety.latched && safetyModeFromCondition(safety.latched_reason) == SafetyMode::Fault); - if (!state.connected) { - state.robot_mode = RobotMode::Disconnected; - } else if (!hr_state.valid) { - state.robot_mode = RobotMode::Unknown; - } else if (state.fault) { - state.robot_mode = RobotMode::Fault; - } else if (safety.latched) { - state.robot_mode = RobotMode::Stopped; - } else if (hr_state.paused != 0) { - state.robot_mode = RobotMode::Paused; - } else if (hr_state.electrified == 0) { - state.robot_mode = RobotMode::PowerOff; - } else if (hr_state.moving != 0 || state.program_running || motion.owner_active) { - state.robot_mode = RobotMode::Running; - } else { - state.robot_mode = RobotMode::Idle; - } - state.safety_mode = safety.latched - ? safetyModeFromCondition(safety.latched_reason) - : safetyModeFromCondition(safety.observed); + state.powered_on = hr_state.valid ? hr_state.electrified != 0 : state.connected; + state.brake_released = hr_state.valid ? hr_state.brake != 0 : state.connected; + state.moving = hr_state.valid ? hr_state.moving != 0 : busy_.load(); + state.program_running = state.moving; + state.protective_stopped = hr_state.valid ? hr_state.safeguard != 0 : false; + state.emergency_stopped = hr_state.valid ? hr_state.emergency_stop != 0 : false; + state.fault = hr_state.valid ? hr_state.error != 0 : false; + state.robot_mode = getRobotMode(); + state.safety_mode = getSafetyMode(); state.control_mode = getControlMode(); - state.speed_scaling = speed_scaling_.load(); + state.speed_scaling = speed_scaling_; state.actual_joint_state = getJointState(); state.target_joint_state = state.actual_joint_state; state.actual_tcp_pose = readTcpPose_(); @@ -309,20 +153,15 @@ ArmState HuayanRobot::getRobotState() const JointGroupState HuayanRobot::getJointState() const { JointGroupState state; - state.position_valid = readJointPositionSample_(state.position); - state.velocity_valid = readJointVelocitySample_(state.velocity); + state.position = readJointPositionRad_(); + state.velocity = readJointVelocityRad_(); state.effort.assign(model_.dof, 0.0); - state.effort_valid = false; - state.sample_monotonic_ns = monotonicNowNs(); return state; } CartesianPose HuayanRobot::getTcpPose(const FrameType frame) const { - if (frame != FrameType::Base) { - CMVR_LOG(ERROR) << "[HuayanRobot] getTcpPose supports Base frame only"; - return {}; - } + (void)frame; return readTcpPose_(); } @@ -331,69 +170,42 @@ RobotMode HuayanRobot::getRobotMode() const if (!isConnected()) { return RobotMode::Disconnected; } + const auto state = readHrState_(); if (!state.valid) { - return RobotMode::Unknown; - } - const auto runtime = runtimeSnapshot_(); - if (runtime) { - const auto safety = runtime->safety.snapshot(); - if (safety.latched) { - const auto mode = safetyModeFromCondition(safety.latched_reason); - return mode == SafetyMode::Fault ? RobotMode::Fault : RobotMode::Stopped; - } + return busy_.load() ? RobotMode::Running : RobotMode::Idle; } if (state.error != 0) { return RobotMode::Fault; } + if (state.emergency_stop != 0) { + return RobotMode::Stopped; + } if (state.paused != 0) { return RobotMode::Paused; } if (state.electrified == 0) { return RobotMode::PowerOff; } - if (state.moving != 0 || (runtime && runtime->program_active.load())) { - return RobotMode::Running; - } - return RobotMode::Idle; + return state.moving != 0 ? RobotMode::Running : RobotMode::Idle; } SafetyMode HuayanRobot::getSafetyMode() const { - const auto runtime = runtimeSnapshot_(); - if (!runtime) { + const auto state = readHrState_(); + if (!state.valid) { return SafetyMode::Unknown; } - (void)readHrState_(); - const auto safety = runtime->safety.snapshot(); - return safetyModeFromCondition( - safety.latched ? safety.latched_reason : safety.observed); -} - -ControlMode HuayanRobot::getControlMode() const -{ - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return ControlMode::None; + if (state.error != 0) { + return SafetyMode::Fault; } - const auto motion = runtime->motion.snapshot(); - const auto kind = motion.owner_active - ? motion.active_kind - : motion.retained_kind; - switch (kind) { - case MotionKind::Joint: - case MotionKind::Linear: - return ControlMode::Position; - case MotionKind::SpeedJoint: - case MotionKind::SpeedLinear: - return ControlMode::Velocity; - case MotionKind::Servo: - return ControlMode::Servo; - case MotionKind::None: - case MotionKind::Program: - return ControlMode::None; + if (state.emergency_stop != 0) { + return SafetyMode::EmergencyStop; } - return ControlMode::None; + if (state.safeguard != 0) { + return SafetyMode::SafeguardStop; + } + return SafetyMode::Normal; } Result HuayanRobot::torqueOn() @@ -402,127 +214,20 @@ Result HuayanRobot::torqueOn() if (!ready.ok()) { return ready; } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] torqueOn failed: runtime is unavailable"); - } - - const auto state = sampleHrState_(runtime); - publishHrState_(runtime, state); - const auto safety = runtime->safety.snapshot(); - if (safety.latched) { - if (!isEmergencyCondition(safety.latched_reason)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] torqueOn cannot clear this safety latch; use the typed recovery API"); - } - return completeSafetyRecovery_( - "torqueOn", - runtime, - safety.epoch, - true, - software_protective_stopped_.load()); - } - if (!state.valid) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] torqueOn failed: safety state is unavailable"); - } - - int ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - ret = HRIF_GrpEnable(box_id_, robot_id_); - } - const auto result = hrResult_(ret, "GrpEnable"); - if (!result.ok()) { - return result; - } - const auto after = sampleHrState_(runtime); - publishHrState_(runtime, after); - if (!after.valid || after.enabled == 0 || after.electrified == 0 || - runtime->safety.snapshot().latched) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] torqueOn failed: enabled state was not confirmed"); - } - return Result::success(); + std::lock_guard lock(mutex_); + return hrResult_(HRIF_GrpEnable(box_id_, robot_id_), "GrpEnable"); } Result HuayanRobot::torqueOff() { + const auto ready = ensureConnected_("torqueOff"); if (!ready.ok()) { return ready; } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] torqueOff failed: runtime is unavailable"); - } - std::lock_guard termination_lock( - runtime->termination_mutex); - const auto stop_result = stopMotion(); - if (!stop_result.ok()) { - return stop_result; - } - huayan_internal::StopRequest poweroff_barrier; - int ret = 0; - { - // Keep a Stop generation active through GrpDisable. A command that - // raced the first Stop is cancelled here before it can submit. - std::lock_guard submission_lock( - runtime->submission_mutex); - poweroff_barrier = runtime->motion.beginStop(); - if (!poweroff_barrier.started()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] torqueOff failed: could not establish the power-off barrier"); - } - runtime->wait_cv.notify_all(); - if (!terminateController_( - runtime, - kControllerStopTimeout, - runtime->program_active.load() || - poweroff_barrier.kind == MotionKind::Program)) { - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] torqueOff failed: final controller Stop was not confirmed"); - } - std::lock_guard sdk_lock(sdk_mutex_); - ret = HRIF_GrpDisable(box_id_, robot_id_); - } - if (ret != 0) { - runtime->motion.failStop(); - return hrResult_(ret, "GrpDisable"); - } - const bool owner_exited = runtime->motion.waitForOwnerExit( - poweroff_barrier.active_token, kOwnerExitTimeout); - bool disabled = false; - const auto deadline = std::chrono::steady_clock::now() + - kControllerStopTimeout; - while (owner_exited && std::chrono::steady_clock::now() < deadline) { - const auto state = sampleHrState_(runtime); - publishHrState_(runtime, state); - if (state.valid && state.enabled == 0 && state.electrified == 0) { - disabled = true; - break; - } - std::unique_lock wait_lock(runtime->wait_mutex); - runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); - } - if (!owner_exited || !disabled || !runtime->motion.completeStop()) { - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] torqueOff failed: disabled state was not confirmed"); - } - return Result::success(); + std::lock_guard lock(mutex_); + return hrResult_(HRIF_GrpDisable(box_id_, robot_id_), "GrpDisable"); } Result HuayanRobot::calibrateZeroQ(const std::string& joint_name) @@ -533,291 +238,105 @@ Result HuayanRobot::calibrateZeroQ(const std::string& joint_name) Result HuayanRobot::emergencyStop() { - const auto ready = ensureConnected_("emergencyStop"); - if (!ready.ok()) { - return ready; - } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] emergencyStop failed: runtime is unavailable"); - } - software_emergency_stopped_.store(true); - { - std::lock_guard submission_lock( - runtime->submission_mutex); - runtime->safety.observe(SafetyCondition::SoftwareEmergencyStop); - runtime->termination_confirmed.store(false); - (void)runtime->motion.cancelActiveForSafety(); - } - runtime->wait_cv.notify_all(); - return stopMotion(); -} - -Result HuayanRobot::protectiveStop() -{ - const auto ready = ensureConnected_("protectiveStop"); - if (!ready.ok()) { - return ready; - } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] protectiveStop failed: runtime is unavailable"); - } - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - // Publish/cancel the local safety event before the vendor call while - // excluding every motion submission. No previously permitted command - // can slip in after EnterSafetyGuard and escape local cancellation. - software_protective_stopped_.store(true); - runtime->safety.observe(SafetyCondition::SoftwareProtectiveStop); - runtime->termination_confirmed.store(false); - (void)runtime->motion.cancelActiveForSafety(); - std::lock_guard sdk_lock(sdk_mutex_); - ret = HRIF_EnterSafetyGuard(box_id_, robot_id_, 1); - } - const auto result = hrResult_(ret, "EnterSafetyGuard"); - if (!result.ok()) { - // The event remains latched because the physical outcome is uncertain. - runtime->wait_cv.notify_all(); - return result; - } - runtime->wait_cv.notify_all(); return stopMotion(); } Result HuayanRobot::setSpeedScaling(const double scaling) { if (scaling < 0.0 || scaling > 1.0) { - return Result::failure( - ArmErrorCode::InvalidArgument, - "speed scaling must be in [0, 1]"); + return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]"); } - if (!isConnected()) { - speed_scaling_.store(scaling); - return Result::success(); + speed_scaling_ = scaling; + if (isConnected()) { + return hrResult_(HRIF_SetOverride(box_id_, robot_id_, scaling), "SetOverride"); } - int ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - ret = HRIF_SetOverride(box_id_, robot_id_, scaling); - } - const auto result = hrResult_(ret, "SetOverride"); - if (result.ok()) { - speed_scaling_.store(scaling); - } - return result; + return Result::success(); } bool HuayanRobot::isProtectiveStopped() const { - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return false; - } - (void)readHrState_(); - const auto safety = runtime->safety.snapshot(); - return safety.latched && isProtectiveCondition(safety.latched_reason); + const auto state = readHrState_(); + return state.valid && state.safeguard != 0; } bool HuayanRobot::isEmergencyStopped() const { - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return false; - } - (void)readHrState_(); - const auto safety = runtime->safety.snapshot(); - return safety.latched && isEmergencyCondition(safety.latched_reason); + const auto state = readHrState_(); + return state.valid && state.emergency_stop != 0; } bool HuayanRobot::isFault() const { - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return false; - } const auto state = readHrState_(); - const auto safety = runtime->safety.snapshot(); - return !state.valid || state.error != 0 || - (safety.latched && - safetyModeFromCondition(safety.latched_reason) == SafetyMode::Fault); + return state.valid && state.error != 0; } -Result HuayanRobot::moveJ( - const JointPositionCommand& target, - const MotionOptions& options) +Result HuayanRobot::moveJ(const JointPositionCommand& target, const MotionOptions& options) { std::string error; if (!validDof_(target.position.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] moveJ failed: runtime is unavailable"); - } - std::unique_lock admission_lock( - runtime->submission_mutex); - huayan_internal::SafetyPermit permit; - const auto ready = ensureMotionReady_("moveJ", runtime, permit); + const auto ready = ensureConnected_("moveJ"); if (!ready.ok()) { return ready; } - const auto start = runtime->motion.begin(MotionKind::Joint); - if (!start.started()) { - return motionStartFailure_("moveJ", start.status); - } - - if (targetReached_(&target.position, nullptr) && - controllerIdleStable_(runtime, std::chrono::milliseconds(300))) { - if (cancellationRequested(options.cancellation_requested) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] moveJ cancelled by its caller or a safety transition"); - } - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::success(); + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_); } std::array q_deg{}; - for (std::size_t i = 0; - i < std::min(target.position.size(), q_deg.size()); - ++i) { + for (std::size_t i = 0; i < std::min(target.position.size(), q_deg.size()); ++i) { q_deg[i] = radToDeg(target.position[i]); } - const double velocity = options.velocity > 0.0 - ? radToDeg(options.velocity) - : kDefaultMoveJVelocityDeg; - const double acceleration = options.acceleration > 0.0 - ? radToDeg(options.acceleration) - : kDefaultMoveJAccelerationDeg; - const double blend = metersToMm(options.blend_radius); - const auto command_id = nextCommandId_(); - if (cancellationRequested(options.cancellation_requested) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] moveJ cancelled before submission"); - } - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - std::lock_guard sdk_lock(sdk_mutex_); - if (cancellationRequested(options.cancellation_requested) || - runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] moveJ cancelled before submission"); - } - ret = HRIF_MoveJ( - box_id_, robot_id_, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], - tcp_name_, ucs_name_, velocity * speed_scaling_.load(), acceleration, - blend, 1, 0, 0, 0, command_id); - } + const double velocity = options.velocity > 0.0 ? radToDeg(options.velocity) : kDefaultMoveJVelocityDeg; + const double acceleration = options.acceleration > 0.0 ? radToDeg(options.acceleration) : kDefaultMoveJAccelerationDeg; + const double blend = metersToMm(options.blend_radius); + const std::string command_id = nextCommandId_(); + + const int ret = HRIF_MoveJ(box_id_, robot_id_, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], + tcp_name_, ucs_name_, velocity * speed_scaling_, acceleration, blend, + 1, 0, 0, 0, command_id); if (ret != 0) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); + busy_.store(false); return hrResult_(ret, "moveJ"); } - admission_lock.unlock(); - return waitMotionDone_( - "moveJ", runtime, start.token, permit, command_id, - &target.position, nullptr, 60000, - options.cancellation_requested); + const auto wait_result = waitMotionDone_("moveJ", 60000); + busy_.store(false); + return wait_result; } -Result HuayanRobot::speedJ( - const JointVelocityCommand& velocity, - const double acceleration, - const double duration) +Result HuayanRobot::speedJ(const JointVelocityCommand& velocity, const double acceleration, const double duration) { std::string error; if (!validDof_(velocity.velocity.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] speedJ failed: runtime is unavailable"); - } - std::unique_lock admission_lock( - runtime->submission_mutex); - huayan_internal::SafetyPermit permit; - const auto ready = ensureMotionReady_("speedJ", runtime, permit); + const auto ready = ensureConnected_("speedJ"); if (!ready.ok()) { return ready; } - const auto start = runtime->motion.begin(MotionKind::SpeedJoint); - if (!start.started()) { - return motionStartFailure_("speedJ", start.status); - } - - const bool zero_command = std::all_of( - velocity.velocity.begin(), velocity.velocity.end(), - [](const double value) { return std::abs(value) < 1e-12; }); - if (zero_command) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::success(); - } std::array qd_deg{}; - for (std::size_t i = 0; - i < std::min(velocity.velocity.size(), qd_deg.size()); - ++i) { + for (std::size_t i = 0; i < std::min(velocity.velocity.size(), qd_deg.size()); ++i) { qd_deg[i] = radToDeg(velocity.velocity[i]); } - const double acceleration_deg = acceleration > 0.0 - ? radToDeg(acceleration) - : kDefaultMoveJAccelerationDeg; - const double run_time = duration > 0.0 ? duration : 0.1; - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] speedJ cancelled before submission by a safety transition"); - } - ret = HRIF_SpeedJ( - box_id_, robot_id_, - qd_deg[0], qd_deg[1], qd_deg[2], - qd_deg[3], qd_deg[4], qd_deg[5], - acceleration_deg, run_time); - if (ret == 0) { - runtime->speed_completion_not_before_ns.store( - monotonicNowNs() + - static_cast(run_time * 1e9)); - } - } + const double acc_deg = acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg; + const double runtime = duration > 0.0 ? duration : 0.1; + + const int ret = HRIF_SpeedJ(box_id_, robot_id_, + qd_deg[0], qd_deg[1], qd_deg[2], qd_deg[3], qd_deg[4], qd_deg[5], + acc_deg, runtime); if (ret != 0) { - runtime->speed_completion_not_before_ns.store(0); - runtime->motion.finish(start.token, MotionFinishMode::Clear); + busy_.store(false); return hrResult_(ret, "SpeedJ"); } - admission_lock.unlock(); - return waitMotionDone_( - "SpeedJ", runtime, start.token, permit, {}, nullptr, nullptr, - std::max(3000, static_cast(run_time * 1000.0) + 3000)); + const auto wait_result = waitMotionDone_("SpeedJ", 60000); + busy_.store(false); + return wait_result; } Result HuayanRobot::stopJ(const double acceleration) @@ -826,195 +345,76 @@ Result HuayanRobot::stopJ(const double acceleration) return stopMotion(); } -Result HuayanRobot::moveL( - const CartesianPose& target, - const MotionOptions& options, - const FrameType frame) +Result HuayanRobot::moveL(const CartesianPose& target, const MotionOptions& options, const FrameType frame) { - if (frame != FrameType::Base) { - return Result::failure( - ArmErrorCode::UnsupportedCommand, - "[HuayanRobot] moveL supports Base frame only"); - } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] moveL failed: runtime is unavailable"); - } - std::unique_lock admission_lock( - runtime->submission_mutex); - huayan_internal::SafetyPermit permit; - const auto ready = ensureMotionReady_("moveL", runtime, permit); + (void)frame; + const auto ready = ensureConnected_("moveL"); if (!ready.ok()) { return ready; } - const auto start = runtime->motion.begin(MotionKind::Linear); - if (!start.started()) { - return motionStartFailure_("moveL", start.status); + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_); } - if (targetReached_(nullptr, &target) && - controllerIdleStable_(runtime, std::chrono::milliseconds(300))) { - if (cancellationRequested(options.cancellation_requested) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] moveL cancelled by its caller or a safety transition"); - } - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::success(); - } - - std::vector q_rad; - if (!readJointPositionSample_(q_rad)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] moveL failed: unable to read reference joints"); - } - for (auto& value : q_rad) { - value = radToDeg(value); - } - const auto q_deg = toSix(q_rad); const auto pose = poseToHrCoord(target); - const double velocity = options.velocity > 0.0 - ? metersToMm(options.velocity) - : kDefaultMoveLVelocityMm; - const double acceleration = options.acceleration > 0.0 - ? metersToMm(options.acceleration) - : kDefaultMoveLAccelerationMm; + const auto q_deg = toSix(currentJointPositionDeg_()); + const double velocity = options.velocity > 0.0 ? metersToMm(options.velocity) : kDefaultMoveLVelocityMm; + const double acceleration = options.acceleration > 0.0 ? metersToMm(options.acceleration) : kDefaultMoveLAccelerationMm; const double blend = metersToMm(options.blend_radius); - const auto command_id = nextCommandId_(); + const std::string command_id = nextCommandId_(); - if (cancellationRequested(options.cancellation_requested) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] moveL cancelled before submission"); - } - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - std::lock_guard sdk_lock(sdk_mutex_); - if (cancellationRequested(options.cancellation_requested) || - runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] moveL cancelled before submission"); - } - ret = HRIF_MoveL( - box_id_, robot_id_, - pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], - q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], - tcp_name_, ucs_name_, velocity * speed_scaling_.load(), acceleration, - blend, 0, 0, 0, command_id); - } + const int ret = HRIF_MoveL(box_id_, robot_id_, + pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], + q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], + tcp_name_, ucs_name_, velocity * speed_scaling_, acceleration, blend, + 0, 0, 0, command_id); if (ret != 0) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); + busy_.store(false); return hrResult_(ret, "moveL"); } - admission_lock.unlock(); - return waitMotionDone_( - "moveL", runtime, start.token, permit, command_id, - nullptr, &target, 60000, - options.cancellation_requested); + const auto wait_result = waitMotionDone_("moveL", 60000); + busy_.store(false); + return wait_result; } -Result HuayanRobot::speedL( - const CartesianVelocity& velocity, - const double acceleration, - const double duration, - const FrameType frame) +Result HuayanRobot::speedL(const CartesianVelocity& velocity, + const double acceleration, + const double duration, + const FrameType frame) { - if (frame != FrameType::Base) { - return Result::failure( - ArmErrorCode::UnsupportedCommand, - "[HuayanRobot] speedL supports Base frame only"); - } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] speedL failed: runtime is unavailable"); - } - std::unique_lock admission_lock( - runtime->submission_mutex); - huayan_internal::SafetyPermit permit; - const auto ready = ensureMotionReady_("speedL", runtime, permit); + const auto ready = ensureConnected_("speedL"); if (!ready.ok()) { return ready; } - const auto start = runtime->motion.begin(MotionKind::SpeedLinear); - if (!start.started()) { - return motionStartFailure_("speedL", start.status); - } + const double vx_mm = metersToMm(velocity.vx); + const double vy_mm = metersToMm(velocity.vy); + const double vz_mm = metersToMm(velocity.vz); + const double wx_deg = radToDeg(velocity.wx); + const double wy_deg = radToDeg(velocity.wy); + const double wz_deg = radToDeg(velocity.wz); - const bool zero_command = - std::abs(velocity.vx) < 1e-12 && - std::abs(velocity.vy) < 1e-12 && - std::abs(velocity.vz) < 1e-12 && - std::abs(velocity.wx) < 1e-12 && - std::abs(velocity.wy) < 1e-12 && - std::abs(velocity.wz) < 1e-12; - if (zero_command) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::success(); - } + const double linear_acc_mm = + acceleration > 0.0 ? metersToMm(acceleration) : kDefaultMoveLAccelerationMm; - const double linear_acceleration = acceleration > 0.0 - ? metersToMm(acceleration) - : kDefaultMoveLAccelerationMm; - const double angular_acceleration = acceleration > 0.0 - ? radToDeg(acceleration) - : kDefaultMoveJAccelerationDeg; - const double run_time = duration > 0.0 ? duration : 0.5; - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] speedL cancelled before submission by a safety transition"); - } - ret = HRIF_SpeedL( - box_id_, robot_id_, - metersToMm(velocity.vx), - metersToMm(velocity.vy), - metersToMm(velocity.vz), - radToDeg(velocity.wx), - radToDeg(velocity.wy), - radToDeg(velocity.wz), - linear_acceleration, - angular_acceleration, - run_time); - if (ret == 0) { - runtime->speed_completion_not_before_ns.store( - monotonicNowNs() + - static_cast(run_time * 1e9)); - } - } + const double angular_acc_deg = + acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg; + + const double runtime = duration > 0.0 ? duration : 0.5; + + std::lock_guard lock(mutex_); + servo_mode_.store(false); + const int ret = HRIF_SpeedL(box_id_, robot_id_, vx_mm, vy_mm, vz_mm, + wx_deg, wy_deg, wz_deg, linear_acc_mm, angular_acc_deg, runtime); if (ret != 0) { - runtime->speed_completion_not_before_ns.store(0); - runtime->motion.finish(start.token, MotionFinishMode::Clear); + busy_.store(false); return hrResult_(ret, "SpeedL"); } - admission_lock.unlock(); - return waitMotionDone_( - "SpeedL", runtime, start.token, permit, {}, nullptr, nullptr, - std::max(3000, static_cast(run_time * 1000.0) + 3000)); + const auto wait_result = waitMotionDone_("SpeedL", 60000); + busy_.store(false); + return wait_result; } -Result HuayanRobot::stopL(const std::optional acceleration) +Result HuayanRobot::stopL(std::optional acceleration = std::nullopt) { (void)acceleration; return stopMotion(); @@ -1022,113 +422,31 @@ Result HuayanRobot::stopL(const std::optional acceleration) Result HuayanRobot::stopMotion() { - if (!connected_.load()) { + if (!isConnected()) { + busy_.store(false); servo_mode_.store(false); return Result::success(); } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] stopMotion failed: runtime is unavailable"); - } - std::lock_guard termination_lock( - runtime->termination_mutex); - huayan_internal::StopRequest request; - bool stopped = false; - { - // Linearize cancellation and the vendor Stop with command submission, - // then release this lock before waiting for the old owner. The owner - // may need publishHrState_ (and therefore submission_mutex) in order to - // observe cancellation and exit. - std::lock_guard submission_lock( - runtime->submission_mutex); - request = runtime->motion.beginStop(); - if (!request.started()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] stopMotion rejected: another Stop is in progress"); - } - runtime->wait_cv.notify_all(); - stopped = terminateController_( - runtime, - kControllerStopTimeout, - runtime->program_active.load() || request.kind == MotionKind::Program); - } - const bool owner_exited = runtime->motion.waitForOwnerExit( - request.active_token, - kOwnerExitTimeout); - if (!stopped || !owner_exited || !runtime->motion.completeStop()) { - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] stopMotion failed: controller idle was not confirmed"); - } + const auto result = hrResult_(HRIF_GrpStop(box_id_, robot_id_), "GrpStop"); + busy_.store(false); servo_mode_.store(false); - return Result::success(); + return result; } Result HuayanRobot::startServoMode(const ServoOptions& options) { - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] startServoMode failed: runtime is unavailable"); - } - std::unique_lock admission_lock( - runtime->submission_mutex); - huayan_internal::SafetyPermit permit; - const auto ready = ensureMotionReady_("startServoMode", runtime, permit); + const auto ready = ensureConnected_("startServoMode"); if (!ready.ok()) { return ready; } - if (servo_mode_.load()) { - return Result::success(); - } - const auto start = runtime->motion.begin(MotionKind::Servo); - if (!start.started()) { - return motionStartFailure_("startServoMode", start.status); - } const double period = options.period > 0.0 ? options.period : 0.008; - const double lookahead = options.lookahead_time > 0.0 - ? options.lookahead_time - : 0.1; - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] startServoMode cancelled by a safety transition"); - } - ret = HRIF_StartServo(box_id_, robot_id_, period, lookahead); - } - if (ret != 0) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return hrResult_(ret, "StartServo"); - } - { - std::lock_guard submission_lock( - runtime->submission_mutex); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - (void)runtime->motion.cancelActiveForSafety(); - runtime->motion.finish(start.token, MotionFinishMode::Clear); - runtime->wait_cv.notify_all(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] startServoMode cancelled by a safety transition"); - } + const double lookahead = options.lookahead_time > 0.0 ? options.lookahead_time : 0.1; + const auto result = hrResult_(HRIF_StartServo(box_id_, robot_id_, period, lookahead), "StartServo"); + if (result.ok()) { servo_mode_.store(true); - runtime->motion.finish(start.token, MotionFinishMode::Retain); } - return Result::success(); + return result; } Result HuayanRobot::servoJ(const JointPositionCommand& target) @@ -1137,137 +455,32 @@ Result HuayanRobot::servoJ(const JointPositionCommand& target) if (!validDof_(target.position.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] servoJ failed: runtime is unavailable"); - } - std::unique_lock admission_lock( - runtime->submission_mutex); - if (!servo_mode_.load()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] servoJ failed: servo mode is not active"); - } - huayan_internal::SafetyPermit permit; - const auto ready = ensureMotionReady_("servoJ", runtime, permit); + const auto ready = ensureConnected_("servoJ"); if (!ready.ok()) { return ready; } - const auto start = runtime->motion.begin(MotionKind::Servo, true); - if (!start.started()) { - return motionStartFailure_("servoJ", start.status); - } + std::array q_deg{}; - for (std::size_t i = 0; - i < std::min(target.position.size(), q_deg.size()); - ++i) { + for (std::size_t i = 0; i < std::min(target.position.size(), q_deg.size()); ++i) { q_deg[i] = radToDeg(target.position[i]); } - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] servoJ cancelled by a safety transition"); - } - ret = HRIF_PushServoJ( - box_id_, robot_id_, - q_deg[0], q_deg[1], q_deg[2], - q_deg[3], q_deg[4], q_deg[5]); - } - if (ret != 0) { - runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); - return hrResult_(ret, "servoJ"); - } - { - std::lock_guard submission_lock( - runtime->submission_mutex); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); - runtime->wait_cv.notify_all(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] servoJ cancelled by a safety transition"); - } - runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); - } - return Result::success(); + return hrResult_(HRIF_PushServoJ(box_id_, robot_id_, + q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5]), + "servoJ"); } -Result HuayanRobot::servoL( - const CartesianPose& target, - const FrameType frame) +Result HuayanRobot::servoL(const CartesianPose& target, const FrameType frame) { - if (frame != FrameType::Base) { - return Result::failure( - ArmErrorCode::UnsupportedCommand, - "[HuayanRobot] servoL supports Base frame only"); - } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] servoL failed: runtime is unavailable"); - } - std::unique_lock admission_lock( - runtime->submission_mutex); - if (!servo_mode_.load()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] servoL failed: servo mode is not active"); - } - huayan_internal::SafetyPermit permit; - const auto ready = ensureMotionReady_("servoL", runtime, permit); + (void)frame; + const auto ready = ensureConnected_("servoL"); if (!ready.ok()) { return ready; } - const auto start = runtime->motion.begin(MotionKind::Servo, true); - if (!start.started()) { - return motionStartFailure_("servoL", start.status); - } + auto coord = poseToHrCoord(target); auto ucs = zeroHrFrame(); auto tcp = zeroHrFrame(); - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] servoL cancelled by a safety transition"); - } - ret = HRIF_PushServoP(box_id_, robot_id_, coord, ucs, tcp); - } - if (ret != 0) { - runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); - return hrResult_(ret, "servoL"); - } - { - std::lock_guard submission_lock( - runtime->submission_mutex); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); - runtime->wait_cv.notify_all(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] servoL cancelled by a safety transition"); - } - runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); - } - return Result::success(); + return hrResult_(HRIF_PushServoP(box_id_, robot_id_, coord, ucs, tcp), "servoL"); } Result HuayanRobot::servoSpeedJ(const JointVelocityCommand& velocity) @@ -1276,9 +489,7 @@ Result HuayanRobot::servoSpeedJ(const JointVelocityCommand& velocity) return unsupported_("servoSpeedJ"); } -Result HuayanRobot::servoSpeedL( - const CartesianVelocity& velocity, - const FrameType frame) +Result HuayanRobot::servoSpeedL(const CartesianVelocity& velocity, const FrameType frame) { (void)velocity; (void)frame; @@ -1287,221 +498,52 @@ Result HuayanRobot::servoSpeedL( Result HuayanRobot::stopServoMode() { + servo_mode_.store(false); return stopMotion(); } Result HuayanRobot::connect(const std::string& ip, const int port) { - if (isConnected()) { + if (connected_.load()) { return Result::success(); } if (ip.empty()) { - return Result::failure( - ArmErrorCode::InvalidArgument, - "[HuayanRobot] ip is empty"); - } - - // A dropped transport can leave the local flag, monitor and an old waiter - // alive. Cancel and join that generation before a new SDK session can be - // created; otherwise the old waiter could issue HRIF reads against it. - const auto stale_runtime = runtimeSnapshot_(); - if (stale_runtime) { - huayan_internal::SafetyCancelResult cancelled; - { - std::unique_lock termination_lock( - stale_runtime->termination_mutex); - std::lock_guard submission_lock( - stale_runtime->submission_mutex); - cancelled = stale_runtime->motion.cancelActiveForSafety(); - connected_.store(false); - stale_runtime->monitor_running.store(false); - stale_runtime->wait_cv.notify_all(); - } - const bool owner_exited = stale_runtime->motion.waitForOwnerExit( - cancelled.active_token, kOwnerExitTimeout); - if (safety_monitor_thread_.joinable()) { - safety_monitor_thread_.join(); - } - if (!owner_exited) { - stale_runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] reconnect failed: the old command owner did not exit"); - } - { - std::lock_guard sdk_lock(sdk_mutex_); - (void)HRIF_DisConnect(box_id_); - } - { - std::lock_guard state_lock(mutex_); - connected_.store(false); - servo_mode_.store(false); - runtime_.reset(); - } + return Result::failure(ArmErrorCode::InvalidArgument, "[HuayanRobot] ip is empty"); } + std::lock_guard lock(mutex_); const int use_port = port > 0 ? port : 10003; - int ret = 0; - { - std::lock_guard state_lock(mutex_); - std::lock_guard sdk_lock(sdk_mutex_); - ret = HRIF_Connect( - box_id_, ip.c_str(), static_cast(use_port)); - if (ret == 0) { - ip_ = ip; - port_ = use_port; - runtime_ = std::make_shared(); - connected_.store(true); - servo_mode_.store(false); - software_emergency_stopped_.store(false); - software_protective_stopped_.store(false); - } - } - if (ret != 0) { + const auto result = hrResult_(HRIF_Connect(box_id_, ip.c_str(), static_cast(use_port)), + "Connect"); + if (!result.ok()) { connected_.store(false); - return hrResult_(ret, "Connect"); + return result; } - - const auto runtime = runtimeSnapshot_(); - const auto initial_state = sampleHrState_(runtime); - publishHrState_(runtime, initial_state); - if (!initial_state.valid) { - { - std::lock_guard sdk_lock(sdk_mutex_); - (void)HRIF_DisConnect(box_id_); - } - std::lock_guard state_lock(mutex_); - connected_.store(false); - runtime_.reset(); - return Result::failure( - ArmErrorCode::ConnectionFailed, - "[HuayanRobot] Connect failed: initial safety state is unavailable"); - } - - // A reconnect must not replace the software generation state while an old - // controller waypoint or box-wide script can still resume. Stop both and - // require three stable idle samples before the first motion permit exists. - if (!terminateController_(runtime, kControllerStopTimeout, true)) { - { - std::lock_guard sdk_lock(sdk_mutex_); - (void)HRIF_DisConnect(box_id_); - } - std::lock_guard state_lock(mutex_); - connected_.store(false); - runtime_.reset(); - return Result::failure( - ArmErrorCode::ConnectionFailed, - "[HuayanRobot] Connect failed: stale controller work could not be cleared"); - } - - safety_monitor_thread_ = std::thread( - &HuayanRobot::safetyMonitorLoop_, this, runtime); + ip_ = ip; + port_ = use_port; + connected_.store(true); return Result::success(); } Result HuayanRobot::disconnect() { - const auto runtime = runtimeSnapshot_(); - if (!runtime && !connected_.load()) { - return Result::success(); - } - - const bool transport_connected = isConnected(); - huayan_internal::StopRequest disconnect_barrier; - bool owner_exited = true; - bool stop_confirmed = !connected_.load(); - int disconnect_ret = 0; - std::unique_lock termination_lock; - if (runtime) { - termination_lock = std::unique_lock( - runtime->termination_mutex); - } - - if (connected_.load() && runtime && transport_connected) { - const auto stop_result = stopMotion(); - if (!stop_result.ok()) { - // Keep the monitor, safety latch and runtime ownership alive. A - // failed Stop must not be hidden by throwing away local state. - return stop_result; - } - stop_confirmed = true; - } - - if (runtime) { - { - std::lock_guard submission_lock( - runtime->submission_mutex); - disconnect_barrier = runtime->motion.beginStop(); - if (!disconnect_barrier.started()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] disconnect failed: could not establish the disconnect barrier"); - } - runtime->wait_cv.notify_all(); - if (transport_connected) { - stop_confirmed = terminateController_( - runtime, - kControllerStopTimeout, - runtime->program_active.load() || - disconnect_barrier.kind == MotionKind::Program); - if (!stop_confirmed) { - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] disconnect failed: final controller Stop was not confirmed"); - } - } - std::lock_guard sdk_lock(sdk_mutex_); - if (connected_.load() || HRIF_IsConnected(box_id_)) { - disconnect_ret = HRIF_DisConnect(box_id_); - } - if (disconnect_ret != 0 && transport_connected) { - runtime->motion.failStop(); - return hrResult_(disconnect_ret, "DisConnect"); - } - connected_.store(false); - runtime->monitor_running.store(false); - } - runtime->wait_cv.notify_all(); - owner_exited = runtime->motion.waitForOwnerExit( - disconnect_barrier.active_token, kOwnerExitTimeout); - if (stop_confirmed && owner_exited) { - (void)runtime->motion.completeStop(); - } else { - runtime->motion.failStop(); - } - } else { + if (connected_.load() || HRIF_IsConnected(box_id_)) { + const auto result = hrResult_(HRIF_DisConnect(box_id_), "DisConnect"); connected_.store(false); + busy_.store(false); + servo_mode_.store(false); + return result; } - if (termination_lock.owns_lock()) { - termination_lock.unlock(); - } - if (safety_monitor_thread_.joinable()) { - safety_monitor_thread_.join(); - } - { - std::lock_guard state_lock(mutex_); - servo_mode_.store(false); - software_emergency_stopped_.store(false); - software_protective_stopped_.store(false); - runtime_.reset(); - } - if (!stop_confirmed || !owner_exited) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] disconnect completed locally, but controller Stop was not confirmed before transport loss"); - } - return hrResult_(disconnect_ret, "DisConnect"); + connected_.store(false); + busy_.store(false); + servo_mode_.store(false); + return Result::success(); } bool HuayanRobot::isConnected() const { - if (!connected_.load()) { - return false; - } - std::lock_guard sdk_lock(sdk_mutex_); - return HRIF_IsConnected(box_id_); + return connected_.load() && HRIF_IsConnected(box_id_); } Result HuayanRobot::shutdown() @@ -1509,81 +551,11 @@ Result HuayanRobot::shutdown() if (!isConnected()) { return Result::success(); } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] shutdown failed: runtime is unavailable"); + auto result = hrResult_(HRIF_ShutdownRobot(box_id_), "ShutdownRobot"); + if (!result.ok()) { + return result; } - const auto poweroff_result = torqueOff(); - if (!poweroff_result.ok()) { - return poweroff_result; - } - - std::unique_lock termination_lock( - runtime->termination_mutex); - huayan_internal::StopRequest shutdown_barrier; - int shutdown_ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - shutdown_barrier = runtime->motion.beginStop(); - if (!shutdown_barrier.started()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] shutdown failed: could not establish the shutdown barrier"); - } - runtime->wait_cv.notify_all(); - if (!terminateController_( - runtime, - kControllerStopTimeout, - runtime->program_active.load() || - shutdown_barrier.kind == MotionKind::Program)) { - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] shutdown failed: final controller Stop was not confirmed"); - } - std::lock_guard sdk_lock(sdk_mutex_); - shutdown_ret = HRIF_ShutdownRobot(box_id_); - if (shutdown_ret == 0) { - connected_.store(false); - runtime->monitor_running.store(false); - } - } - if (shutdown_ret != 0) { - runtime->motion.failStop(); - return hrResult_(shutdown_ret, "ShutdownRobot"); - } - runtime->wait_cv.notify_all(); - const bool owner_exited = runtime->motion.waitForOwnerExit( - shutdown_barrier.active_token, kOwnerExitTimeout); - if (owner_exited) { - (void)runtime->motion.completeStop(); - } else { - runtime->motion.failStop(); - } - termination_lock.unlock(); - if (safety_monitor_thread_.joinable()) { - safety_monitor_thread_.join(); - } - { - std::lock_guard sdk_lock(sdk_mutex_); - (void)HRIF_DisConnect(box_id_); - } - { - std::lock_guard state_lock(mutex_); - servo_mode_.store(false); - software_emergency_stopped_.store(false); - software_protective_stopped_.store(false); - runtime_.reset(); - } - if (!owner_exited) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] shutdown succeeded, but a raced command owner did not exit cleanly"); - } - return Result::success(); + return disconnect(); } Result HuayanRobot::clearFault() @@ -1592,169 +564,22 @@ Result HuayanRobot::clearFault() if (!ready.ok()) { return ready; } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] clearFault failed: runtime is unavailable"); - } - const auto state = sampleHrState_(runtime); - publishHrState_(runtime, state); - const auto safety = runtime->safety.snapshot(); - if (safety.latched) { - if (safety.latched_reason == SafetyCondition::RobotFault || - safety.latched_reason == SafetyCondition::Unknown) { - return completeSafetyRecovery_( - "clearFault", - runtime, - safety.epoch, - false, - false); - } - // Resetting a controller fault is useful after a physical E-stop, but - // this API deliberately does not clear that differently typed latch. - int reset_ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - reset_ret = HRIF_GrpReset(box_id_, robot_id_); - } - return hrResult_(reset_ret, "GrpReset"); - } - int ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - ret = HRIF_GrpReset(box_id_, robot_id_); - } - return hrResult_(ret, "GrpReset"); -} - -Result HuayanRobot::unlockProtectiveStop() -{ - const auto ready = ensureConnected_("unlockProtectiveStop"); - if (!ready.ok()) { - return ready; - } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] unlockProtectiveStop failed: runtime is unavailable"); - } - const auto state = sampleHrState_(runtime); - publishHrState_(runtime, state); - const auto safety = runtime->safety.snapshot(); - if (!safety.latched) { - return Result::success(); - } - if (!isProtectiveCondition(safety.latched_reason) && - !software_protective_stopped_.load()) { - return Result::failure( - ArmErrorCode::RobotInEmergencyStop, - "[HuayanRobot] unlockProtectiveStop rejected: the latched stop is not protective"); - } - return completeSafetyRecovery_( - "unlockProtectiveStop", - runtime, - safety.epoch, - false, - software_protective_stopped_.load()); + return hrResult_(HRIF_GrpReset(box_id_, robot_id_), "GrpReset"); } Result HuayanRobot::loadProgram(const std::string& program_name) { - if (program_name.empty()) { - return Result::failure( - ArmErrorCode::InvalidArgument, - "[HuayanRobot] loadProgram failed: program name is empty"); - } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] loadProgram failed: runtime is unavailable"); - } - std::unique_lock admission_lock( - runtime->submission_mutex); - huayan_internal::SafetyPermit permit; - const auto ready = ensureMotionReady_("loadProgram", runtime, permit); - if (!ready.ok()) { - return ready; - } - const auto start = runtime->motion.begin(MotionKind::Program); - if (!start.started()) { - return motionStartFailure_("loadProgram", start.status); - } - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] loadProgram cancelled by a safety transition"); - } - ret = HRIF_SwitchScript(box_id_, robot_id_, program_name); - runtime->motion.finish(start.token, MotionFinishMode::Clear); - } - return hrResult_(ret, "SwitchScript"); + (void)program_name; + return Result::success(); } Result HuayanRobot::playProgram() { - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] playProgram failed: runtime is unavailable"); - } - std::unique_lock admission_lock( - runtime->submission_mutex); - huayan_internal::SafetyPermit permit; - const auto ready = ensureMotionReady_("playProgram", runtime, permit); + const auto ready = ensureConnected_("playProgram"); if (!ready.ok()) { return ready; } - const auto start = runtime->motion.begin(MotionKind::Program); - if (!start.started()) { - return motionStartFailure_("playProgram", start.status); - } - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] playProgram cancelled by a safety transition"); - } - ret = HRIF_StartScript(box_id_); - } - if (ret != 0) { - runtime->motion.finish(start.token, MotionFinishMode::Clear); - return hrResult_(ret, "StartScript"); - } - { - std::lock_guard submission_lock( - runtime->submission_mutex); - if (runtime->motion.cancelled(start.token) || - !runtime->safety.validate(permit)) { - (void)runtime->motion.cancelActiveForSafety(); - runtime->motion.finish(start.token, MotionFinishMode::Clear); - runtime->wait_cv.notify_all(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] playProgram cancelled by a safety transition"); - } - runtime->program_active.store(true); - runtime->motion.finish(start.token, MotionFinishMode::Retain); - } - return Result::success(); + return hrResult_(HRIF_StartScript(box_id_), "StartScript"); } Result HuayanRobot::pauseProgram() @@ -1763,38 +588,7 @@ Result HuayanRobot::pauseProgram() if (!ready.ok()) { return ready; } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] pauseProgram failed: runtime is unavailable"); - } - std::unique_lock admission_lock( - runtime->submission_mutex); - if (!runtime || !runtime->program_active.load()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] pauseProgram failed: no tracked program is active"); - } - const auto safety = runtime->safety.tryPermit(); - if (!safety) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] pauseProgram rejected by the safety latch"); - } - int ret = 0; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - std::lock_guard sdk_lock(sdk_mutex_); - if (!runtime->safety.validate(*safety)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] pauseProgram cancelled by a safety transition"); - } - ret = HRIF_PauseScript(box_id_); - } - return hrResult_(ret, "PauseScript"); + return hrResult_(HRIF_PauseScript(box_id_), "PauseScript"); } Result HuayanRobot::stopProgram() @@ -1803,45 +597,12 @@ Result HuayanRobot::stopProgram() if (!ready.ok()) { return ready; } - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] stopProgram failed: runtime is unavailable"); - } - std::lock_guard termination_lock( - runtime->termination_mutex); - huayan_internal::StopRequest request; - bool stopped = false; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - request = runtime->motion.beginStop(MotionKind::Program); - if (!request.started()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] stopProgram rejected: another Stop is in progress"); - } - runtime->wait_cv.notify_all(); - stopped = terminateController_( - runtime, kControllerStopTimeout, true); - } - const bool owner_exited = runtime->motion.waitForOwnerExit( - request.active_token, kOwnerExitTimeout); - if (!stopped || !owner_exited || !runtime->motion.completeStop()) { - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] stopProgram failed: controller idle was not confirmed"); - } - servo_mode_.store(false); - return Result::success(); + return hrResult_(HRIF_StopScript(box_id_), "StopScript"); } -std::vector HuayanRobot::ik( - const std::string& base_link, - const std::string& ee_link, - const CartesianPose& pose) +std::vector HuayanRobot::ik(const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) { (void)base_link; (void)ee_link; @@ -1850,9 +611,7 @@ std::vector HuayanRobot::ik( return {}; } -CartesianPose HuayanRobot::fk( - const std::string& base_link, - const std::string& ee_link) +CartesianPose HuayanRobot::fk(const std::string& base_link, const std::string& ee_link) { (void)base_link; (void)ee_link; @@ -1870,163 +629,50 @@ CartesianVelocity HuayanRobot::getSpeedLCommandTwistBase() const return readTcpVelocity_(); } -bool HuayanRobot::busy() const -{ - const auto runtime = runtimeSnapshot_(); - return runtime && runtime->motion.busy(); -} - Result HuayanRobot::ensureConnected_(const std::string& context) const { if (!isConnected()) { - return Result::failure( - ArmErrorCode::NotConnected, - "[HuayanRobot] " + context + " failed: arm is not connected"); + return Result::failure(ArmErrorCode::NotConnected, + "[HuayanRobot] " + context + " failed: arm is not connected"); } return Result::success(); } -Result HuayanRobot::ensureMotionReady_( - const std::string& context, - const std::shared_ptr& runtime, - huayan_internal::SafetyPermit& permit) const -{ - const auto connected = ensureConnected_(context); - if (!connected.ok()) { - return connected; - } - if (!runtime) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] " + context + " failed: runtime is unavailable"); - } - if (runtimeSnapshot_().get() != runtime.get()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] " + context + - " rejected: the controller session changed before admission"); - } - const auto state = sampleHrState_(runtime); - publishHrState_(runtime, state); - if (!state.valid) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] " + context + - " failed: current safety state is unavailable"); - } - - const auto safety = runtime->safety.snapshot(); - if (safety.latched || - !huayan_internal::isMotionSafe(safety.observed)) { - const auto reason = safety.latched - ? safety.latched_reason - : safety.observed; - ArmErrorCode code = ArmErrorCode::CommandRejected; - if (isEmergencyCondition(reason)) { - code = ArmErrorCode::RobotInEmergencyStop; - } else if (isProtectiveCondition(reason)) { - code = ArmErrorCode::RobotInProtectiveStop; - } else if (safetyModeFromCondition(reason) == SafetyMode::Fault) { - code = ArmErrorCode::RobotInFault; - } - return Result::failure( - code, - "[HuayanRobot] " + context + - " rejected: safety event is latched; explicit recovery is required"); - } - if (state.error != 0) { - return Result::failure( - ArmErrorCode::RobotInFault, - "[HuayanRobot] " + context + " failed: robot error, code=" + - std::to_string(state.error_code)); - } - if (state.electrified == 0 || state.enabled == 0) { - return Result::failure( - ArmErrorCode::RobotNotPowered, - "[HuayanRobot] " + context + " failed: robot is not enabled"); - } - const auto maybe_permit = runtime->safety.tryPermit(); - if (!maybe_permit) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] " + context + " rejected by the safety state"); - } - permit = *maybe_permit; - return Result::success(); -} - Result HuayanRobot::unsupported_(const std::string& name) const { - const std::string message = - "[HuayanRobot] " + name + " is not implemented"; + const std::string message = "[HuayanRobot] " + name + " is not implemented"; CMVR_LOG(ERROR) << message; return Result::failure(ArmErrorCode::UnsupportedCommand, message); } -Result HuayanRobot::hrResult_( - const int code, - const std::string& context) const +Result HuayanRobot::hrResult_(const int code, const std::string& context) const { if (code == 0) { return Result::success(); } std::string sdk_message; - { - std::lock_guard sdk_lock(sdk_mutex_); - (void)HRIF_GetErrorCodeStr(box_id_, code, sdk_message); - } - std::ostringstream message; - message << "[HuayanRobot] " << context << " failed, code=" << code; + (void)HRIF_GetErrorCodeStr(box_id_, code, sdk_message); + std::ostringstream oss; + oss << "[HuayanRobot] " << context << " failed, code=" << code; if (!sdk_message.empty()) { - message << ", message=" << sdk_message; + oss << ", message=" << sdk_message; } - CMVR_LOG(ERROR) << message.str(); - return Result::failure(ArmErrorCode::CommandFailed, message.str()); + const auto message = oss.str(); + CMVR_LOG(ERROR) << message; + return Result::failure(ArmErrorCode::CommandFailed, message); } -Result HuayanRobot::motionStartFailure_( - const std::string& context, - const MotionStartStatus status) const -{ - ArmErrorCode code = ArmErrorCode::RobotNotReady; - std::string reason = "arm is busy"; - switch (status) { - case MotionStartStatus::Stopping: - reason = "a Stop operation is in progress"; - break; - case MotionStartStatus::Blocked: - code = ArmErrorCode::CommandRejected; - reason = "controller ownership is blocked until explicit recovery"; - break; - case MotionStartStatus::Invalid: - code = ArmErrorCode::InvalidArgument; - reason = "invalid motion kind"; - break; - case MotionStartStatus::Busy: - break; - case MotionStartStatus::Started: - return Result::success(); - } - return Result::failure( - code, - "[HuayanRobot] " + context + " rejected: " + reason); -} - -bool HuayanRobot::validDof_( - const std::size_t size, - std::string& error) const +bool HuayanRobot::validDof_(const std::size_t size, std::string& error) const { if (size != model_.dof) { - error = "[HuayanRobot] command dof mismatch, expected=" + - std::to_string(model_.dof) + ", actual=" + - std::to_string(size); + error = "[HuayanRobot] command dof mismatch, expected=" + std::to_string(model_.dof) + + ", actual=" + std::to_string(size); CMVR_LOG(ERROR) << error; return false; } if (model_.dof > 6) { - error = "[HuayanRobot] command dof exceeds SDK limit: " + - std::to_string(model_.dof); + error = "[HuayanRobot] command dof exceeds SDK limit: " + std::to_string(model_.dof); CMVR_LOG(ERROR) << error; return false; } @@ -2034,142 +680,132 @@ bool HuayanRobot::validDof_( } HuayanRobot::HrState HuayanRobot::readHrState_() const -{ - const auto runtime = runtimeSnapshot_(); - if (!runtime) { - return {}; - } - const auto state = sampleHrState_(runtime); - publishHrState_(runtime, state); - return state; -} - -HuayanRobot::HrState HuayanRobot::sampleHrState_( - const std::shared_ptr& runtime) const { HrState state; - if (!runtime || !connected_.load()) { + if (!isConnected()) { return state; } - int state_ret = -1; - int safety_ret = -1; - { - std::lock_guard sdk_lock(sdk_mutex_); - if (!HRIF_IsConnected(box_id_)) { - return state; - } - state_ret = HRIF_ReadRobotState( - box_id_, robot_id_, - state.moving, - state.enabled, - state.error, - state.error_code, - state.error_axis, - state.brake, - state.paused, - state.emergency_stop, - state.safeguard, - state.electrified, - state.connected_to_box, - state.blending_done, - state.in_pos); - safety_ret = HRIF_ReadEmergencyInfo( - box_id_, robot_id_, - state.emergency_signal_fault, - state.emergency_input, - state.safeguard_signal_fault, - state.safeguard_input); + const int ret = HRIF_ReadRobotState(box_id_, robot_id_, + state.moving, + state.enabled, + state.error, + state.error_code, + state.error_axis, + state.brake, + state.paused, + state.emergency_stop, + state.safeguard, + state.electrified, + state.connected_to_box, + state.blending_done, + state.in_pos); + state.valid = ret == 0; + if (ret != 0) { + CMVR_LOG(ERROR) << "[HuayanRobot] read robot state failed, code=" << ret; } - state.valid = state_ret == 0 && safety_ret == 0 && - state.connected_to_box != 0; return state; } -void HuayanRobot::publishHrState_( - const std::shared_ptr& runtime, - const HrState& state) const -{ - if (!runtime) { - return; - } - std::lock_guard submission_lock( - runtime->submission_mutex); - const auto before = runtime->safety.snapshot(); - huayan_internal::RawSafetyState raw; - raw.valid = state.valid; - raw.emergency_signal_fault = state.emergency_signal_fault; - raw.emergency_stop = state.emergency_stop != 0 || - state.emergency_input != 0; - raw.safeguard_signal_fault = state.safeguard_signal_fault; - raw.safeguard_stop = state.safeguard != 0 || - state.safeguard_input != 0; - raw.robot_fault = state.error; - raw.software_emergency_stop = software_emergency_stopped_.load(); - raw.software_protective_stop = software_protective_stopped_.load(); - runtime->safety.observe(raw); - const auto after = runtime->safety.snapshot(); - if (state.valid) { - runtime->last_valid_sample_ns.store(monotonicNowNs()); - } - if (after.latched && - (!before.latched || after.epoch != before.epoch || - after.observed != before.observed)) { - runtime->termination_confirmed.store(false); - (void)runtime->motion.cancelActiveForSafety(); - runtime->wait_cv.notify_all(); - } -} - std::vector HuayanRobot::readJointPositionRad_() const { - std::vector values; - if (!readJointPositionSample_(values)) { - values.assign(model_.dof, 0.0); + std::vector q(model_.dof, 0.0); + if (!isConnected()) { + return q; } - return values; + + double j1 = 0.0; + double j2 = 0.0; + double j3 = 0.0; + double j4 = 0.0; + double j5 = 0.0; + double j6 = 0.0; + const int ret = HRIF_ReadActJointPos(box_id_, robot_id_, j1, j2, j3, j4, j5, j6); + if (ret != 0) { + CMVR_LOG(ERROR) << "[HuayanRobot] read joint position failed, code=" << ret; + return q; + } + + const std::array values{j1, j2, j3, j4, j5, j6}; + for (std::size_t i = 0; i < std::min(q.size(), values.size()); ++i) { + q[i] = degToRad(values[i]); + } + return q; } std::vector HuayanRobot::readJointVelocityRad_() const { - std::vector values; - if (!readJointVelocitySample_(values)) { - values.assign(model_.dof, 0.0); + std::vector qd(model_.dof, 0.0); + if (!isConnected()) { + return qd; } - return values; + + double j1 = 0.0; + double j2 = 0.0; + double j3 = 0.0; + double j4 = 0.0; + double j5 = 0.0; + double j6 = 0.0; + const int ret = HRIF_ReadActJointVel(box_id_, robot_id_, j1, j2, j3, j4, j5, j6); + if (ret != 0) { + CMVR_LOG(ERROR) << "[HuayanRobot] read joint velocity failed, code=" << ret; + return qd; + } + + const std::array values{j1, j2, j3, j4, j5, j6}; + for (std::size_t i = 0; i < std::min(qd.size(), values.size()); ++i) { + qd[i] = degToRad(values[i]); + } + return qd; } CartesianPose HuayanRobot::readTcpPose_() const { CartesianPose pose; - (void)readTcpPoseSample_(pose); - return pose; -} - -CartesianVelocity HuayanRobot::readTcpVelocity_() const -{ - CartesianVelocity velocity; - if (!connected_.load()) { - return velocity; + if (!isConnected()) { + return pose; } + double x = 0.0; double y = 0.0; double z = 0.0; double rx = 0.0; double ry = 0.0; double rz = 0.0; - int ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - if (!HRIF_IsConnected(box_id_)) { - return velocity; - } - ret = HRIF_ReadActTcpVel( - box_id_, robot_id_, x, y, z, rx, ry, rz); - } + const int ret = HRIF_ReadActTcpPos(box_id_, robot_id_, x, y, z, rx, ry, rz); if (ret != 0) { + CMVR_LOG(ERROR) << "[HuayanRobot] read tcp pose failed, code=" << ret; + return pose; + } + + pose.x = mmToMeters(x); + pose.y = mmToMeters(y); + pose.z = mmToMeters(z); + pose.rx = degToRad(rx); + pose.ry = degToRad(ry); + pose.rz = degToRad(rz); + return pose; +} + +CartesianVelocity HuayanRobot::readTcpVelocity_() const +{ + CartesianVelocity velocity; + if (!isConnected()) { return velocity; } + + double x = 0.0; + double y = 0.0; + double z = 0.0; + double rx = 0.0; + double ry = 0.0; + double rz = 0.0; + const int ret = HRIF_ReadActTcpVel(box_id_, robot_id_, x, y, z, rx, ry, rz); + if (ret != 0) { + CMVR_LOG(ERROR) << "[HuayanRobot] read tcp velocity failed, code=" << ret; + return velocity; + } + velocity.vx = mmToMeters(x); velocity.vy = mmToMeters(y); velocity.vz = mmToMeters(z); @@ -2179,114 +815,14 @@ CartesianVelocity HuayanRobot::readTcpVelocity_() const return velocity; } -bool HuayanRobot::readJointPositionSample_( - std::vector& values) const -{ - values.assign(model_.dof, 0.0); - if (!connected_.load()) { - return false; - } - double j1 = 0.0; - double j2 = 0.0; - double j3 = 0.0; - double j4 = 0.0; - double j5 = 0.0; - double j6 = 0.0; - int ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - if (!HRIF_IsConnected(box_id_)) { - return false; - } - ret = HRIF_ReadActJointPos( - box_id_, robot_id_, j1, j2, j3, j4, j5, j6); - } - if (ret != 0) { - return false; - } - const std::array sample{j1, j2, j3, j4, j5, j6}; - for (std::size_t i = 0; - i < std::min(values.size(), sample.size()); - ++i) { - values[i] = degToRad(sample[i]); - } - return true; -} - -bool HuayanRobot::readJointVelocitySample_( - std::vector& values) const -{ - values.assign(model_.dof, 0.0); - if (!connected_.load()) { - return false; - } - double j1 = 0.0; - double j2 = 0.0; - double j3 = 0.0; - double j4 = 0.0; - double j5 = 0.0; - double j6 = 0.0; - int ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - if (!HRIF_IsConnected(box_id_)) { - return false; - } - ret = HRIF_ReadActJointVel( - box_id_, robot_id_, j1, j2, j3, j4, j5, j6); - } - if (ret != 0) { - return false; - } - const std::array sample{j1, j2, j3, j4, j5, j6}; - for (std::size_t i = 0; - i < std::min(values.size(), sample.size()); - ++i) { - values[i] = degToRad(sample[i]); - } - return true; -} - -bool HuayanRobot::readTcpPoseSample_(CartesianPose& pose) const -{ - pose = {}; - if (!connected_.load()) { - return false; - } - double x = 0.0; - double y = 0.0; - double z = 0.0; - double rx = 0.0; - double ry = 0.0; - double rz = 0.0; - int ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - if (!HRIF_IsConnected(box_id_)) { - return false; - } - ret = HRIF_ReadActTcpPos( - box_id_, robot_id_, x, y, z, rx, ry, rz); - } - if (ret != 0) { - return false; - } - pose.x = mmToMeters(x); - pose.y = mmToMeters(y); - pose.z = mmToMeters(z); - pose.rx = degToRad(rx); - pose.ry = degToRad(ry); - pose.rz = degToRad(rz); - return true; -} - std::vector HuayanRobot::currentJointPositionDeg_() const { - auto values = readJointPositionRad_(); - for (auto& value : values) { - value = radToDeg(value); + const auto q_rad = readJointPositionRad_(); + std::vector q_deg(q_rad.size(), 0.0); + for (std::size_t i = 0; i < q_rad.size(); ++i) { + q_deg[i] = radToDeg(q_rad[i]); } - return values; + return q_deg; } std::string HuayanRobot::nextCommandId_() const @@ -2294,431 +830,56 @@ std::string HuayanRobot::nextCommandId_() const return id_ + "_" + std::to_string(++command_seq_); } -Result HuayanRobot::waitMotionDone_( - const std::string& context, - const std::shared_ptr& runtime, - const huayan_internal::MotionToken motion_token, - const huayan_internal::SafetyPermit safety_permit, - const std::string& command_id, - const std::vector* joint_target, - const CartesianPose* tcp_target, - const int timeout_ms, - const std::function& cancellation_requested) const +Result HuayanRobot::waitMotionDone_(const std::string& context, const int timeout_ms) const { - const auto started_at = std::chrono::steady_clock::now(); - bool saw_motion = false; - bool saw_command_id = false; - int stable_completion_samples = 0; - - while (runtime->monitor_running.load()) { - const bool caller_cancelled = - cancellationRequested(cancellation_requested); - if (caller_cancelled || - runtime->motion.cancelled(motion_token) || - !runtime->safety.validate(safety_permit)) { - runtime->motion.finish( - motion_token, - caller_cancelled - ? MotionFinishMode::Retain - : MotionFinishMode::Clear); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] " + context + - " cancelled by its caller, Stop, or a safety transition"); - } - + const auto start = std::chrono::steady_clock::now(); + while (true) { bool done = false; - std::string current_command_id; - int done_ret = 0; - int id_ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - done_ret = HRIF_IsMotionDone(box_id_, robot_id_, done); - if (!command_id.empty()) { - id_ret = HRIF_ReadCurWaypointID( - box_id_, robot_id_, current_command_id); + const int ret = HRIF_IsMotionDone(box_id_, robot_id_, done); + if (ret != 0) { + return hrResult_(ret, "IsMotionDone(" + context + ")"); + } + const auto state = readHrState_(); + if (state.valid) { + if (state.error != 0) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] " + context + " failed: robot error, code=" + + std::to_string(state.error_code)); + } + + if (state.emergency_stop != 0) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] " + context + " failed: emergency stop"); + } + + if (state.safeguard != 0) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] " + context + " failed: safeguard stop"); } } - if (done_ret != 0 || id_ret != 0) { - const bool stopped = terminateController_( - runtime, kControllerStopTimeout, false); - if (stopped) { - runtime->motion.finish(motion_token, MotionFinishMode::Clear); - } else { - runtime->motion.failMotion(motion_token); - } - return hrResult_( - done_ret != 0 ? done_ret : id_ret, - done_ret != 0 - ? "IsMotionDone(" + context + ")" - : "ReadCurWaypointID(" + context + ")"); - } - const auto state = sampleHrState_(runtime); - publishHrState_(runtime, state); - if (!state.valid) { - continue; - } - saw_motion = saw_motion || state.moving != 0 || !done; - saw_command_id = saw_command_id || - (!command_id.empty() && current_command_id == command_id); - - if (state.error != 0 || state.emergency_stop != 0 || - state.safeguard != 0 || state.emergency_input != 0 || - state.safeguard_input != 0) { - continue; + if (done) { + return Result::success(); } const auto elapsed = std::chrono::duration_cast( - std::chrono::steady_clock::now() - started_at); - const bool correlated = command_id.empty() - ? (saw_motion || - monotonicNowNs() >= - runtime->speed_completion_not_before_ns.load()) - : (saw_motion || saw_command_id || - elapsed >= kCompletionCorrelationGrace); - const bool at_target = targetReached_(joint_target, tcp_target); - if (done && state.moving == 0 && correlated && at_target) { - ++stable_completion_samples; - if (stable_completion_samples >= 2) { - runtime->motion.finish(motion_token, MotionFinishMode::Clear); - return Result::success(); - } - } else { - stable_completion_samples = 0; - } + std::chrono::steady_clock::now() - start).count(); - if (elapsed.count() > timeout_ms) { - const bool stopped = terminateController_( - runtime, kControllerStopTimeout, false); - if (stopped) { - runtime->motion.finish(motion_token, MotionFinishMode::Clear); - } else { - runtime->motion.failMotion(motion_token); + if (elapsed > timeout_ms) { + { + std::lock_guard lock(mutex_); + (void)HRIF_GrpStop(box_id_, robot_id_); } + return Result::failure( - ArmErrorCode::Timeout, + ArmErrorCode::CommandFailed, "[HuayanRobot] " + context + " timeout"); } - std::unique_lock wait_lock(runtime->wait_mutex); - runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); - } - - runtime->motion.failMotion(motion_token); - return Result::failure( - ArmErrorCode::NotConnected, - "[HuayanRobot] " + context + " cancelled by disconnect"); -} - -bool HuayanRobot::targetReached_( - const std::vector* joint_target, - const CartesianPose* tcp_target) const -{ - if (joint_target) { - std::vector current; - if (!readJointPositionSample_(current) || - current.size() != joint_target->size()) { - return false; - } - for (std::size_t i = 0; i < current.size(); ++i) { - if (angularDistance(current[i], (*joint_target)[i]) > - kJointTargetToleranceRad) { - return false; - } - } - } - if (tcp_target) { - CartesianPose current; - if (!readTcpPoseSample_(current)) { - return false; - } - if (std::abs(current.x - tcp_target->x) > kTcpPositionToleranceM || - std::abs(current.y - tcp_target->y) > kTcpPositionToleranceM || - std::abs(current.z - tcp_target->z) > kTcpPositionToleranceM || - angularDistance(current.rx, tcp_target->rx) > kTcpRotationToleranceRad || - angularDistance(current.ry, tcp_target->ry) > kTcpRotationToleranceRad || - angularDistance(current.rz, tcp_target->rz) > kTcpRotationToleranceRad) { - return false; - } - } - return true; -} - -bool HuayanRobot::controllerIdleStable_( - const std::shared_ptr& runtime, - const std::chrono::milliseconds timeout) const -{ - const auto deadline = std::chrono::steady_clock::now() + timeout; - int stable_samples = 0; - while (std::chrono::steady_clock::now() < deadline) { - const auto state = sampleHrState_(runtime); - publishHrState_(runtime, state); - bool done = false; - int done_ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - done_ret = HRIF_IsMotionDone(box_id_, robot_id_, done); - } - std::vector velocity; - const bool velocity_valid = readJointVelocitySample_(velocity); - const bool velocity_zero = velocity_valid && std::all_of( - velocity.begin(), velocity.end(), - [](const double value) { - return std::abs(value) <= kIdleVelocityToleranceRad; - }); - if (state.valid && done_ret == 0 && done && - state.moving == 0 && velocity_zero) { - ++stable_samples; - if (stable_samples >= 3) { - return true; - } - } else { - stable_samples = 0; - } - std::unique_lock wait_lock(runtime->wait_mutex); - runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); - } - return false; -} - -bool HuayanRobot::terminateController_( - const std::shared_ptr& runtime, - const std::chrono::milliseconds timeout, - const bool stop_program) const -{ - std::lock_guard termination_lock( - runtime->termination_mutex); - std::lock_guard submission_lock( - runtime->submission_mutex); - runtime->termination_confirmed.store(false); - int stop_ret = 0; - int script_ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - stop_ret = HRIF_GrpStop(box_id_, robot_id_); - if (stop_program) { - script_ret = HRIF_StopScript(box_id_); - } - } - if (stop_ret != 0 || script_ret != 0) { - (void)hrResult_(stop_ret != 0 ? stop_ret : script_ret, - stop_ret != 0 ? "GrpStop" : "StopScript"); - return false; - } - if (stop_program) { - runtime->program_active.store(false); - } - servo_mode_.store(false); - const bool idle = controllerIdleStable_(runtime, timeout); - runtime->termination_confirmed.store(idle); - return idle; -} - -Result HuayanRobot::completeSafetyRecovery_( - const std::string& context, - const std::shared_ptr& runtime, - const std::uint64_t expected_epoch, - const bool enable_robot, - const bool release_software_guard) -{ - if (!runtime || expected_epoch == 0 || - runtime->safety.snapshot().epoch != expected_epoch) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] " + context + - " rejected: safety event changed before recovery"); - } - std::lock_guard termination_lock( - runtime->termination_mutex); - - if (release_software_guard) { - int ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - ret = HRIF_EnterSafetyGuard(box_id_, robot_id_, 0); - } - const auto result = hrResult_(ret, "ExitSafetyGuard"); - if (!result.ok()) { - return result; - } - } - software_protective_stopped_.store(false); - software_emergency_stopped_.store(false); - - auto state = sampleHrState_(runtime); - publishHrState_(runtime, state); - if (!state.valid || state.emergency_signal_fault != 0 || - state.emergency_input != 0 || state.emergency_stop != 0 || - state.safeguard_signal_fault != 0 || state.safeguard_input != 0 || - state.safeguard != 0) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] " + context + - " rejected: hardware safety input is still active or unreadable"); - } - if (runtime->safety.snapshot().epoch != expected_epoch) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] " + context + - " rejected: a newer safety event was observed"); - } - - huayan_internal::StopRequest stop_request; - bool stopped = false; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - stop_request = runtime->motion.beginStop(); - if (!stop_request.started()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] " + context + - " rejected: another Stop operation is in progress"); - } - runtime->wait_cv.notify_all(); - stopped = terminateController_( - runtime, - kControllerStopTimeout, - runtime->program_active.load() || - stop_request.kind == MotionKind::Program); - } - const bool owner_exited = runtime->motion.waitForOwnerExit( - stop_request.active_token, - kOwnerExitTimeout); - if (!stopped || !owner_exited) { - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] " + context + - " failed: controller termination was not confirmed"); - } - - int reset_ret = 0; - int enable_ret = 0; - { - std::lock_guard sdk_lock(sdk_mutex_); - reset_ret = HRIF_GrpReset(box_id_, robot_id_); - if (reset_ret == 0 && enable_robot) { - enable_ret = HRIF_GrpEnable(box_id_, robot_id_); - } - } - if (reset_ret != 0 || enable_ret != 0) { - runtime->motion.failStop(); - return hrResult_( - reset_ret != 0 ? reset_ret : enable_ret, - reset_ret != 0 ? "GrpReset" : "GrpEnable"); - } - - const auto recovery_deadline = - std::chrono::steady_clock::now() + kControllerStopTimeout; - bool robot_ready = false; - while (std::chrono::steady_clock::now() < recovery_deadline) { - state = sampleHrState_(runtime); - publishHrState_(runtime, state); - const auto safety = runtime->safety.snapshot(); - if (safety.epoch != expected_epoch) { - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] " + context + - " cancelled by a newer safety event"); - } - robot_ready = state.valid && state.error == 0 && - state.emergency_stop == 0 && state.emergency_input == 0 && - state.safeguard == 0 && state.safeguard_input == 0 && - (!enable_robot || - (state.enabled != 0 && state.electrified != 0)); - if (robot_ready && - huayan_internal::isMotionSafe(safety.observed)) { - break; - } - std::unique_lock wait_lock(runtime->wait_mutex); - runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); - } - if (!robot_ready) { - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::RobotNotReady, - "[HuayanRobot] " + context + - " failed: safe robot state was not confirmed after reset"); - } - - const auto recovery = runtime->safety.beginRecovery(expected_epoch); - if (!recovery) { - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] " + context + - " rejected: safety recovery epoch is no longer valid"); - } - const bool idle = controllerIdleStable_(runtime, kControllerStopTimeout); - if (!idle || !runtime->motion.completeStop()) { - runtime->safety.failRecovery(*recovery); - runtime->motion.failStop(); - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] " + context + - " failed: stable controller idle was not confirmed"); - } - if (!runtime->safety.completeRecovery( - *recovery, - robot_ready, - idle, - runtime->termination_confirmed.load())) { - (void)runtime->motion.cancelActiveForSafety(); - return Result::failure( - ArmErrorCode::CommandRejected, - "[HuayanRobot] " + context + - " cancelled by a safety transition during recovery"); - } - return Result::success(); -} - -std::shared_ptr -HuayanRobot::runtimeSnapshot_() const -{ - std::lock_guard state_lock(mutex_); - return runtime_; -} - -void HuayanRobot::safetyMonitorLoop_( - const std::shared_ptr& runtime) -{ - while (runtime->monitor_running.load()) { - const auto state = sampleHrState_(runtime); - publishHrState_(runtime, state); - const auto safety = runtime->safety.snapshot(); - if (safety.latched && - (!runtime->termination_confirmed.load() || - !state.valid || state.moving != 0 || - runtime->program_active.load())) { - std::lock_guard termination_lock( - runtime->termination_mutex); - huayan_internal::SafetyCancelResult cancelled; - bool stopped = false; - { - std::lock_guard submission_lock( - runtime->submission_mutex); - cancelled = runtime->motion.cancelActiveForSafety(); - stopped = terminateController_( - runtime, - std::chrono::milliseconds(1000), - runtime->program_active.load() || - cancelled.kind == MotionKind::Program); - } - const bool owner_exited = runtime->motion.waitForOwnerExit( - cancelled.active_token, - std::chrono::milliseconds(1000)); - runtime->termination_confirmed.store(stopped && owner_exited); - servo_mode_.store(false); - } - - std::unique_lock wait_lock(runtime->wait_mutex); - runtime->wait_cv.wait_for( - wait_lock, - kSafetyPollPeriod, - [runtime]() { return !runtime->monitor_running.load(); }); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); } } diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 01c5cd75..5db631fa 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -9,18 +9,12 @@ #define CMVR_ES_HUAYAN_ROBOT_H #include -#include -#include -#include #include #include -#include #include -#include #include #include "cmvr/config/arm_config/arm_config.pb.h" -#include "devices/arm/huayan_arm/huayan_lifecycle_state.h" #include "devices/arm/robot_arm.h" namespace cmvr::device { @@ -41,14 +35,13 @@ public: CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override; - ControlMode getControlMode() const override; - bool supportsActionQueueMotion() const noexcept override { return true; } + ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; } Result torqueOn() override; Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; - Result protectiveStop() override; + Result protectiveStop() override { return emergencyStop(); } Result recoverProtectiveStop( const JointTrajectory&, const MotionOptions&) override @@ -58,7 +51,7 @@ public: "protective recovery is not implemented for HuayanRobot"); } Result setSpeedScaling(double scaling) override; - double getSpeedScaling() const override { return speed_scaling_.load(); } + double getSpeedScaling() const override { return speed_scaling_; } bool isProtectiveStopped() const override; bool isEmergencyStopped() const override; bool isFault() const override; @@ -86,7 +79,7 @@ public: Result brakeRelease() override { return torqueOn(); } Result shutdown() override; Result clearFault() override; - Result unlockProtectiveStop() override; + Result unlockProtectiveStop() override { return clearFault(); } Result loadProgram(const std::string& program_name) override; Result playProgram() override; Result pauseProgram() override; @@ -99,7 +92,7 @@ public: CartesianPose fk(const std::string& base_link, const std::string& ee_link) override; CartesianPose fk(bool is_tcp = true) override; CartesianVelocity getSpeedLCommandTwistBase() const override; - bool busy() const override; + bool busy() const override { return busy_.load(); } private: struct HrState { @@ -116,69 +109,21 @@ private: int connected_to_box{0}; int blending_done{0}; int in_pos{0}; - int emergency_signal_fault{0}; - int emergency_input{0}; - int safeguard_signal_fault{0}; - int safeguard_input{0}; bool valid{false}; }; - struct RuntimeState; - Result ensureConnected_(const std::string& context) const; - Result ensureMotionReady_( - const std::string& context, - const std::shared_ptr& runtime, - huayan_internal::SafetyPermit& permit) const; Result unsupported_(const std::string& name) const; Result hrResult_(int code, const std::string& context) const; - Result motionStartFailure_( - const std::string& context, - huayan_internal::MotionStartStatus status) const; bool validDof_(std::size_t size, std::string& error) const; HrState readHrState_() const; - HrState sampleHrState_( - const std::shared_ptr& runtime) const; - void publishHrState_( - const std::shared_ptr& runtime, - const HrState& state) const; std::vector readJointPositionRad_() const; std::vector readJointVelocityRad_() const; CartesianPose readTcpPose_() const; CartesianVelocity readTcpVelocity_() const; - bool readJointPositionSample_(std::vector& values) const; - bool readJointVelocitySample_(std::vector& values) const; - bool readTcpPoseSample_(CartesianPose& pose) const; std::vector currentJointPositionDeg_() const; std::string nextCommandId_() const; - Result waitMotionDone_( - const std::string& context, - const std::shared_ptr& runtime, - huayan_internal::MotionToken motion_token, - huayan_internal::SafetyPermit safety_permit, - const std::string& command_id, - const std::vector* joint_target, - const CartesianPose* tcp_target, - int timeout_ms, - const std::function& cancellation_requested = {}) const; - bool targetReached_( - const std::vector* joint_target, - const CartesianPose* tcp_target) const; - bool controllerIdleStable_( - const std::shared_ptr& runtime, - std::chrono::milliseconds timeout) const; - bool terminateController_( - const std::shared_ptr& runtime, - std::chrono::milliseconds timeout, - bool stop_program) const; - Result completeSafetyRecovery_( - const std::string& context, - const std::shared_ptr& runtime, - std::uint64_t expected_epoch, - bool enable_robot, - bool release_software_guard); - std::shared_ptr runtimeSnapshot_() const; - void safetyMonitorLoop_(const std::shared_ptr& runtime); + Result waitMotionDone_(const std::string& context, int timeout_ms) const; private: config::RobotArmConfig cfg_; @@ -190,16 +135,12 @@ private: unsigned int robot_id_{0}; std::string tcp_name_{"TCP"}; std::string ucs_name_{"Base"}; - std::atomic speed_scaling_{1.0}; + double speed_scaling_{1.0}; std::atomic connected_{false}; - mutable std::atomic servo_mode_{false}; - std::atomic software_emergency_stopped_{false}; - std::atomic software_protective_stopped_{false}; + std::atomic busy_{false}; + std::atomic servo_mode_{false}; mutable std::mutex mutex_; - mutable std::recursive_mutex sdk_mutex_; mutable std::atomic command_seq_{0}; - std::shared_ptr runtime_; - std::thread safety_monitor_thread_; }; } // namespace cmvr::device diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h b/cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h deleted file mode 100644 index 6475abd4..00000000 --- a/cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h +++ /dev/null @@ -1,504 +0,0 @@ -#ifndef CMVR_ES_HUAYAN_LIFECYCLE_STATE_H -#define CMVR_ES_HUAYAN_LIFECYCLE_STATE_H - -#include -#include -#include -#include -#include -#include - -namespace cmvr::device::huayan_internal { - -enum class MotionKind { - None, - Joint, - Linear, - SpeedJoint, - SpeedLinear, - Servo, - Program, -}; - -struct MotionToken { - std::uint64_t generation{0}; - MotionKind kind{MotionKind::None}; - - bool valid() const noexcept - { - return generation != 0 && kind != MotionKind::None; - } -}; - -enum class MotionStartStatus { - Started, - Invalid, - Busy, - Stopping, - Blocked, -}; - -struct MotionStartResult { - MotionStartStatus status{MotionStartStatus::Busy}; - MotionToken token; - - bool started() const noexcept - { - return status == MotionStartStatus::Started; - } -}; - -enum class MotionFinishMode { - RestorePrevious, - Clear, - Retain, -}; - -enum class StopStartStatus { - Started, - AlreadyStopping, -}; - -struct StopRequest { - StopStartStatus status{StopStartStatus::AlreadyStopping}; - MotionKind kind{MotionKind::None}; - MotionToken active_token; - bool tracked_motion{false}; - - bool started() const noexcept - { - return status == StopStartStatus::Started; - } -}; - -struct SafetyCancelResult { - MotionKind kind{MotionKind::None}; - MotionToken active_token; - bool tracked_motion{false}; -}; - -struct MotionSnapshot { - MotionKind active_kind{MotionKind::None}; - MotionKind retained_kind{MotionKind::None}; - std::uint64_t active_generation{0}; - bool owner_active{false}; - bool stop_in_progress{false}; - bool blocked{false}; -}; - -// Tracks a single Huayan controller operation owner. Generation tokens make -// completion from an older RPC harmless after Stop or a safety event has -// cancelled it. Servo and program operations may retain their kind after the -// submitting RPC returns; begin(kind, true) supports same-kind updates while -// that retained controller mode remains active. -class MotionState final { -public: - MotionStartResult begin( - const MotionKind kind, - const bool replace_retained_same_kind = false) - { - std::lock_guard lock(mutex_); - if (kind == MotionKind::None) { - return {MotionStartStatus::Invalid, {}}; - } - if (stop_in_progress_) { - return {MotionStartStatus::Stopping, {}}; - } - if (blocked_) { - return {MotionStartStatus::Blocked, {}}; - } - if (owner_active_) { - return {MotionStartStatus::Busy, {}}; - } - if (retained_kind_ != MotionKind::None && - (!replace_retained_same_kind || retained_kind_ != kind)) { - return {MotionStartStatus::Busy, {}}; - } - - const MotionToken token{++next_generation_, kind}; - owner_active_ = true; - active_token_ = token; - previous_kind_ = retained_kind_; - return {MotionStartStatus::Started, token}; - } - - void finish( - const MotionToken token, - const MotionFinishMode mode = MotionFinishMode::RestorePrevious) - { - std::lock_guard lock(mutex_); - if (!owner_active_ || - active_token_.generation != token.generation) { - return; - } - - owner_active_ = false; - active_token_ = {}; - if (!stop_in_progress_ && !blocked_) { - switch (mode) { - case MotionFinishMode::RestorePrevious: - retained_kind_ = previous_kind_; - break; - case MotionFinishMode::Clear: - retained_kind_ = MotionKind::None; - break; - case MotionFinishMode::Retain: - retained_kind_ = token.kind; - break; - } - } - previous_kind_ = MotionKind::None; - owner_finished_cv_.notify_all(); - } - - void failMotion(const MotionToken token) - { - std::lock_guard lock(mutex_); - if (!owner_active_ || - active_token_.generation != token.generation) { - return; - } - - owner_active_ = false; - active_token_ = {}; - retained_kind_ = token.kind; - previous_kind_ = MotionKind::None; - blocked_ = true; - owner_finished_cv_.notify_all(); - } - - StopRequest beginStop( - const MotionKind requested_kind = MotionKind::None) - { - std::lock_guard lock(mutex_); - if (stop_in_progress_) { - return {}; - } - - stop_in_progress_ = true; - const MotionToken active = owner_active_ - ? active_token_ - : MotionToken{}; - if (active.valid()) { - cancelled_generation_ = std::max( - cancelled_generation_, active.generation); - } - - MotionKind kind = MotionKind::None; - if (active.valid()) { - kind = active.kind; - } else if (retained_kind_ != MotionKind::None) { - kind = retained_kind_; - } else { - kind = requested_kind; - } - if (kind != MotionKind::None) { - retained_kind_ = kind; - } - - return { - StopStartStatus::Started, - kind, - active, - active.valid() || kind != MotionKind::None}; - } - - SafetyCancelResult cancelActiveForSafety() - { - std::lock_guard lock(mutex_); - const MotionToken active = owner_active_ - ? active_token_ - : MotionToken{}; - if (active.valid()) { - cancelled_generation_ = std::max( - cancelled_generation_, active.generation); - } - - const MotionKind kind = active.valid() - ? active.kind - : retained_kind_; - if (kind != MotionKind::None) { - retained_kind_ = kind; - } - // A hardware safety transition is independent of a concurrent - // software Stop. New controller operations remain rejected until - // termination is positively confirmed. - blocked_ = true; - owner_finished_cv_.notify_all(); - return {kind, active, active.valid() || kind != MotionKind::None}; - } - - bool cancelled(const MotionToken token) const - { - std::lock_guard lock(mutex_); - return token.valid() && - token.generation <= cancelled_generation_; - } - - bool waitForOwnerExit( - const MotionToken token, - const std::chrono::milliseconds timeout) - { - if (!token.valid()) { - return true; - } - std::unique_lock lock(mutex_); - return owner_finished_cv_.wait_for( - lock, - timeout, - [this, token]() { - return !owner_active_ || - active_token_.generation != token.generation; - }); - } - - bool ownerActive(const MotionToken token) const - { - if (!token.valid()) { - return false; - } - std::lock_guard lock(mutex_); - return owner_active_ && - active_token_.generation == token.generation; - } - - bool completeStop() - { - std::lock_guard lock(mutex_); - if (owner_active_) { - return false; - } - stop_in_progress_ = false; - blocked_ = false; - active_token_ = {}; - retained_kind_ = MotionKind::None; - previous_kind_ = MotionKind::None; - owner_finished_cv_.notify_all(); - return true; - } - - void failStop() - { - std::lock_guard lock(mutex_); - stop_in_progress_ = false; - blocked_ = true; - owner_finished_cv_.notify_all(); - } - - MotionSnapshot snapshot() const - { - std::lock_guard lock(mutex_); - return { - owner_active_ ? active_token_.kind : MotionKind::None, - retained_kind_, - owner_active_ ? active_token_.generation : 0, - owner_active_, - stop_in_progress_, - blocked_}; - } - - bool busy() const - { - const auto state = snapshot(); - return state.owner_active || state.stop_in_progress || - state.blocked || - state.retained_kind != MotionKind::None; - } - -private: - mutable std::mutex mutex_; - std::condition_variable owner_finished_cv_; - std::uint64_t next_generation_{0}; - std::uint64_t cancelled_generation_{0}; - MotionToken active_token_; - MotionKind retained_kind_{MotionKind::None}; - MotionKind previous_kind_{MotionKind::None}; - bool owner_active_{false}; - bool stop_in_progress_{false}; - bool blocked_{false}; -}; - -enum class SafetyCondition { - Unknown, - Normal, - EmergencyStop, - SafeguardStop, - RobotFault, - EmergencySignalFault, - SafeguardSignalFault, - SoftwareEmergencyStop, - SoftwareProtectiveStop, -}; - -struct RawSafetyState { - bool valid{false}; - int emergency_signal_fault{0}; - int emergency_stop{0}; - int safeguard_signal_fault{0}; - int safeguard_stop{0}; - int robot_fault{0}; - bool software_emergency_stop{false}; - bool software_protective_stop{false}; -}; - -inline SafetyCondition classifySafetyCondition( - const RawSafetyState& state) noexcept -{ - if (!state.valid) { - return SafetyCondition::Unknown; - } - if (state.emergency_signal_fault != 0) { - return SafetyCondition::EmergencySignalFault; - } - if (state.safeguard_signal_fault != 0) { - return SafetyCondition::SafeguardSignalFault; - } - if (state.emergency_stop != 0) { - return SafetyCondition::EmergencyStop; - } - if (state.safeguard_stop != 0) { - return SafetyCondition::SafeguardStop; - } - if (state.robot_fault != 0) { - return SafetyCondition::RobotFault; - } - if (state.software_emergency_stop) { - return SafetyCondition::SoftwareEmergencyStop; - } - if (state.software_protective_stop) { - return SafetyCondition::SoftwareProtectiveStop; - } - return SafetyCondition::Normal; -} - -inline bool isMotionSafe(const SafetyCondition condition) noexcept -{ - return condition == SafetyCondition::Normal; -} - -struct SafetyPermit { - std::uint64_t epoch{0}; - - bool valid() const noexcept { return epoch != 0; } -}; - -struct RecoveryToken { - std::uint64_t epoch{0}; - - bool valid() const noexcept { return epoch != 0; } -}; - -struct SafetySnapshot { - SafetyCondition observed{SafetyCondition::Unknown}; - SafetyCondition latched_reason{SafetyCondition::Unknown}; - std::uint64_t epoch{0}; - bool latched{false}; - bool recovery_in_progress{false}; -}; - -// Safety inputs are events, not merely levels. Returning to Normal never -// clears a prior unsafe event. Explicit recovery is tied atomically to the -// event epoch, so a second event invalidates an older in-flight recovery. -class SafetyState final { -public: - void observe(const RawSafetyState& raw_state) - { - observe(classifySafetyCondition(raw_state)); - } - - void observe(const SafetyCondition condition) - { - std::lock_guard lock(mutex_); - const bool changed = observed_ != condition; - observed_ = condition; - if (isMotionSafe(condition)) { - return; - } - - if (!latched_ || recovery_in_progress_ || changed) { - ++epoch_; - } - latched_ = true; - recovery_in_progress_ = false; - latched_reason_ = condition; - } - - std::optional tryPermit() const - { - std::lock_guard lock(mutex_); - if (latched_ || !isMotionSafe(observed_)) { - return std::nullopt; - } - return SafetyPermit{epoch_}; - } - - bool validate(const SafetyPermit permit) const - { - std::lock_guard lock(mutex_); - return permit.valid() && permit.epoch == epoch_ && !latched_ && - isMotionSafe(observed_); - } - - std::optional beginRecovery( - const std::uint64_t expected_epoch) - { - std::lock_guard lock(mutex_); - if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ || - recovery_in_progress_ || !isMotionSafe(observed_)) { - return std::nullopt; - } - recovery_in_progress_ = true; - return RecoveryToken{epoch_}; - } - - bool completeRecovery( - const RecoveryToken token, - const bool robot_ready, - const bool controller_idle, - const bool cancellation_confirmed) - { - std::lock_guard lock(mutex_); - if (!token.valid() || token.epoch != epoch_ || !latched_ || - !recovery_in_progress_ || !isMotionSafe(observed_) || - !robot_ready || !controller_idle || !cancellation_confirmed) { - return false; - } - - latched_ = false; - recovery_in_progress_ = false; - latched_reason_ = SafetyCondition::Unknown; - ++epoch_; - return true; - } - - void failRecovery(const RecoveryToken token) - { - std::lock_guard lock(mutex_); - if (token.valid() && token.epoch == epoch_) { - recovery_in_progress_ = false; - } - } - - SafetySnapshot snapshot() const - { - std::lock_guard lock(mutex_); - return { - observed_, - latched_reason_, - epoch_, - latched_, - recovery_in_progress_}; - } - -private: - mutable std::mutex mutex_; - SafetyCondition observed_{SafetyCondition::Unknown}; - SafetyCondition latched_reason_{SafetyCondition::Unknown}; - std::uint64_t epoch_{1}; - bool latched_{false}; - bool recovery_in_progress_{false}; -}; - -} // namespace cmvr::device::huayan_internal - -#endif // CMVR_ES_HUAYAN_LIFECYCLE_STATE_H diff --git a/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp b/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp deleted file mode 100644 index 00cb8e27..00000000 --- a/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp +++ /dev/null @@ -1,874 +0,0 @@ -#include "devices/arm/huayan_arm/huayan_arm.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "HR_Pro.h" - -namespace { - -using Clock = std::chrono::steady_clock; -using namespace std::chrono_literals; - -constexpr double kPi = 3.14159265358979323846; - -double radToDeg(const double value) -{ - return value * 180.0 / kPi; -} - -struct FakeSdkState final { - std::mutex mutex; - bool connected{false}; - bool enabled{true}; - bool electrified{true}; - bool robot_error{false}; - bool paused{false}; - bool emergency_input{false}; - bool emergency_signal_fault{false}; - bool safeguard_input{false}; - bool safeguard_signal_fault{false}; - bool software_safeguard{false}; - - bool motion_active{false}; - bool motion_is_joint{true}; - bool stop_pending{false}; - bool hold_next_motion{false}; - bool stale_done_once{false}; - Clock::time_point completion_at{}; - Clock::time_point stop_complete_at{}; - std::array joint_position_deg{}; - std::array joint_target_deg{}; - std::array tcp_position_hr{}; - std::array tcp_target_hr{}; - std::string waypoint_id; - - bool servo_started{false}; - bool program_running{false}; - std::string selected_program; - - int move_j_calls{0}; - int move_l_calls{0}; - int speed_j_calls{0}; - int speed_l_calls{0}; - int group_stop_calls{0}; - int group_reset_calls{0}; - int start_servo_calls{0}; - int stop_script_calls{0}; - int idle_velocity_reads_after_stop{0}; - bool count_idle_reads{false}; - - void refreshLocked() - { - const auto now = Clock::now(); - const bool safety_active = emergency_input || safeguard_input || - software_safeguard; - - if (stop_pending && now >= stop_complete_at) { - stop_pending = false; - motion_active = false; - stale_done_once = false; - count_idle_reads = true; - } - - if (motion_active && !stop_pending && !safety_active && - completion_at != Clock::time_point{} && now >= completion_at) { - motion_active = false; - stale_done_once = false; - if (motion_is_joint) { - joint_position_deg = joint_target_deg; - } else { - tcp_position_hr = tcp_target_hr; - } - } - } - - bool movingLocked() - { - refreshLocked(); - return motion_active && !emergency_input && !safeguard_input && - !software_safeguard; - } - - bool doneLocked() - { - refreshLocked(); - return !motion_active; - } - - void startMotionLocked(const bool joint) - { - motion_active = true; - motion_is_joint = joint; - stop_pending = false; - count_idle_reads = false; - stale_done_once = true; - if (hold_next_motion) { - // The fallback deadline keeps a failed test from leaving a worker - // blocked for the production 60 second timeout. - completion_at = Clock::now() + 3s; - hold_next_motion = false; - } else { - completion_at = Clock::now() + 120ms; - } - } -}; - -FakeSdkState g_sdk; - -void resetFakeSdk() -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.connected = false; - g_sdk.enabled = true; - g_sdk.electrified = true; - g_sdk.robot_error = false; - g_sdk.paused = false; - g_sdk.emergency_input = false; - g_sdk.emergency_signal_fault = false; - g_sdk.safeguard_input = false; - g_sdk.safeguard_signal_fault = false; - g_sdk.software_safeguard = false; - g_sdk.motion_active = false; - g_sdk.motion_is_joint = true; - g_sdk.stop_pending = false; - g_sdk.hold_next_motion = false; - g_sdk.stale_done_once = false; - g_sdk.completion_at = {}; - g_sdk.stop_complete_at = {}; - g_sdk.joint_position_deg = {}; - g_sdk.joint_target_deg = {}; - g_sdk.tcp_position_hr = {}; - g_sdk.tcp_target_hr = {}; - g_sdk.waypoint_id.clear(); - g_sdk.servo_started = false; - g_sdk.program_running = false; - g_sdk.selected_program.clear(); - g_sdk.move_j_calls = 0; - g_sdk.move_l_calls = 0; - g_sdk.speed_j_calls = 0; - g_sdk.speed_l_calls = 0; - g_sdk.group_stop_calls = 0; - g_sdk.group_reset_calls = 0; - g_sdk.start_servo_calls = 0; - g_sdk.stop_script_calls = 0; - g_sdk.idle_velocity_reads_after_stop = 0; - g_sdk.count_idle_reads = false; -} - -void holdNextMotion() -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.hold_next_motion = true; -} - -void setHardwareEmergencyStop(const bool active) -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.emergency_input = active; - // If the wrapper never sends a real group Stop, releasing the switch makes - // the pending fake waypoint move again. This models the field failure. -} - -void setEmergencySignalFault(const bool active) -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.emergency_signal_fault = active; -} - -void dropFakeTransport() -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.connected = false; -} - -template -bool waitUntil(Predicate&& predicate, - const std::chrono::milliseconds timeout = 2s) -{ - const auto deadline = Clock::now() + timeout; - while (Clock::now() < deadline) { - if (predicate()) { - return true; - } - std::this_thread::sleep_for(10ms); - } - return predicate(); -} - -cmvr::config::RobotArmConfig makeConfig() -{ - cmvr::config::RobotArmConfig cfg; - cfg.set_id("huayan_fake_sdk"); - auto* vendor = cfg.mutable_vendor(); - vendor->set_brand(cmvr::config::VENDOR_ROBOT_ARM_BRAND_HUAYAN_ARM); - vendor->set_ip("127.0.0.1"); - vendor->set_port(10003); - vendor->set_model("HuayanFake"); - vendor->set_dof(6); - vendor->set_base_frame("Base"); - vendor->set_tool_frame("TCP"); - for (int i = 1; i <= 6; ++i) { - vendor->add_joint_names("joint_" + std::to_string(i)); - } - return cfg; -} - -int failures = 0; - -#define CHECK_TRUE(condition) \ - do { \ - if (!(condition)) { \ - std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ - << #condition << std::endl; \ - ++failures; \ - } \ - } while (false) - -} // namespace - -// The test executable exports these strong symbols. On ELF platforms they -// interpose the real SDK definitions used by libhuayan_arm, giving the test a -// deterministic controller without opening a network connection. -extern "C" { - -int HRIF_Connect(unsigned int, const char*, unsigned short) -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.connected = true; - return 0; -} - -int HRIF_DisConnect(unsigned int) -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.connected = false; - g_sdk.motion_active = false; - return 0; -} - -bool HRIF_IsConnected(unsigned int) -{ - std::lock_guard lock(g_sdk.mutex); - return g_sdk.connected; -} - -int HRIF_GetErrorCodeStr(unsigned int, int error_code, std::string& message) -{ - message = "fake SDK error " + std::to_string(error_code); - return 0; -} - -int HRIF_GrpEnable(unsigned int, unsigned int) -{ - std::lock_guard lock(g_sdk.mutex); - if (g_sdk.emergency_input || g_sdk.safeguard_input || - g_sdk.software_safeguard) { - return 101; - } - g_sdk.enabled = true; - g_sdk.electrified = true; - return 0; -} - -int HRIF_GrpDisable(unsigned int, unsigned int) -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.enabled = false; - g_sdk.electrified = false; - return 0; -} - -int HRIF_GrpReset(unsigned int, unsigned int) -{ - std::lock_guard lock(g_sdk.mutex); - ++g_sdk.group_reset_calls; - if (g_sdk.emergency_input || g_sdk.safeguard_input || - g_sdk.software_safeguard) { - return 102; - } - g_sdk.robot_error = false; - return 0; -} - -int HRIF_GrpStop(unsigned int, unsigned int) -{ - std::lock_guard lock(g_sdk.mutex); - ++g_sdk.group_stop_calls; - g_sdk.idle_velocity_reads_after_stop = 0; - g_sdk.count_idle_reads = false; - if (g_sdk.motion_active) { - g_sdk.stop_pending = true; - g_sdk.stop_complete_at = Clock::now() + 120ms; - g_sdk.completion_at = {}; - } else { - g_sdk.stop_pending = false; - g_sdk.count_idle_reads = true; - } - g_sdk.servo_started = false; - return 0; -} - -int HRIF_SetOverride(unsigned int, unsigned int, double) -{ - return 0; -} - -int HRIF_ReadRobotState(unsigned int, unsigned int, - int& moving, int& enabled, int& error, - int& error_code, int& error_axis, int& brake, - int& paused, int& emergency_stop, int& safeguard, - int& electrified, int& connected_to_box, - int& blending_done, int& in_position) -{ - std::lock_guard lock(g_sdk.mutex); - if (!g_sdk.connected) { - return 201; - } - moving = g_sdk.movingLocked() ? 1 : 0; - enabled = g_sdk.enabled ? 1 : 0; - error = g_sdk.robot_error ? 1 : 0; - error_code = g_sdk.robot_error ? 9001 : 0; - error_axis = 0; - brake = g_sdk.enabled ? 1 : 0; - paused = g_sdk.paused ? 1 : 0; - emergency_stop = g_sdk.emergency_input ? 1 : 0; - safeguard = (g_sdk.safeguard_input || g_sdk.software_safeguard) ? 1 : 0; - electrified = g_sdk.electrified ? 1 : 0; - connected_to_box = 1; - blending_done = moving == 0 ? 1 : 0; - in_position = g_sdk.doneLocked() ? 1 : 0; - return 0; -} - -int HRIF_ReadEmergencyInfo(unsigned int, unsigned int, - int& emergency_signal_fault, - int& emergency_input, - int& safeguard_signal_fault, - int& safeguard_input) -{ - std::lock_guard lock(g_sdk.mutex); - if (!g_sdk.connected) { - return 202; - } - emergency_signal_fault = g_sdk.emergency_signal_fault ? 1 : 0; - emergency_input = g_sdk.emergency_input ? 1 : 0; - safeguard_signal_fault = g_sdk.safeguard_signal_fault ? 1 : 0; - safeguard_input = - (g_sdk.safeguard_input || g_sdk.software_safeguard) ? 1 : 0; - return 0; -} - -int HRIF_ReadCurWaypointID(unsigned int, unsigned int, std::string& waypoint) -{ - std::lock_guard lock(g_sdk.mutex); - waypoint = g_sdk.waypoint_id; - return 0; -} - -int HRIF_IsMotionDone(unsigned int, unsigned int, bool& done) -{ - std::lock_guard lock(g_sdk.mutex); - if (g_sdk.stale_done_once) { - g_sdk.stale_done_once = false; - done = true; - } else { - done = g_sdk.doneLocked(); - } - return 0; -} - -int HRIF_ReadActJointPos(unsigned int, unsigned int, - double& j1, double& j2, double& j3, - double& j4, double& j5, double& j6) -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.refreshLocked(); - j1 = g_sdk.joint_position_deg[0]; - j2 = g_sdk.joint_position_deg[1]; - j3 = g_sdk.joint_position_deg[2]; - j4 = g_sdk.joint_position_deg[3]; - j5 = g_sdk.joint_position_deg[4]; - j6 = g_sdk.joint_position_deg[5]; - return 0; -} - -int HRIF_ReadActJointVel(unsigned int, unsigned int, - double& j1, double& j2, double& j3, - double& j4, double& j5, double& j6) -{ - std::lock_guard lock(g_sdk.mutex); - const double velocity = g_sdk.movingLocked() ? 5.0 : 0.0; - j1 = j2 = j3 = j4 = j5 = j6 = velocity; - if (velocity == 0.0 && g_sdk.count_idle_reads) { - ++g_sdk.idle_velocity_reads_after_stop; - } - return 0; -} - -int HRIF_ReadActTcpPos(unsigned int, unsigned int, - double& x, double& y, double& z, - double& rx, double& ry, double& rz) -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.refreshLocked(); - x = g_sdk.tcp_position_hr[0]; - y = g_sdk.tcp_position_hr[1]; - z = g_sdk.tcp_position_hr[2]; - rx = g_sdk.tcp_position_hr[3]; - ry = g_sdk.tcp_position_hr[4]; - rz = g_sdk.tcp_position_hr[5]; - return 0; -} - -int HRIF_ReadActTcpVel(unsigned int, unsigned int, - double& x, double& y, double& z, - double& rx, double& ry, double& rz) -{ - std::lock_guard lock(g_sdk.mutex); - const double velocity = g_sdk.movingLocked() ? 5.0 : 0.0; - x = y = z = rx = ry = rz = velocity; - return 0; -} - -int HRIF_MoveJ(unsigned int, unsigned int, - double, double, double, double, double, double, - double j1, double j2, double j3, - double j4, double j5, double j6, - std::string, std::string, double, double, double, - int, int, int, int, std::string command_id) -{ - std::lock_guard lock(g_sdk.mutex); - ++g_sdk.move_j_calls; - g_sdk.joint_target_deg = {j1, j2, j3, j4, j5, j6}; - g_sdk.waypoint_id = std::move(command_id); - g_sdk.startMotionLocked(true); - return 0; -} - -int HRIF_MoveL(unsigned int, unsigned int, - double x, double y, double z, - double rx, double ry, double rz, - double, double, double, double, double, double, - std::string, std::string, double, double, double, - int, int, int, std::string command_id) -{ - std::lock_guard lock(g_sdk.mutex); - ++g_sdk.move_l_calls; - g_sdk.tcp_target_hr = {x, y, z, rx, ry, rz}; - g_sdk.waypoint_id = std::move(command_id); - g_sdk.startMotionLocked(false); - return 0; -} - -int HRIF_SpeedJ(unsigned int, unsigned int, - double, double, double, double, double, double, - double, double) -{ - std::lock_guard lock(g_sdk.mutex); - ++g_sdk.speed_j_calls; - g_sdk.startMotionLocked(true); - return 0; -} - -int HRIF_SpeedL(unsigned int, unsigned int, - double, double, double, double, double, double, - double, double, double) -{ - std::lock_guard lock(g_sdk.mutex); - ++g_sdk.speed_l_calls; - g_sdk.startMotionLocked(false); - return 0; -} - -int HRIF_StartServo(unsigned int, unsigned int, double, double) -{ - std::lock_guard lock(g_sdk.mutex); - ++g_sdk.start_servo_calls; - g_sdk.servo_started = true; - return 0; -} - -int HRIF_PushServoJ(unsigned int, unsigned int, - double j1, double j2, double j3, - double j4, double j5, double j6) -{ - std::lock_guard lock(g_sdk.mutex); - if (!g_sdk.servo_started) { - return 301; - } - g_sdk.joint_position_deg = {j1, j2, j3, j4, j5, j6}; - return 0; -} - -int HRIF_PushServoP(unsigned int, unsigned int, - std::vector& coord, - std::vector&, - std::vector&) -{ - std::lock_guard lock(g_sdk.mutex); - if (!g_sdk.servo_started || coord.size() < 6) { - return 302; - } - std::copy_n(coord.begin(), 6, g_sdk.tcp_position_hr.begin()); - return 0; -} - -int HRIF_SwitchScript(unsigned int, unsigned int, std::string script_name) -{ - std::lock_guard lock(g_sdk.mutex); - if (script_name.empty()) { - return 401; - } - g_sdk.selected_program = std::move(script_name); - return 0; -} - -int HRIF_StartScript(unsigned int) -{ - std::lock_guard lock(g_sdk.mutex); - if (g_sdk.selected_program.empty()) { - return 402; - } - g_sdk.program_running = true; - g_sdk.paused = false; - return 0; -} - -int HRIF_PauseScript(unsigned int) -{ - std::lock_guard lock(g_sdk.mutex); - if (!g_sdk.program_running) { - return 403; - } - g_sdk.paused = true; - return 0; -} - -int HRIF_StopScript(unsigned int) -{ - std::lock_guard lock(g_sdk.mutex); - ++g_sdk.stop_script_calls; - g_sdk.program_running = false; - g_sdk.paused = false; - return 0; -} - -int HRIF_EnterSafetyGuard(unsigned int, unsigned int, int flag) -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.software_safeguard = flag != 0; - return 0; -} - -int HRIF_ShutdownRobot(unsigned int) -{ - std::lock_guard lock(g_sdk.mutex); - g_sdk.connected = false; - g_sdk.enabled = false; - g_sdk.electrified = false; - return 0; -} - -} // extern "C" - -int main() -{ - using namespace cmvr::device; - - resetFakeSdk(); - HuayanRobot arm(makeConfig()); - CHECK_TRUE(arm.connect("127.0.0.1", 10003).ok()); - - MotionOptions options; - options.velocity = 0.4; - options.acceleration = 0.8; - CHECK_TRUE(arm.supportsActionQueueMotion()); - - JointPositionCommand joint_a{{0.10, -0.05, 0.08, 0.0, 0.02, -0.03}}; - MotionOptions cancelled_options = options; - cancelled_options.cancellation_requested = []() { return true; }; - CHECK_TRUE(!arm.moveJ(joint_a, cancelled_options).ok()); - CartesianPose cancelled_pose; - cancelled_pose.x = 0.20; - cancelled_pose.z = 0.30; - CHECK_TRUE(!arm.moveL(cancelled_pose, cancelled_options).ok()); - { - std::lock_guard lock(g_sdk.mutex); - CHECK_TRUE(g_sdk.move_j_calls == 0); - CHECK_TRUE(g_sdk.move_l_calls == 0); - } - - // Once a command has been accepted, caller cancellation returns promptly - // but retains the typed motion barrier until Stop confirms controller idle. - std::atomic cancel_during_wait{false}; - MotionOptions cancellable_options = options; - cancellable_options.cancellation_requested = [&cancel_during_wait]() { - return cancel_during_wait.load(); - }; - JointPositionCommand cancelled_in_wait{ - {0.05, -0.02, 0.04, 0.01, 0.0, -0.01}}; - holdNextMotion(); - auto cancelled_motion = std::async(std::launch::async, [&]() { - return arm.moveJ(cancelled_in_wait, cancellable_options); - }); - CHECK_TRUE(waitUntil([&]() { - std::lock_guard lock(g_sdk.mutex); - return g_sdk.move_j_calls > 0; - })); - cancel_during_wait.store(true); - CHECK_TRUE(cancelled_motion.wait_for(1s) == std::future_status::ready); - if (cancelled_motion.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(!cancelled_motion.get().ok()); - } - CHECK_TRUE(arm.busy()); - CHECK_TRUE(arm.stopMotion().ok()); - CHECK_TRUE(!arm.busy()); - - // The first IsMotionDone read intentionally reports the preceding idle - // state. Completion must be correlated with the command/target. Once the - // target is reached, an identical command is an idempotent no-op. - CHECK_TRUE(arm.moveJ(joint_a, options).ok()); - int move_j_after_first = 0; - { - std::lock_guard lock(g_sdk.mutex); - move_j_after_first = g_sdk.move_j_calls; - } - CHECK_TRUE(arm.moveJ(joint_a, options).ok()); - { - std::lock_guard lock(g_sdk.mutex); - CHECK_TRUE(g_sdk.move_j_calls == move_j_after_first); - } - - CartesianPose pose_a; - pose_a.x = 0.31; - pose_a.y = -0.12; - pose_a.z = 0.42; - pose_a.rx = 0.08; - pose_a.ry = -0.04; - pose_a.rz = 0.12; - CHECK_TRUE(arm.moveL(pose_a, options).ok()); - int move_l_after_first = 0; - { - std::lock_guard lock(g_sdk.mutex); - move_l_after_first = g_sdk.move_l_calls; - } - CHECK_TRUE(arm.moveL(pose_a, options).ok()); - { - std::lock_guard lock(g_sdk.mutex); - CHECK_TRUE(g_sdk.move_l_calls == move_l_after_first); - } - - // Stop must cancel the old owner and wait until the controller reports - // stable idle; clearing the owner immediately after GrpStop would fail the - // elapsed-time and consecutive-idle checks below. - JointPositionCommand joint_b{{0.22, -0.08, 0.14, 0.03, 0.04, -0.01}}; - holdNextMotion(); - const int before_held_move = move_j_after_first; - auto held_move = std::async(std::launch::async, [&]() { - return arm.moveJ(joint_b, options); - }); - CHECK_TRUE(waitUntil([&]() { - std::lock_guard lock(g_sdk.mutex); - return g_sdk.move_j_calls > before_held_move; - })); - const auto stop_started = Clock::now(); - CHECK_TRUE(arm.stopMotion().ok()); - const auto stop_elapsed = Clock::now() - stop_started; - CHECK_TRUE(stop_elapsed >= 100ms); - CHECK_TRUE(held_move.wait_for(1s) == std::future_status::ready); - if (held_move.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(!held_move.get().ok()); - } - CHECK_TRUE(!arm.busy()); - { - std::lock_guard lock(g_sdk.mutex); - CHECK_TRUE(g_sdk.idle_velocity_reads_after_stop >= 3); - } - CHECK_TRUE(arm.moveJ(joint_b, options).ok()); - - // A hardware E-stop cancels and terminates the active waypoint. Releasing - // the switch does not clear the software latch or grant a new permit. - JointPositionCommand joint_c{{0.34, -0.02, 0.09, 0.05, -0.02, 0.07}}; - holdNextMotion(); - int before_estop_move = 0; - int before_estop_stop = 0; - { - std::lock_guard lock(g_sdk.mutex); - before_estop_move = g_sdk.move_j_calls; - before_estop_stop = g_sdk.group_stop_calls; - } - auto estop_move = std::async(std::launch::async, [&]() { - return arm.moveJ(joint_c, options); - }); - CHECK_TRUE(waitUntil([&]() { - std::lock_guard lock(g_sdk.mutex); - return g_sdk.move_j_calls > before_estop_move; - })); - setHardwareEmergencyStop(true); - CHECK_TRUE(waitUntil([&]() { - std::lock_guard lock(g_sdk.mutex); - return g_sdk.group_stop_calls > before_estop_stop; - })); - CHECK_TRUE(estop_move.wait_for(2s) == std::future_status::ready); - if (estop_move.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(!estop_move.get().ok()); - } - setHardwareEmergencyStop(false); - std::this_thread::sleep_for(150ms); - - int move_count_while_latched = 0; - { - std::lock_guard lock(g_sdk.mutex); - move_count_while_latched = g_sdk.move_j_calls; - } - const auto rejected_while_latched = arm.moveJ(joint_a, options); - CHECK_TRUE(!rejected_while_latched.ok()); - { - std::lock_guard lock(g_sdk.mutex); - CHECK_TRUE(g_sdk.move_j_calls == move_count_while_latched); - } - CHECK_TRUE(arm.clearFault().ok()); - CHECK_TRUE(arm.torqueOn().ok()); - CHECK_TRUE(arm.moveJ(joint_a, options).ok()); - - // Speed commands own the controller while waiting. A different motion is - // rejected, and Stop releases ownership only after termination. - holdNextMotion(); - JointVelocityCommand speed{{0.1, 0.0, 0.0, 0.0, 0.0, 0.0}}; - int speed_calls_before = 0; - { - std::lock_guard lock(g_sdk.mutex); - speed_calls_before = g_sdk.speed_j_calls; - } - auto speed_motion = std::async(std::launch::async, [&]() { - return arm.speedJ(speed, 0.5, 2.0); - }); - CHECK_TRUE(waitUntil([&]() { - std::lock_guard lock(g_sdk.mutex); - return g_sdk.speed_j_calls > speed_calls_before; - })); - CHECK_TRUE(!arm.moveL(pose_a, options).ok()); - CHECK_TRUE(arm.stopMotion().ok()); - CHECK_TRUE(speed_motion.wait_for(1s) == std::future_status::ready); - if (speed_motion.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(!speed_motion.get().ok()); - } - CHECK_TRUE(!arm.busy()); - - // SpeedL used to hold the SDK mutex while waiting, which deadlocked its - // own timeout/Stop path. A concurrent Stop must cancel it, settle the - // controller, and allow a genuinely new Move command afterwards. - holdNextMotion(); - CartesianVelocity line_speed; - line_speed.vx = 0.05; - int speed_l_calls_before = 0; - { - std::lock_guard lock(g_sdk.mutex); - speed_l_calls_before = g_sdk.speed_l_calls; - } - auto line_speed_motion = std::async(std::launch::async, [&]() { - return arm.speedL(line_speed, 0.5, 2.0, FrameType::Base); - }); - CHECK_TRUE(waitUntil([&]() { - std::lock_guard lock(g_sdk.mutex); - return g_sdk.speed_l_calls > speed_l_calls_before; - })); - CHECK_TRUE(arm.stopMotion().ok()); - CHECK_TRUE(line_speed_motion.wait_for(1s) == std::future_status::ready); - if (line_speed_motion.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(!line_speed_motion.get().ok()); - } - CHECK_TRUE(!arm.busy()); - CHECK_TRUE(arm.moveJ(joint_b, options).ok()); - - // A dual-channel emergency input mismatch is a typed emergency latch. It - // remains blocked after the wiring level is healthy and is recovered only - // through the emergency recovery path. - int stops_before_signal_fault = 0; - { - std::lock_guard lock(g_sdk.mutex); - stops_before_signal_fault = g_sdk.group_stop_calls; - } - setEmergencySignalFault(true); - CHECK_TRUE(waitUntil([&]() { - std::lock_guard lock(g_sdk.mutex); - return g_sdk.group_stop_calls > stops_before_signal_fault; - })); - setEmergencySignalFault(false); - std::this_thread::sleep_for(100ms); - CHECK_TRUE(!arm.moveJ(joint_c, options).ok()); - CHECK_TRUE(arm.torqueOn().ok()); - CHECK_TRUE(arm.moveJ(joint_c, options).ok()); - - // Power-off holds a terminal barrier through GrpDisable. Motion remains - // denied until an explicit enable confirms the powered state again. - CHECK_TRUE(arm.torqueOff().ok()); - CHECK_TRUE(!arm.moveJ(joint_a, options).ok()); - CHECK_TRUE(arm.torqueOn().ok()); - CHECK_TRUE(arm.moveJ(joint_a, options).ok()); - - // Servo and program modes retain ownership beyond the start call. Stop of - // a retained program must use StopScript as well as the group stop path. - ServoOptions servo_options; - CHECK_TRUE(arm.startServoMode(servo_options).ok()); - CHECK_TRUE(!arm.moveJ(joint_b, options).ok()); - CHECK_TRUE(arm.servoJ(joint_b).ok()); - CHECK_TRUE(arm.stopServoMode().ok()); - CHECK_TRUE(!arm.busy()); - - CHECK_TRUE(arm.loadProgram("fake_program.script").ok()); - CHECK_TRUE(arm.playProgram().ok()); - CHECK_TRUE(!arm.moveJ(joint_c, options).ok()); - int stop_script_calls_before = 0; - { - std::lock_guard lock(g_sdk.mutex); - stop_script_calls_before = g_sdk.stop_script_calls; - } - CHECK_TRUE(arm.stopMotion().ok()); - { - std::lock_guard lock(g_sdk.mutex); - CHECK_TRUE(g_sdk.stop_script_calls > stop_script_calls_before); - } - CHECK_TRUE(!arm.busy()); - - // Retire an in-flight stale generation after transport loss before - // reconnecting; no old waiter may issue SDK reads into the new session. - holdNextMotion(); - int moves_before_transport_loss = 0; - { - std::lock_guard lock(g_sdk.mutex); - moves_before_transport_loss = g_sdk.move_j_calls; - } - auto transport_lost_move = std::async(std::launch::async, [&]() { - return arm.moveJ(joint_c, options); - }); - CHECK_TRUE(waitUntil([&]() { - std::lock_guard lock(g_sdk.mutex); - return g_sdk.move_j_calls > moves_before_transport_loss; - })); - dropFakeTransport(); - CHECK_TRUE(arm.connect("127.0.0.1", 10003).ok()); - CHECK_TRUE(transport_lost_move.wait_for(1s) == std::future_status::ready); - if (transport_lost_move.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(!transport_lost_move.get().ok()); - } - CHECK_TRUE(arm.moveJ(joint_b, options).ok()); - CHECK_TRUE(arm.disconnect().ok()); - - resetFakeSdk(); - HuayanRobot shutdown_arm(makeConfig()); - CHECK_TRUE(shutdown_arm.connect("127.0.0.1", 10003).ok()); - CHECK_TRUE(shutdown_arm.shutdown().ok()); - CHECK_TRUE(!shutdown_arm.isConnected()); - return failures == 0 ? 0 : 1; -} diff --git a/cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp b/cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp deleted file mode 100644 index 8bb93ff7..00000000 --- a/cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp +++ /dev/null @@ -1,232 +0,0 @@ -#include "devices/arm/huayan_arm/huayan_lifecycle_state.h" - -#include -#include - -namespace { - -#define CHECK_TRUE(condition) \ - do { \ - if (!(condition)) { \ - std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ - << #condition << std::endl; \ - return 1; \ - } \ - } while (false) - -} // namespace - -int main() -{ - using namespace cmvr::device::huayan_internal; - - MotionState motion; - CHECK_TRUE(motion.begin(MotionKind::None).status == - MotionStartStatus::Invalid); - - // A completed target does not poison an identical subsequent command, - // while an actually concurrent command is rejected. - const auto first_joint = motion.begin(MotionKind::Joint); - CHECK_TRUE(first_joint.started()); - CHECK_TRUE(motion.begin(MotionKind::Joint).status == - MotionStartStatus::Busy); - CHECK_TRUE(motion.begin(MotionKind::Linear).status == - MotionStartStatus::Busy); - motion.finish(first_joint.token); - const auto repeated_joint = motion.begin(MotionKind::Joint); - CHECK_TRUE(repeated_joint.started()); - CHECK_TRUE(repeated_joint.token.generation > - first_joint.token.generation); - motion.finish(first_joint.token); - CHECK_TRUE(motion.ownerActive(repeated_joint.token)); - motion.finish(repeated_joint.token); - CHECK_TRUE(!motion.busy()); - - // Stop cancels the current generation and cannot complete before its - // owner exits. - const auto linear = motion.begin(MotionKind::Linear); - CHECK_TRUE(linear.started()); - const auto stop_linear = motion.beginStop(); - CHECK_TRUE(stop_linear.started()); - CHECK_TRUE(stop_linear.kind == MotionKind::Linear); - CHECK_TRUE(stop_linear.active_token.generation == - linear.token.generation); - CHECK_TRUE(stop_linear.tracked_motion); - CHECK_TRUE(motion.cancelled(linear.token)); - CHECK_TRUE(motion.beginStop().status == - StopStartStatus::AlreadyStopping); - CHECK_TRUE(motion.begin(MotionKind::Joint).status == - MotionStartStatus::Stopping); - CHECK_TRUE(!motion.waitForOwnerExit( - linear.token, std::chrono::milliseconds(1))); - CHECK_TRUE(!motion.completeStop()); - motion.finish(linear.token); - CHECK_TRUE(motion.waitForOwnerExit( - linear.token, std::chrono::milliseconds(1))); - CHECK_TRUE(motion.completeStop()); - CHECK_TRUE(!motion.busy()); - - // An uncertain submission/completion remains fail-closed until a - // positively acknowledged Stop clears it. - const auto failed_speed = motion.begin(MotionKind::SpeedLinear); - CHECK_TRUE(failed_speed.started()); - motion.failMotion(failed_speed.token); - CHECK_TRUE(motion.snapshot().blocked); - CHECK_TRUE(motion.begin(MotionKind::Joint).status == - MotionStartStatus::Blocked); - const auto stop_failed_speed = motion.beginStop(); - CHECK_TRUE(stop_failed_speed.kind == MotionKind::SpeedLinear); - CHECK_TRUE(stop_failed_speed.tracked_motion); - CHECK_TRUE(motion.completeStop()); - - const auto failed_stop_motion = motion.begin(MotionKind::Joint); - CHECK_TRUE(failed_stop_motion.started()); - const auto failed_stop = motion.beginStop(); - CHECK_TRUE(failed_stop.kind == MotionKind::Joint); - motion.failStop(); - CHECK_TRUE(motion.snapshot().blocked); - CHECK_TRUE(motion.begin(MotionKind::Linear).status == - MotionStartStatus::Blocked); - motion.finish(failed_stop_motion.token); - const auto retry_failed_stop = motion.beginStop(); - CHECK_TRUE(retry_failed_stop.kind == MotionKind::Joint); - CHECK_TRUE(motion.completeStop()); - - // Servo and program modes remain owned after their start RPC returns. - const auto servo = motion.begin(MotionKind::Servo); - CHECK_TRUE(servo.started()); - motion.finish(servo.token, MotionFinishMode::Retain); - CHECK_TRUE(motion.snapshot().retained_kind == MotionKind::Servo); - CHECK_TRUE(motion.begin(MotionKind::Program).status == - MotionStartStatus::Busy); - const auto servo_update = motion.begin(MotionKind::Servo, true); - CHECK_TRUE(servo_update.started()); - motion.finish(servo_update.token); - CHECK_TRUE(motion.snapshot().retained_kind == MotionKind::Servo); - const auto stop_servo = motion.beginStop(); - CHECK_TRUE(stop_servo.kind == MotionKind::Servo); - CHECK_TRUE(stop_servo.tracked_motion); - CHECK_TRUE(motion.completeStop()); - - const auto program = motion.begin(MotionKind::Program); - CHECK_TRUE(program.started()); - motion.finish(program.token, MotionFinishMode::Retain); - const auto cancelled_program = motion.cancelActiveForSafety(); - CHECK_TRUE(cancelled_program.kind == MotionKind::Program); - CHECK_TRUE(cancelled_program.tracked_motion); - CHECK_TRUE(!cancelled_program.active_token.valid()); - CHECK_TRUE(motion.begin(MotionKind::Joint).status == - MotionStartStatus::Blocked); - const auto stop_program = motion.beginStop(); - CHECK_TRUE(stop_program.kind == MotionKind::Program); - CHECK_TRUE(motion.completeStop()); - - const auto safety_move = motion.begin(MotionKind::SpeedJoint); - CHECK_TRUE(safety_move.started()); - const auto cancelled_move = motion.cancelActiveForSafety(); - CHECK_TRUE(cancelled_move.kind == MotionKind::SpeedJoint); - CHECK_TRUE(cancelled_move.active_token.generation == - safety_move.token.generation); - CHECK_TRUE(motion.cancelled(safety_move.token)); - motion.finish(safety_move.token); - const auto stop_safety_move = motion.beginStop(); - CHECK_TRUE(stop_safety_move.kind == MotionKind::SpeedJoint); - CHECK_TRUE(motion.completeStop()); - - RawSafetyState raw; - CHECK_TRUE(classifySafetyCondition(raw) == SafetyCondition::Unknown); - raw.valid = true; - CHECK_TRUE(classifySafetyCondition(raw) == SafetyCondition::Normal); - raw.software_protective_stop = true; - CHECK_TRUE(classifySafetyCondition(raw) == - SafetyCondition::SoftwareProtectiveStop); - raw.software_emergency_stop = true; - CHECK_TRUE(classifySafetyCondition(raw) == - SafetyCondition::SoftwareEmergencyStop); - raw.robot_fault = 1; - CHECK_TRUE(classifySafetyCondition(raw) == - SafetyCondition::RobotFault); - raw.safeguard_stop = 1; - CHECK_TRUE(classifySafetyCondition(raw) == - SafetyCondition::SafeguardStop); - raw.emergency_stop = 1; - CHECK_TRUE(classifySafetyCondition(raw) == - SafetyCondition::EmergencyStop); - raw.safeguard_signal_fault = 1; - CHECK_TRUE(classifySafetyCondition(raw) == - SafetyCondition::SafeguardSignalFault); - raw.emergency_signal_fault = 1; - CHECK_TRUE(classifySafetyCondition(raw) == - SafetyCondition::EmergencySignalFault); - - SafetyState safety; - CHECK_TRUE(!safety.tryPermit().has_value()); - safety.observe(SafetyCondition::Normal); - const auto initial_permit = safety.tryPermit(); - CHECK_TRUE(initial_permit.has_value()); - CHECK_TRUE(safety.validate(*initial_permit)); - - safety.observe(SafetyCondition::EmergencyStop); - CHECK_TRUE(safety.snapshot().latched); - CHECK_TRUE(!safety.validate(*initial_permit)); - CHECK_TRUE(!safety.beginRecovery(safety.snapshot().epoch).has_value()); - const auto first_emergency_epoch = safety.snapshot().epoch; - safety.observe(SafetyCondition::EmergencyStop); - safety.observe(SafetyCondition::EmergencyStop); - CHECK_TRUE(safety.snapshot().epoch == first_emergency_epoch); - - // Releasing the hardware switch only changes the observed level; it does - // not clear the event latch or issue a new motion permit. - safety.observe(SafetyCondition::Normal); - CHECK_TRUE(safety.snapshot().latched); - CHECK_TRUE(!safety.tryPermit().has_value()); - - const auto not_ready = safety.beginRecovery(safety.snapshot().epoch); - CHECK_TRUE(not_ready.has_value()); - CHECK_TRUE(!safety.completeRecovery(*not_ready, false, true, true)); - CHECK_TRUE(safety.snapshot().latched); - safety.failRecovery(*not_ready); - - const auto not_idle = safety.beginRecovery(safety.snapshot().epoch); - CHECK_TRUE(not_idle.has_value()); - CHECK_TRUE(!safety.completeRecovery(*not_idle, true, false, true)); - CHECK_TRUE(safety.snapshot().latched); - safety.failRecovery(*not_idle); - - const auto not_cancelled = - safety.beginRecovery(safety.snapshot().epoch); - CHECK_TRUE(not_cancelled.has_value()); - CHECK_TRUE(!safety.completeRecovery(*not_cancelled, true, true, false)); - CHECK_TRUE(safety.snapshot().latched); - safety.failRecovery(*not_cancelled); - - const auto recovery_retry = - safety.beginRecovery(safety.snapshot().epoch); - CHECK_TRUE(recovery_retry.has_value()); - CHECK_TRUE(safety.completeRecovery( - *recovery_retry, true, true, true)); - const auto recovered_permit = safety.tryPermit(); - CHECK_TRUE(recovered_permit.has_value()); - CHECK_TRUE(safety.validate(*recovered_permit)); - - // A second safety event, including the same physical E-stop being pressed - // again, invalidates an older recovery token atomically. - safety.observe(SafetyCondition::EmergencyStop); - safety.observe(SafetyCondition::Normal); - const auto stale_recovery = - safety.beginRecovery(safety.snapshot().epoch); - CHECK_TRUE(stale_recovery.has_value()); - safety.observe(SafetyCondition::EmergencyStop); - safety.observe(SafetyCondition::Normal); - CHECK_TRUE(!safety.completeRecovery( - *stale_recovery, true, true, true)); - CHECK_TRUE(safety.snapshot().latched); - CHECK_TRUE(!safety.snapshot().recovery_in_progress); - - const auto stale_epoch = safety.snapshot().epoch; - safety.observe(SafetyCondition::SafeguardStop); - safety.observe(SafetyCondition::Normal); - CHECK_TRUE(!safety.beginRecovery(stale_epoch).has_value()); - - return 0; -} diff --git a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt index 405a1588..a194fa73 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt @@ -26,7 +26,6 @@ add_executable(motor_robot_arm_mujoco_test target_link_libraries(motor_robot_arm_mujoco_test PRIVATE cmvr_es::device::motor_robot_arm - cmvr_es::algorithms::arm_control cmvr_es::device::motor_manager cmvr_es::device::mujoco_motor_driver cmvr_es::mujoco_viewer diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index ff1b5b49..73be9284 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -36,13 +36,6 @@ public: RobotMode getRobotMode() const override { return RobotMode::Idle; } SafetyMode getSafetyMode() const override; ControlMode getControlMode() const override { return ControlMode::Position; } - bool supportsTeleopGroupServo() const noexcept override - { - // commandCyclicPosition is currently dispatched one joint at a time. - // A config switch cannot turn that partial-write behavior into the - // atomic/timed group primitive required by network teleoperation. - return false; - } Result torqueOn() override; Result torqueOff() override; @@ -103,12 +96,6 @@ public: bool busy() const override; private: - enum class TrajectoryExecutionResult { - Completed, - Canceled, - Failed, - }; - bool containsJoint_(const std::string& joint_name) const; bool safetyStopRequested_() const; std::optional safetyStopResult_(const std::string& command, @@ -121,10 +108,7 @@ private: std::vector readJointPosition_() const; bool configureAlgorithms_(); - TrajectoryExecutionResult executeMoveLTrajectory_( - const CartesianJointTrajectory& trajectory, - const std::function& cancellation_requested, - std::uint64_t motion_generation); + bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory); static CartesianVelocityController::Config toCartesianVelocityControllerConfig_( const config::CartesianVelocityControllerConfig& config); @@ -140,7 +124,6 @@ private: std::unordered_set joint_set_; std::string motor_system_id_; std::shared_ptr motor_manager_{nullptr}; - std::uint64_t motor_control_claim_id_{0}; std::shared_ptr ik_solver_{nullptr}; std::shared_ptr joint_planner_{nullptr}; @@ -148,10 +131,7 @@ private: std::unique_ptr cartesian_velocity_controller_{nullptr}; mutable std::mutex mutex_; - std::atomic motion_generation_{0}; std::atomic busy_{false}; - std::atomic powered_on_{false}; - mutable std::atomic joint_state_sequence_{0}; double speed_scaling_{1.0}; std::atomic protective_stopped_{false}; std::atomic emergency_stopped_{false}; diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index d0c72027..5acc3f39 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -35,36 +35,6 @@ struct AtomicFlagGuard { ~AtomicFlagGuard() { flag.store(false); } }; -bool cancellationRequested( - const std::function& cancellation_requested) noexcept -{ - if (!cancellation_requested) { - return false; - } - try { - return cancellation_requested(); - } catch (...) { - return true; - } -} - -const config::JointLimitsConfig* configuredJointLimits( - const config::ArmKinematicsConfig& kinematics) -{ - switch (kinematics.algorithm_case()) { - case config::ArmKinematicsConfig::kPinocchioDlsIkSolver: - return &kinematics.pinocchio_dls_ik_solver() - .joint_limit_policy() - .limits(); - case config::ArmKinematicsConfig::kPinocchioQpIkSolver: - return &kinematics.pinocchio_qp_ik_solver() - .joint_limit_policy() - .limits(); - default: - return nullptr; - } -} - } // namespace MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg) @@ -103,39 +73,6 @@ MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg) model_.manufacturer = "cmvr"; model_.dof = static_cast(dof_); model_.joint_names = joint_names_; - - const auto* configured_limits = configuredJointLimits(cfg_.kinematics()); - if (configured_limits != nullptr && configured_limits->enable() && - configured_limits->source() == - config::JOINT_LIMIT_SOURCE_CUSTOM && - configured_limits->joints_size() == dof_) { - bool valid_limits = true; - model_.joint_limits.reserve(static_cast(dof_)); - for (int index = 0; index < dof_; ++index) { - const auto& source = configured_limits->joints(index); - if (source.joint_name() != joint_names_[static_cast(index)] || - !std::isfinite(source.q_lb()) || - !std::isfinite(source.q_ub()) || - !std::isfinite(source.qd()) || - source.q_lb() >= source.q_ub() || - source.qd() <= 0.0) { - valid_limits = false; - break; - } - JointLimit limit; - limit.lower = source.q_lb(); - limit.upper = source.q_ub(); - limit.max_velocity = source.qd(); - limit.max_acceleration = source.qdd(); - model_.joint_limits.push_back(limit); - } - if (!valid_limits) { - model_.joint_limits.clear(); - CMVR_LOG(ERROR) - << "[MotorRobotArm] invalid or misordered custom joint limits: " - << id_; - } - } } MotorRobotArm::~MotorRobotArm() @@ -143,9 +80,6 @@ MotorRobotArm::~MotorRobotArm() if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } - if (motor_manager_ && motor_control_claim_id_ != 0U) { - motor_manager_->releaseArmJoints(motor_control_claim_id_); - } } bool MotorRobotArm::init() @@ -194,16 +128,6 @@ bool MotorRobotArm::init() CMVR_LOG(ERROR) << "[MotorRobotArm] failed to configure algorithms: " << id_; return false; } - if (motor_control_claim_id_ == 0U) { - std::string claim_error; - if (!motor_manager_->claimArmJoints( - id_, joint_names_, motor_control_claim_id_, &claim_error)) { - CMVR_LOG(ERROR) - << "[MotorRobotArm] failed to claim direct motor control: " - << id_ << ", detail=" << claim_error; - return false; - } - } CMVR_LOG(INFO) << "[MotorRobotArm] (init): Arm '" << id_ << "' init success"; return true; } @@ -219,8 +143,8 @@ ArmState MotorRobotArm::getRobotState() const const bool emergency_stopped = emergency_stopped_.load(); ArmState state; state.connected = motor_manager_ != nullptr; - state.powered_on = powered_on_.load(std::memory_order_acquire); - state.brake_released = state.powered_on && !emergency_stopped; + state.powered_on = true; + state.brake_released = !emergency_stopped; state.moving = busy(); state.protective_stopped = protective_stopped; state.emergency_stopped = emergency_stopped; @@ -240,35 +164,15 @@ JointGroupState MotorRobotArm::getJointState() const state.position.reserve(joint_names_.size()); state.velocity.reserve(joint_names_.size()); state.effort.reserve(joint_names_.size()); - bool values_valid = true; for (const auto& joint_name : joint_names_) { auto motor = getMotor_(joint_name); if (!motor) { - values_valid = false; continue; } - const double position = motor->getQ(); - const double velocity = motor->getQd(); - values_valid = - values_valid && std::isfinite(position) && std::isfinite(velocity); - state.position.push_back(position); - state.velocity.push_back(velocity); + state.position.push_back(motor->getQ()); + state.velocity.push_back(motor->getQd()); state.effort.push_back(0.0); } - values_valid = - values_valid && state.position.size() == joint_names_.size() && - state.velocity.size() == joint_names_.size(); - state.sequence = - joint_state_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; - state.sample_monotonic_ns = - std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()) - .count(); - state.position_valid = values_valid; - state.velocity_valid = values_valid; - // MotorRobotArm currently has no verified effort feedback path. The zero - // placeholders above must never be advertised as measured torque. - state.effort_valid = false; return state; } @@ -314,15 +218,11 @@ Result MotorRobotArm::torqueOn() } } emergency_stopped_.store(false); - powered_on_.store(true, std::memory_order_release); return Result::success(); } Result MotorRobotArm::torqueOff() { - // Until every joint reports a successful disable, the aggregate powered - // state is unknown and therefore must not satisfy a require_powered gate. - powered_on_.store(false, std::memory_order_release); for (const auto& joint_name : joint_names_) { auto motor = getMotor_(joint_name); if (!motor) { @@ -368,7 +268,6 @@ Result MotorRobotArm::calibrateZeroQ(const std::string& joint_name) Result MotorRobotArm::emergencyStop() { - motion_generation_.fetch_add(1, std::memory_order_acq_rel); if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } @@ -610,8 +509,6 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti if (const auto stopped = safetyStopResult_("moveJ")) { return *stopped; } - const auto motion_generation = - motion_generation_.load(std::memory_order_acquire); std::string error; if (!validatePositionCommand_(target, error)) { return Result::failure(ArmErrorCode::InvalidArgument, error); @@ -623,12 +520,7 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_); } BusyGuard busy_guard{busy_}; - - if (cancellationRequested(options.cancellation_requested)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[MotorRobotArm] moveJ canceled before planning: " + id_); - } + std::lock_guard lock(mutex_); JointTrajectory samples; if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) { @@ -640,36 +532,19 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti std::vector> motors; motors.reserve(joint_names_.size()); - if (cancellationRequested(options.cancellation_requested)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[MotorRobotArm] moveJ canceled before dispatch: " + id_); - } - { - std::lock_guard lock(mutex_); - if (motion_generation_.load(std::memory_order_acquire) != - motion_generation) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[MotorRobotArm] moveJ canceled before dispatch: " + id_); + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } - for (const auto& joint_name : joint_names_) { - auto motor = getMotor_(joint_name); - if (!motor) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "motor not found for joint: " + joint_name); + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { + if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to set cyclic position mode for joint: " + + joint_name); } - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { - return Result::failure( - ArmErrorCode::CommandFailed, - "failed to set cyclic position mode for joint: " + - joint_name); - } - } - motors.push_back(std::move(motor)); } + motors.push_back(std::move(motor)); } const auto t0 = std::chrono::steady_clock::now(); @@ -687,28 +562,10 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti std::copy_n(sample.velocity.begin(), std::min(sample.velocity.size(), command_velocity.size()), command_velocity.begin()); - if (cancellationRequested(options.cancellation_requested)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[MotorRobotArm] moveJ canceled during execution: " + id_); - } - { - // The local generation and one complete joint frame are ordered - // against stopMotion(). External cancellation is intentionally - // evaluated before taking the device mutex because it is caller code. - std::lock_guard lock(mutex_); - if (motion_generation_.load(std::memory_order_acquire) != - motion_generation) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[MotorRobotArm] moveJ canceled during execution: " + id_); - } - if (!motor_manager_->commandCyclicPositionsAtomic( - motors, sample.position, command_velocity)) { - return Result::failure( - ArmErrorCode::CommandFailed, - "failed to submit atomic cyclic position command"); - } + if (!motor_manager_->commandCyclicPositionsAtomic( + motors, sample.position, command_velocity)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to submit atomic cyclic position command"); } if (k + 1 < samples.size()) { const double next_t = samples[k + 1].time_s > 0.0 @@ -766,7 +623,6 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, Result MotorRobotArm::stopJ(const double acceleration) { - motion_generation_.fetch_add(1, std::memory_order_acq_rel); if (safetyStopRequested_()) { return Result::success(); } @@ -782,8 +638,6 @@ Result MotorRobotArm::moveL(const CartesianPose& target, if (const auto stopped = safetyStopResult_("moveL")) { return *stopped; } - const auto motion_generation = - motion_generation_.load(std::memory_order_acquire); if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } @@ -801,12 +655,6 @@ Result MotorRobotArm::moveL(const CartesianPose& target, } BusyGuard busy_guard{busy_}; - if (cancellationRequested(options.cancellation_requested)) { - return Result::failure( - ArmErrorCode::CommandRejected, - "[MotorRobotArm] moveL canceled before planning: " + id_); - } - std::vector q_start; std::vector qd_now; if (!readArmState_(q_start, qd_now)) { @@ -831,25 +679,13 @@ Result MotorRobotArm::moveL(const CartesianPose& target, << ", executable_path_m=" << trajectory.executable_path_length; } - switch (executeMoveLTrajectory_( - trajectory, - options.cancellation_requested, - motion_generation)) { - case TrajectoryExecutionResult::Completed: - return Result::success(); - case TrajectoryExecutionResult::Canceled: - return Result::failure( - ArmErrorCode::CommandRejected, - "[MotorRobotArm] moveL canceled during execution: " + id_); - case TrajectoryExecutionResult::Failed: - if (const auto stopped = safetyStopResult_("moveL", true)) { - return *stopped; - } - return Result::failure( - ArmErrorCode::CommandFailed, "moveL execution failed"); + if (executeMoveLTrajectory_(trajectory)) { + return Result::success(); } - return Result::failure(ArmErrorCode::CommandFailed, - "moveL execution failed"); + if (const auto stopped = safetyStopResult_("moveL", true)) { + return *stopped; + } + return Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed"); } Result MotorRobotArm::speedL(const CartesianVelocity& velocity, @@ -879,26 +715,16 @@ Result MotorRobotArm::stopL(const std::optional acceleration) Result MotorRobotArm::stopMotion() { - // Revoke position trajectories before stopping the velocity worker. The - // controller fences its old worker and sends zero before joining it. - motion_generation_.fetch_add(1, std::memory_order_acq_rel); - if (cartesian_velocity_controller_) { - cartesian_velocity_controller_->shutdown(); - } - JointVelocityCommand zero; - zero.velocity.assign(joint_names_.size(), 0.0); - return speedJ(zero, 0.0, 0.0); + stopL(0.0); + return stopJ(0.0); } Result MotorRobotArm::shutdown() { - motion_generation_.fetch_add(1, std::memory_order_acq_rel); if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } - JointVelocityCommand zero; - zero.velocity.assign(joint_names_.size(), 0.0); - return speedJ(zero, 0.0, 0.0); + return stopJ(0.0); } Result MotorRobotArm::startServoMode(const ServoOptions& options) @@ -1185,44 +1011,29 @@ bool MotorRobotArm::configureAlgorithms_() return true; } -MotorRobotArm::TrajectoryExecutionResult -MotorRobotArm::executeMoveLTrajectory_( - const CartesianJointTrajectory& trajectory, - const std::function& cancellation_requested, - const std::uint64_t motion_generation) +bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory) { if (trajectory.position.empty() || trajectory.velocity.size() != trajectory.position.size() || trajectory.time.size() != trajectory.position.size()) { - return TrajectoryExecutionResult::Failed; + return false; } if (trajectory.position.size() == 1) { - return cancellationRequested(cancellation_requested) || - motion_generation_.load(std::memory_order_acquire) != - motion_generation - ? TrajectoryExecutionResult::Canceled - : TrajectoryExecutionResult::Completed; + return true; } std::vector> motors; motors.reserve(joint_names_.size()); - if (cancellationRequested(cancellation_requested)) { - return TrajectoryExecutionResult::Canceled; - } { std::lock_guard lock(mutex_); - if (motion_generation_.load(std::memory_order_acquire) != - motion_generation) { - return TrajectoryExecutionResult::Canceled; - } for (const auto& joint_name : joint_names_) { auto motor = getMotor_(joint_name); if (!motor) { - return TrajectoryExecutionResult::Failed; + return false; } if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { - return TrajectoryExecutionResult::Failed; + return false; } } motors.push_back(std::move(motor)); @@ -1232,40 +1043,23 @@ MotorRobotArm::executeMoveLTrajectory_( auto next_deadline = std::chrono::steady_clock::now(); for (std::size_t i = 1; i < trajectory.position.size(); ++i) { if (safetyStopRequested_()) { - return TrajectoryExecutionResult::Failed; + return false; } const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]); const auto& position = trajectory.position[i]; const auto& velocity = trajectory.velocity[i]; if (position.size() != motors.size() || velocity.size() != motors.size()) { - return TrajectoryExecutionResult::Failed; + return false; } - if (cancellationRequested(cancellation_requested)) { - return TrajectoryExecutionResult::Canceled; - } - { - // Keep the local stop decision and the complete joint frame in the - // same critical section as stopMotion()/stopJ(). - std::lock_guard lock(mutex_); - if (motion_generation_.load(std::memory_order_acquire) != - motion_generation) { - return TrajectoryExecutionResult::Canceled; - } - if (!motor_manager_->commandCyclicPositionsAtomic( - motors, position, velocity)) { - return TrajectoryExecutionResult::Failed; - } + if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) { + return false; } next_deadline += std::chrono::duration_cast( std::chrono::duration(dt_segment)); std::this_thread::sleep_until(next_deadline); } - return cancellationRequested(cancellation_requested) || - motion_generation_.load(std::memory_order_acquire) != - motion_generation - ? TrajectoryExecutionResult::Canceled - : TrajectoryExecutionResult::Completed; + return true; } CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityControllerConfig_( diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp index 44148dd5..5ec14ee0 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp @@ -2,17 +2,12 @@ #include #include -#include #include #include -#include #include -#include -#include #include #include #include -#include #include #include #include @@ -104,142 +99,6 @@ struct ArmMujocoConfigCase { const char* config_file; }; -class BlockingCartesianMotionPlanner final : public CartesianMotionPlanner { -public: - bool configureSpeedL(const config::SpeedLPlannerConfig&, std::size_t) override - { - return true; - } - - bool configureMoveL(const config::MoveLPlannerConfig&) override - { - return true; - } - - bool planMoveL(const CartesianPose&, - const std::vector&, - const std::vector&, - double, - double, - double, - FrameType, - CartesianJointTrajectory&) override - { - return false; - } - - bool speedLStep(const CartesianVelocity&, - double, - const std::vector& q_measured, - const std::vector&, - std::vector& qd_command, - FrameType) override - { - std::unique_lock lock(mutex_); - if (step_count_++ == 0) { - first_step_entered_ = true; - condition_.notify_all(); - condition_.wait(lock, [&] { return release_first_step_; }); - } - qd_command.assign(q_measured.size(), 0.4); - return true; - } - - bool updateSpeedLAcceleration(double) override - { - return true; - } - - CartesianVelocity getSpeedLCommandTwistBase() const override - { - return {}; - } - - bool waitForFirstStep(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [&] { return first_step_entered_; }); - } - - void releaseFirstStep() - { - { - std::lock_guard lock(mutex_); - release_first_step_ = true; - } - condition_.notify_all(); - } - -private: - mutable std::mutex mutex_; - std::condition_variable condition_; - std::size_t step_count_{0}; - bool first_step_entered_{false}; - bool release_first_step_{false}; -}; - -class VelocityCommandRecorder { -public: - Result record(const JointVelocityCommand& velocity, double) - { - { - std::lock_guard lock(mutex_); - commands_.push_back(velocity.velocity); - } - condition_.notify_all(); - return Result::success(); - } - - bool waitForZero(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for(lock, timeout, [&] { - return std::any_of(commands_.begin(), commands_.end(), isZero_); - }); - } - - bool waitForNonZeroAfter(const std::size_t index, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for(lock, timeout, [&] { - return index < commands_.size() && - std::any_of(commands_.begin() + index, - commands_.end(), - [](const auto& command) { - return !isZero_(command); - }); - }); - } - - std::size_t size() const - { - std::lock_guard lock(mutex_); - return commands_.size(); - } - - bool allZeroFrom(const std::size_t index) const - { - std::lock_guard lock(mutex_); - return index <= commands_.size() && - std::all_of(commands_.begin() + index, - commands_.end(), isZero_); - } - -private: - static bool isZero_(const std::vector& command) - { - return std::all_of(command.begin(), command.end(), [](const double value) { - return std::abs(value) < 1e-12; - }); - } - - mutable std::mutex mutex_; - std::condition_variable condition_; - std::vector> commands_; -}; - void PrintTo(const ArmMujocoConfigCase& value, std::ostream* os) { *os << value.name << " (" << value.config_file << ")"; @@ -455,240 +314,6 @@ TEST_P(MotorRobotArmMujocoTest, MoveL) EXPECT_LT(outcome.move_l_error, 0.04); } -TEST_P(MotorRobotArmMujocoTest, StopMotionDoesNotWaitForMoveJCancellationCallback) -{ - MotionOptions options; - options.velocity = 0.4; - options.acceleration = 2.0; - - std::mutex cancellation_mutex; - std::condition_variable cancellation_condition; - int cancellation_checks = 0; - bool release_dispatch_check = false; - options.cancellation_requested = [&] { - std::unique_lock lock(cancellation_mutex); - ++cancellation_checks; - cancellation_condition.notify_all(); - if (cancellation_checks == 3) { - cancellation_condition.wait(lock, [&] { - return release_dispatch_check; - }); - } - return false; - }; - - std::vector target(kDof, 0.0); - target[0] = 0.2; - auto motion = std::async(std::launch::async, [&] { - return arm_->moveJ(JointPositionCommand{target}, options); - }); - - bool cancellation_blocked = false; - { - std::unique_lock lock(cancellation_mutex); - cancellation_blocked = cancellation_condition.wait_for( - lock, std::chrono::seconds(2), [&] { - return cancellation_checks >= 3; - }); - } - - auto stop = std::async(std::launch::async, [&] { - return arm_->stopMotion(); - }); - EXPECT_TRUE(cancellation_blocked); - EXPECT_EQ(stop.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - - { - std::lock_guard lock(cancellation_mutex); - release_dispatch_check = true; - } - cancellation_condition.notify_all(); - - const auto motion_result = motion.get(); - const auto stop_result = stop.get(); - EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected) - << motion_result.message; - EXPECT_TRUE(stop_result.ok()) << stop_result.message; -} - -TEST_P(MotorRobotArmMujocoTest, StopMotionDoesNotWaitForMoveLCancellationCallback) -{ - const std::vector initial{ - 0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0}; - MotionOptions joint_options; - joint_options.velocity = 2.8; - joint_options.acceleration = 20.0; - const auto setup = arm_->moveJ( - JointPositionCommand{initial}, joint_options); - ASSERT_TRUE(setup.ok()) << setup.message; - - CartesianPose target = arm_->getTcpPose(); - target.x += 0.05; - MotionOptions options; - options.velocity = 0.4; - options.acceleration = 5.0; - options.jerk = 20.0; - - std::mutex cancellation_mutex; - std::condition_variable cancellation_condition; - int cancellation_checks = 0; - bool release_dispatch_check = false; - options.cancellation_requested = [&] { - std::unique_lock lock(cancellation_mutex); - ++cancellation_checks; - cancellation_condition.notify_all(); - if (cancellation_checks == 3) { - cancellation_condition.wait(lock, [&] { - return release_dispatch_check; - }); - } - return false; - }; - - auto motion = std::async(std::launch::async, [&] { - return arm_->moveL(target, options, FrameType::Base); - }); - - bool cancellation_blocked = false; - { - std::unique_lock lock(cancellation_mutex); - cancellation_blocked = cancellation_condition.wait_for( - lock, std::chrono::seconds(2), [&] { - return cancellation_checks >= 3; - }); - } - - auto stop = std::async(std::launch::async, [&] { - return arm_->stopMotion(); - }); - EXPECT_TRUE(cancellation_blocked); - EXPECT_EQ(stop.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - - { - std::lock_guard lock(cancellation_mutex); - release_dispatch_check = true; - } - cancellation_condition.notify_all(); - - const auto motion_result = motion.get(); - const auto stop_result = stop.get(); - EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected) - << motion_result.message; - EXPECT_TRUE(stop_result.ok()) << stop_result.message; -} - -TEST_P(MotorRobotArmMujocoTest, StopMotionCancelsMoveJWithoutExternalCallback) -{ - MotionOptions options; - options.velocity = 0.1; - options.acceleration = 0.5; - - std::vector target(kDof, 0.0); - target[0] = 0.4; - auto motion = std::async(std::launch::async, [&] { - return arm_->moveJ(JointPositionCommand{target}, options); - }); - - waitFor([&] { return arm_->busy(); }, std::chrono::seconds(1)); - const auto stop_result = arm_->stopMotion(); - const auto motion_result = motion.get(); - - EXPECT_TRUE(stop_result.ok()) << stop_result.message; - EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected) - << motion_result.message; - EXPECT_FALSE(arm_->busy()); -} - -TEST_P(MotorRobotArmMujocoTest, StopMotionCancelsMoveLWithoutExternalCallback) -{ - const std::vector initial{ - 0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0}; - MotionOptions joint_options; - joint_options.velocity = 2.8; - joint_options.acceleration = 20.0; - const auto setup = arm_->moveJ( - JointPositionCommand{initial}, joint_options); - ASSERT_TRUE(setup.ok()) << setup.message; - - CartesianPose target = arm_->getTcpPose(); - target.x += 0.08; - MotionOptions options; - options.velocity = 0.1; - options.acceleration = 1.0; - options.jerk = 5.0; - auto motion = std::async(std::launch::async, [&] { - return arm_->moveL(target, options, FrameType::Base); - }); - - waitFor([&] { return arm_->busy(); }, std::chrono::seconds(1)); - const auto stop_result = arm_->stopMotion(); - const auto motion_result = motion.get(); - - EXPECT_TRUE(stop_result.ok()) << stop_result.message; - EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected) - << motion_result.message; - EXPECT_FALSE(arm_->busy()); -} - -TEST(CartesianVelocityControllerTest, - ShutdownFencesStaleWriteAndLaterSpeedLRestartsWorker) -{ - constexpr std::size_t dof = 2; - auto planner = std::make_shared(); - VelocityCommandRecorder recorder; - CartesianVelocityController controller( - CartesianVelocityController::Config{}, - planner, - dof, - [](std::vector& q, std::vector& qd) { - q.assign(dof, 0.0); - qd.assign(dof, 0.0); - return true; - }, - [&](const JointVelocityCommand& command, const double acceleration) { - return recorder.record(command, acceleration); - }); - - CartesianVelocity velocity; - velocity.vx = 0.1; - const auto first = controller.speedL( - velocity, 0.5, 0.0, FrameType::Base); - ASSERT_TRUE(first.ok()) << first.message; - const bool first_step_entered = - planner->waitForFirstStep(std::chrono::seconds(1)); - if (!first_step_entered) { - planner->releaseFirstStep(); - controller.shutdown(); - FAIL() << "velocity worker did not enter the blocking planner step"; - } - - auto shutdown = std::async(std::launch::async, [&] { - controller.shutdown(); - }); - EXPECT_TRUE(recorder.waitForZero(std::chrono::seconds(1))); - EXPECT_EQ(shutdown.wait_for(std::chrono::milliseconds(20)), - std::future_status::timeout); - - const auto zero_index = recorder.size(); - planner->releaseFirstStep(); - EXPECT_EQ(shutdown.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - shutdown.get(); - EXPECT_TRUE(recorder.allZeroFrom(zero_index)); - EXPECT_FALSE(controller.busy()); - - const auto restart_index = recorder.size(); - const auto restarted = controller.speedL( - velocity, 0.5, 0.0, FrameType::Base); - EXPECT_TRUE(restarted.ok()) << restarted.message; - EXPECT_TRUE(recorder.waitForNonZeroAfter( - restart_index, std::chrono::seconds(1))); - controller.shutdown(); - EXPECT_FALSE(controller.busy()); -} - TEST_P(MotorRobotArmMujocoTest, SpeedL) { MotorRobotArm& arm = *arm_; diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index e7ee5116..f3b927d2 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -2,7 +2,6 @@ #define CMVR_ES_ROBOT_ARM_H #include -#include #include #include #include @@ -29,44 +28,7 @@ public: virtual SafetyMode getSafetyMode() const = 0; virtual ControlMode getControlMode() const = 0; - // Queued actions require synchronous completion, cooperative cancellation - // at the final device-submission boundary, and a bounded typed Stop which - // returns success only after controller idle is confirmed. Backends must - // opt in only after all of these semantics have been validated. - virtual bool supportsActionQueueMotion() const noexcept { return false; } - - // ArmTeleop requires an explicitly reviewed group-servo implementation. - // Existing and vendor arms remain unavailable until their implementations - // override this capability after timing and partial-write validation. - virtual bool supportsTeleopGroupServo() const noexcept { return false; } - virtual JointEffortSource jointEffortSource() const noexcept - { - return JointEffortSource::Unspecified; - } - virtual Result torqueOn() = 0; - // Long-running startup implementations may cooperatively observe loss of - // their control lease. Backends which have not adopted cancellation retain - // the legacy behavior, while still rejecting an already-cancelled request - // before entering the vendor API. - virtual Result torqueOn( - const std::function& cancellation_requested) - { - if (cancellation_requested) { - try { - if (cancellation_requested()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "torqueOn cancelled before execution"); - } - } catch (...) { - return Result::failure( - ArmErrorCode::CommandRejected, - "torqueOn cancellation check failed"); - } - } - return torqueOn(); - } virtual Result torqueOff() = 0; virtual Result calibrateZeroQ(const std::string& joint_name) = 0; @@ -114,27 +76,6 @@ public: FrameType frame = FrameType::Base) = 0; virtual Result stopServoMode() = 0; - // Torque streaming is optional. Backends which do not provide an atomic - // group torque port retain source compatibility and fail explicitly. - virtual Result startTorqueMode(const TorqueServoOptions&) - { - return Result::failure( - ArmErrorCode::UnsupportedCommand, - "torque servo mode is unsupported by this RobotArm"); - } - virtual Result servoTorque(const JointTorqueCommand&) - { - return Result::failure( - ArmErrorCode::UnsupportedCommand, - "torque servo command is unsupported by this RobotArm"); - } - virtual Result stopTorqueMode() - { - return Result::failure( - ArmErrorCode::UnsupportedCommand, - "torque servo mode is unsupported by this RobotArm"); - } - virtual Result connect(const std::string& ip, int port) = 0; virtual Result disconnect() = 0; virtual bool isConnected() const = 0; diff --git a/cmvr-es/devices/arm/robot_arm_factory.h b/cmvr-es/devices/arm/robot_arm_factory.h index e86598e2..0a23742c 100644 --- a/cmvr-es/devices/arm/robot_arm_factory.h +++ b/cmvr-es/devices/arm/robot_arm_factory.h @@ -9,7 +9,6 @@ #include "devices/arm/aubo_arm/aubo_arm.h" #include "devices/arm/huayan_arm/huayan_arm.h" #include "devices/arm/motor_robot_arm/include/motor_robot_arm.h" -#include "devices/arm/ume_robot_arm/include/ume_robot_arm.h" namespace cmvr::device { @@ -37,9 +36,6 @@ public: return nullptr; } - case config::RobotArmConfig::kUme: - return std::make_shared(cfg); - case config::RobotArmConfig::BACKEND_NOT_SET: default: { diff --git a/cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt b/cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt deleted file mode 100644 index f92b6ecd..00000000 --- a/cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt +++ /dev/null @@ -1,92 +0,0 @@ -add_library(ume_robot_arm SHARED - src/damiao_mit_codec.cpp - src/damiao_can_fd_chain.cpp - src/ume_robot_arm.cpp -) - -target_include_directories(ume_robot_arm PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} -) - -target_link_libraries(ume_robot_arm - PUBLIC - cmvr_es::device::canbus - cmvr_es::ik_solver - cmvr_es::common - PRIVATE - cmvr_es::proto - cmvr_es::logging - pthread -) - -add_library(cmvr_es::device::ume_robot_arm ALIAS ume_robot_arm) -install(TARGETS ume_robot_arm LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - add_executable(damiao_mit_codec_test - tests/damiao_mit_codec_test.cpp - ) - target_link_libraries(damiao_mit_codec_test - PRIVATE - cmvr_es::device::ume_robot_arm - gtest - gtest_main - pthread - ) - add_test( - NAME damiao_mit_codec_test - COMMAND damiao_mit_codec_test - ) - set(_ume_robot_arm_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _ume_robot_arm_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(damiao_mit_codec_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_ume_robot_arm_test_environment}" - ) - - add_executable(damiao_can_fd_chain_test - tests/damiao_can_fd_chain_test.cpp - ) - target_link_libraries(damiao_can_fd_chain_test - PRIVATE - cmvr_es::device::ume_robot_arm - gtest - gtest_main - pthread - ) - add_test( - NAME damiao_can_fd_chain_test - COMMAND damiao_can_fd_chain_test - ) - set_tests_properties(damiao_can_fd_chain_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_ume_robot_arm_test_environment}" - ) - - add_executable(ume_robot_arm_test - tests/ume_robot_arm_test.cpp - ) - target_link_libraries(ume_robot_arm_test - PRIVATE - cmvr_es::device::ume_robot_arm - gtest - gtest_main - pthread - ) - target_compile_definitions(ume_robot_arm_test PRIVATE - CMVR_UME_ARM_CONFIG_PATH="${PROJECT_SOURCE_DIR}/cmvr-es/config/devices/arm/ume_arms.pb.txt" - ) - add_test( - NAME ume_robot_arm_test - COMMAND ume_robot_arm_test - ) - set_tests_properties(ume_robot_arm_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_ume_robot_arm_test_environment}" - ) -endif() diff --git a/cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h b/cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h deleted file mode 100644 index 32bfd0e7..00000000 --- a/cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h +++ /dev/null @@ -1,145 +0,0 @@ -#ifndef CMVR_ES_DAMIAO_CAN_FD_CHAIN_H -#define CMVR_ES_DAMIAO_CAN_FD_CHAIN_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "arm/ume_robot_arm/include/damiao_mit_codec.h" -#include "common/types/arm/arm_types.h" - -namespace cmvr::device { - -class AbstractCanbus; - -struct DamiaoJointSpec { - std::string joint_name; - std::uint32_t command_id{0}; - std::uint32_t feedback_id{0}; - std::uint8_t reported_motor_id{0}; - DamiaoMotorModel model{DamiaoMotorModel::Unknown}; - int direction{1}; - double zero_offset_rad{0.0}; - double joint_lower_rad{0.0}; - double joint_upper_rad{0.0}; - double max_velocity_rad_s{0.0}; - double max_torque_nm{0.0}; - std::uint16_t healthy_status_mask{0}; - std::uint8_t max_driver_temperature_raw{0}; - std::uint8_t max_motor_temperature_raw{0}; -}; - -struct DamiaoChainOptions { - bool is_fd{true}; - bool bitrate_switch{true}; - bool hardware_enabled{false}; -}; - -struct DamiaoChainStatistics { - std::uint64_t exchanges{0}; - std::uint64_t deadline_misses{0}; - std::uint64_t unknown_feedback{0}; - std::uint64_t duplicate_feedback{0}; - std::uint64_t rejected_commands{0}; - std::uint64_t protocol_saturations{0}; -}; - -enum class DamiaoChainState : std::uint8_t { - Closed = 0, - Initialized, - Passive, - Armed, - Active, - FaultLatched, - Stopped -}; - -class DamiaoCanFdChain { -public: - DamiaoCanFdChain(std::shared_ptr bus, - std::vector joints, - DamiaoChainOptions options); - ~DamiaoCanFdChain(); - - DamiaoCanFdChain(const DamiaoCanFdChain&) = delete; - DamiaoCanFdChain& operator=(const DamiaoCanFdChain&) = delete; - - Result init(); - Result openPassive(); - Result clearFault(std::chrono::steady_clock::time_point deadline); - Result arm(std::chrono::steady_clock::time_point deadline); - Result setZero(std::size_t joint_index, - std::chrono::steady_clock::time_point deadline); - Result exchange(const DamiaoMitCommand* joint_commands, - std::size_t command_count, - DamiaoJointFeedback* joint_feedback, - std::size_t feedback_count, - std::chrono::steady_clock::time_point deadline); - Result disable() noexcept; - Result latchFault(const std::string& reason) noexcept; - void stop() noexcept; - - DamiaoChainState state() const noexcept { return state_.load(); } - std::size_t size() const noexcept { return joints_.size(); } - bool hardwareEnabled() const noexcept { return options_.hardware_enabled; } - DamiaoChainStatistics statistics() const; - std::string lastError() const; - const std::vector& joints() const noexcept { return joints_; } - -private: - Result validateConfig_() const; - Result sendModeAll_( - DamiaoMode mode, - std::chrono::steady_clock::time_point deadline, - bool expect_feedback); - Result sendModeOne_( - std::size_t joint_index, - DamiaoMode mode, - std::chrono::steady_clock::time_point deadline); - Result receiveCycle_( - DamiaoJointFeedback* feedback, - std::size_t feedback_count, - std::chrono::steady_clock::time_point deadline); - bool sendFrames_( - const std::vector& frames, - std::chrono::steady_clock::time_point deadline) noexcept; - bool sendFramesBestEffort_( - const std::vector& frames) noexcept; - bool feedbackTransportAndHealthValid_( - const CanFrame& frame, - const DamiaoJointSpec& joint, - const DamiaoJointFeedback& feedback) const noexcept; - bool latchFaultAndDisable_(const std::string& reason) noexcept; - bool bestEffortZeroAndDisable_() noexcept; - void setError_(const std::string& error) noexcept; - std::size_t jointIndexForFeedbackId_(std::uint32_t id) const noexcept; - DamiaoMitCommand toMotorCommand_( - const DamiaoJointSpec& spec, - const DamiaoMitCommand& command, - bool& safety_saturated) const noexcept; - void toJointFeedback_(const DamiaoJointSpec& spec, - DamiaoJointFeedback& feedback) const noexcept; - - std::shared_ptr bus_; - std::vector joints_; - DamiaoChainOptions options_; - std::vector tx_frames_; - std::vector rx_frames_; - std::vector feedback_scratch_; - std::vector feedback_seen_; - - mutable std::mutex io_mutex_; - mutable std::mutex status_mutex_; - std::atomic state_{DamiaoChainState::Closed}; - DamiaoChainStatistics statistics_; - std::string last_error_; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_DAMIAO_CAN_FD_CHAIN_H diff --git a/cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h b/cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h deleted file mode 100644 index 447473c7..00000000 --- a/cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h +++ /dev/null @@ -1,135 +0,0 @@ -#ifndef CMVR_ES_DAMIAO_MIT_CODEC_H -#define CMVR_ES_DAMIAO_MIT_CODEC_H - -#include - -#include "canbus/abstract_canbus.h" - -namespace cmvr::device { - -enum class DamiaoMotorModel : std::uint8_t { - Unknown = 0, - DM4310, - DM4310_48V, - DM4340, - DM4340_48V, - DM6006, - DM8006, - DM8009, - DM10010L, - DM10010, - DMH3510, - DMH6215, - DMG6220 -}; - -struct DamiaoMotorLimits { - double q_max_rad{0.0}; - double dq_max_rad_s{0.0}; - double tau_max_nm{0.0}; - - bool valid() const noexcept; -}; - -struct DamiaoMitCommand { - double q_rad{0.0}; - double dq_rad_s{0.0}; - double kp{0.0}; - double kd{0.0}; - double tau_ff_nm{0.0}; -}; - -enum DamiaoSaturation : std::uint8_t { - DAMIAO_SATURATION_NONE = 0, - DAMIAO_SATURATION_Q = 1U << 0U, - DAMIAO_SATURATION_DQ = 1U << 1U, - DAMIAO_SATURATION_KP = 1U << 2U, - DAMIAO_SATURATION_KD = 1U << 3U, - DAMIAO_SATURATION_TAU = 1U << 4U -}; - -enum class DamiaoCodecError : std::uint8_t { - None = 0, - UnknownModel, - InvalidLimits, - NonFiniteInput, - InvalidCanId, - InvalidFrame, - UnexpectedFeedbackId -}; - -struct DamiaoEncodeResult { - DamiaoCodecError error{DamiaoCodecError::None}; - std::uint8_t saturation_mask{DAMIAO_SATURATION_NONE}; - - explicit operator bool() const noexcept - { - return error == DamiaoCodecError::None; - } -}; - -struct DamiaoJointFeedback { - std::uint8_t reported_motor_id{0}; - std::uint8_t status{0}; - std::uint8_t driver_temperature_raw{0}; - std::uint8_t motor_temperature_raw{0}; - double q_rad{0.0}; - double dq_rad_s{0.0}; - double tau_nm{0.0}; - std::int64_t rx_monotonic_ns{0}; - bool valid{false}; -}; - -enum class DamiaoMode : std::uint8_t { - ClearFault, - Enable, - Disable, - SetZero -}; - -class DamiaoMitCodec { -public: - static constexpr double kKpMax = 500.0; - static constexpr double kKdMax = 5.0; - - static DamiaoMotorLimits limitsFor(DamiaoMotorModel model) noexcept; - - static DamiaoEncodeResult encodeMit( - std::uint32_t command_id, - DamiaoMotorModel model, - const DamiaoMitCommand& command, - bool is_fd, - bool bitrate_switch, - CanFrame& frame) noexcept; - - static DamiaoCodecError decodeFeedback( - const CanFrame& frame, - std::uint32_t expected_feedback_id, - DamiaoMotorModel model, - DamiaoJointFeedback& feedback) noexcept; - - static DamiaoCodecError encodeMode( - std::uint32_t command_id, - DamiaoMode mode, - bool is_fd, - bool bitrate_switch, - CanFrame& frame) noexcept; - - // Public for protocol golden-vector tests. The unusual +1 decode behavior - // intentionally matches the legacy UME Python implementation. - static std::uint16_t floatToUint( - double value, - double minimum, - double maximum, - unsigned bits, - bool& saturated) noexcept; - static double uintToFloat( - std::uint16_t value, - double minimum, - double maximum, - unsigned bits) noexcept; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_DAMIAO_MIT_CODEC_H diff --git a/cmvr-es/devices/arm/ume_robot_arm/include/ume_robot_arm.h b/cmvr-es/devices/arm/ume_robot_arm/include/ume_robot_arm.h deleted file mode 100644 index 552a61d5..00000000 --- a/cmvr-es/devices/arm/ume_robot_arm/include/ume_robot_arm.h +++ /dev/null @@ -1,189 +0,0 @@ -#ifndef CMVR_ES_UME_ROBOT_ARM_H -#define CMVR_ES_UME_ROBOT_ARM_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "arm/robot_arm.h" -#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h" -#include "cmvr/config/arm_config/arm_config.pb.h" - -namespace cmvr::device { - -class AbstractCanbus; - -struct UmeArmSample { - static constexpr std::size_t kDof = 8; - - std::uint64_t sequence{0}; - std::int64_t sample_monotonic_ns{0}; - std::array q{}; - std::array dq{}; - std::array tau_measured{}; - std::array motor_rx_time_ns{}; - std::uint8_t valid_mask{0}; -}; - -// One UmeRobotArm represents one physical eight-axis leader arm and one -// SocketCAN-FD interface. The class owns its local high-frequency actuator -// loop; networking and follower kinematics remain outside this device. -class UmeRobotArm final : public RobotArm { -public: - explicit UmeRobotArm(const config::RobotArmConfig& cfg); - UmeRobotArm(const config::RobotArmConfig& cfg, - std::shared_ptr canbus); - ~UmeRobotArm() override; - - std::string typeName() const override { return "UmeRobotArm"; } - bool init() override; - bool start() override; - bool stop() override; - DeviceHealthSnapshot healthSnapshot() override; - - RobotModel getRobotModel() const override { return model_; } - std::size_t getDof() const override { return UmeArmSample::kDof; } - ArmState getRobotState() const override; - JointGroupState getJointState() const override; - Result readSample(UmeArmSample& sample) const; - CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; - RobotMode getRobotMode() const override; - SafetyMode getSafetyMode() const override; - ControlMode getControlMode() const override; - - Result torqueOn() override; - Result torqueOff() override; - Result calibrateZeroQ(const std::string& joint_name) override; - - Result emergencyStop() override; - Result protectiveStop() override; - Result recoverProtectiveStop( - const JointTrajectory&, - const MotionOptions&) override - { - return Result::failure( - ArmErrorCode::UnsupportedCommand, - "protective recovery is not implemented for UmeRobotArm"); - } - Result setSpeedScaling(double scaling) override; - double getSpeedScaling() const override { return 1.0; } - bool isProtectiveStopped() const override - { - return protective_stopped_.load(); - } - bool isEmergencyStopped() const override - { - return emergency_stopped_.load(); - } - bool isFault() const override { return fault_latched_.load(); } - - Result moveJ(const JointPositionCommand& target, - const MotionOptions& options) override; - Result speedJ(const JointVelocityCommand& velocity, - double acceleration, - double duration) override; - Result stopJ(double acceleration) override; - Result moveL(const CartesianPose& target, - const MotionOptions& options, - FrameType frame = FrameType::Base) override; - Result speedL(const CartesianVelocity& velocity, - double acceleration, - double duration, - FrameType frame = FrameType::Base) override; - Result stopL(std::optional acceleration = std::nullopt) override; - Result stopMotion() override; - - Result startServoMode(const ServoOptions& options) override; - Result servoJ(const JointPositionCommand& target) override; - Result servoL(const CartesianPose& target, - FrameType frame = FrameType::Base) override; - Result servoSpeedJ(const JointVelocityCommand& velocity) override; - Result servoSpeedL(const CartesianVelocity& velocity, - FrameType frame = FrameType::Base) override; - Result stopServoMode() override; - - Result startTorqueMode(const TorqueServoOptions& options) override; - Result servoTorque(const JointTorqueCommand& target) override; - Result stopTorqueMode() override; - - Result connect(const std::string& ip, int port) override; - Result disconnect() override; - bool isConnected() const override { return initialized_.load(); } - Result powerOn() override { return torqueOn(); } - Result powerOff() override { return torqueOff(); } - Result brakeRelease() override; - Result shutdown() override; - Result clearFault() override; - Result unlockProtectiveStop() override; - Result loadProgram(const std::string& program_name) override; - Result playProgram() override; - Result pauseProgram() override; - Result stopProgram() override; - - std::vector ik(const std::string& base_link, - const std::string& ee_link, - const CartesianPose& pose) override; - std::shared_ptr kinematicsSolver() const override - { - return ik_solver_; - } - CartesianPose fk(const std::string& base_link, - const std::string& ee_link) override; - CartesianPose fk(bool is_tcp = true) override; - CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; } - bool busy() const override { return powered_on_.load(); } - -private: - void normalizeConfig_(); - bool buildModelAndChain_(); - void controlLoop_() noexcept; - void recordFault_(const std::string& message) noexcept; - Result requirePassive_(const std::string& operation) const; - static Result unsupported_(const std::string& operation); - static std::int64_t monotonicNowNs_() noexcept; - - config::RobotArmConfig cfg_; - config::UmeRobotArmBackendConfig ume_cfg_; - std::shared_ptr canbus_; - std::unique_ptr chain_; - std::vector joint_specs_; - RobotModel model_; - std::shared_ptr ik_solver_; - - mutable std::mutex lifecycle_mutex_; - mutable std::mutex command_mutex_; - mutable std::mutex sample_mutex_; - mutable std::mutex status_mutex_; - mutable std::mutex kinematics_mutex_; - std::thread control_thread_; - - std::array latest_torque_command_{}; - UmeArmSample latest_sample_; - std::string last_error_; - - std::atomic initialized_{false}; - std::atomic running_{false}; - std::atomic torque_mode_{false}; - std::atomic powered_on_{false}; - std::atomic fault_latched_{false}; - std::atomic protective_stopped_{false}; - std::atomic emergency_stopped_{false}; - std::atomic command_ready_{false}; - std::atomic command_sequence_{0}; - std::atomic command_time_ns_{0}; - std::atomic loop_period_ns_{1250000}; - std::atomic command_watchdog_ms_{20}; - std::uint32_t cycle_deadline_us_{900}; - std::uint32_t feedback_watchdog_ms_{20}; - std::uint32_t shutdown_timeout_ms_{50}; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_UME_ROBOT_ARM_H diff --git a/cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp b/cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp deleted file mode 100644 index 2c4830e3..00000000 --- a/cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp +++ /dev/null @@ -1,705 +0,0 @@ -#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h" - -#include -#include -#include -#include - -#include "canbus/abstract_canbus.h" - -namespace cmvr::device { -namespace { - -Result invalidArgument(const std::string& message) -{ - return Result::failure(ArmErrorCode::InvalidArgument, message); -} - -Result commandFailed(const std::string& message) -{ - return Result::failure(ArmErrorCode::CommandFailed, message); -} - -Result notReady(const std::string& message) -{ - return Result::failure(ArmErrorCode::RobotNotReady, message); -} - -} // namespace - -DamiaoCanFdChain::DamiaoCanFdChain( - std::shared_ptr bus, - std::vector joints, - DamiaoChainOptions options) - : bus_(std::move(bus)), - joints_(std::move(joints)), - options_(options), - feedback_scratch_(joints_.size()), - feedback_seen_(joints_.size(), false) -{ - tx_frames_.reserve(joints_.size()); - rx_frames_.reserve(1); -} - -DamiaoCanFdChain::~DamiaoCanFdChain() -{ - stop(); -} - -Result DamiaoCanFdChain::validateConfig_() const -{ - if (!bus_) { - return invalidArgument("Damiao CAN bus is null"); - } - if (joints_.empty()) { - return invalidArgument("Damiao joint list is empty"); - } - - std::unordered_set names; - std::unordered_set command_ids; - std::unordered_set feedback_ids; - std::unordered_set reported_ids; - for (const auto& joint : joints_) { - if (joint.joint_name.empty() || - !names.insert(joint.joint_name).second) { - return invalidArgument("Damiao joint names must be non-empty and unique"); - } - if (joint.command_id == 0 || joint.command_id > 0x7FFU || - !command_ids.insert(joint.command_id).second) { - return invalidArgument("Damiao command IDs must be unique standard CAN IDs"); - } - if (joint.feedback_id == 0 || joint.feedback_id > 0x7FFU || - !feedback_ids.insert(joint.feedback_id).second) { - return invalidArgument("Damiao feedback IDs must be unique standard CAN IDs"); - } - if (joint.reported_motor_id > 0x0FU || - !reported_ids.insert(joint.reported_motor_id).second) { - return invalidArgument("Damiao reported motor IDs must be unique 4-bit values"); - } - if (!DamiaoMitCodec::limitsFor(joint.model).valid()) { - return invalidArgument("Damiao motor model is unknown"); - } - if (joint.direction != 1 && joint.direction != -1) { - return invalidArgument("Damiao joint direction must be +1 or -1"); - } - if (!std::isfinite(joint.zero_offset_rad) || - !std::isfinite(joint.joint_lower_rad) || - !std::isfinite(joint.joint_upper_rad) || - joint.joint_upper_rad <= joint.joint_lower_rad || - !std::isfinite(joint.max_velocity_rad_s) || - joint.max_velocity_rad_s <= 0.0 || - !std::isfinite(joint.max_torque_nm) || - joint.max_torque_nm <= 0.0) { - return invalidArgument("Damiao mechanical limits are invalid"); - } - if (options_.hardware_enabled && - (joint.healthy_status_mask == 0U || - joint.max_driver_temperature_raw == 0U || - joint.max_motor_temperature_raw == 0U)) { - return invalidArgument( - "Damiao hardware enable requires a reviewed feedback-status " - "whitelist and nonzero raw temperature thresholds"); - } - } - return Result::success(); -} - -Result DamiaoCanFdChain::init() -{ - std::lock_guard lock(io_mutex_); - const auto config_result = validateConfig_(); - if (!config_result.ok()) { - setError_(config_result.message); - state_.store(DamiaoChainState::FaultLatched); - return config_result; - } - if (state_.load() != DamiaoChainState::Closed && - state_.load() != DamiaoChainState::Stopped) { - return Result::success(); - } - if (!bus_->init()) { - setError_("failed to initialize Damiao CAN bus"); - state_.store(DamiaoChainState::FaultLatched); - return notReady(lastError()); - } - state_.store(DamiaoChainState::Initialized); - return Result::success(); -} - -Result DamiaoCanFdChain::openPassive() -{ - std::lock_guard lock(io_mutex_); - if (state_.load() != DamiaoChainState::Initialized) { - return notReady("Damiao chain is not initialized"); - } - if (!bus_->start()) { - setError_("failed to start Damiao CAN bus"); - state_.store(DamiaoChainState::FaultLatched); - return notReady(lastError()); - } - // Deliberately no clear-fault or enable command here. - state_.store(DamiaoChainState::Passive); - return Result::success(); -} - -Result DamiaoCanFdChain::clearFault( - const std::chrono::steady_clock::time_point deadline) -{ - std::lock_guard lock(io_mutex_); - if (!options_.hardware_enabled) { - return Result::failure( - ArmErrorCode::CommandRejected, - "Damiao hardware commands are disabled by configuration"); - } - const auto current = state_.load(); - if (current != DamiaoChainState::Passive && - current != DamiaoChainState::FaultLatched) { - return notReady("clearFault requires a disabled Damiao chain"); - } - const auto result = sendModeAll_( - DamiaoMode::ClearFault, deadline, true); - if (!result.ok()) { - state_.store(DamiaoChainState::FaultLatched); - return result; - } - // Clearing a fault never arms the motors. - state_.store(DamiaoChainState::Passive); - return Result::success(); -} - -Result DamiaoCanFdChain::arm( - const std::chrono::steady_clock::time_point deadline) -{ - std::lock_guard lock(io_mutex_); - if (!options_.hardware_enabled) { - return Result::failure( - ArmErrorCode::CommandRejected, - "Damiao hardware commands are disabled by configuration"); - } - if (state_.load() != DamiaoChainState::Passive) { - return notReady("Damiao chain must be passive before arm"); - } - const auto result = sendModeAll_(DamiaoMode::Enable, deadline, true); - if (!result.ok()) { - latchFaultAndDisable_(result.message); - return Result::failure(result.code, lastError()); - } - state_.store(DamiaoChainState::Armed); - return Result::success(); -} - -Result DamiaoCanFdChain::setZero( - const std::size_t joint_index, - const std::chrono::steady_clock::time_point deadline) -{ - std::lock_guard lock(io_mutex_); - if (!options_.hardware_enabled) { - return Result::failure( - ArmErrorCode::CommandRejected, - "Damiao hardware commands are disabled by configuration"); - } - if (state_.load() != DamiaoChainState::Passive) { - return notReady("setZero requires a passive Damiao chain"); - } - return sendModeOne_(joint_index, DamiaoMode::SetZero, deadline); -} - -DamiaoMitCommand DamiaoCanFdChain::toMotorCommand_( - const DamiaoJointSpec& spec, - const DamiaoMitCommand& command, - bool& safety_saturated) const noexcept -{ - DamiaoMitCommand motor = command; - safety_saturated = false; - - const double limited_q = - std::clamp(command.q_rad, spec.joint_lower_rad, spec.joint_upper_rad); - const double limited_dq = - std::clamp(command.dq_rad_s, - -spec.max_velocity_rad_s, spec.max_velocity_rad_s); - const double limited_tau = - std::clamp(command.tau_ff_nm, - -spec.max_torque_nm, spec.max_torque_nm); - safety_saturated = - limited_q != command.q_rad || - limited_dq != command.dq_rad_s || - limited_tau != command.tau_ff_nm; - - motor.q_rad = - spec.direction * (limited_q - spec.zero_offset_rad); - motor.dq_rad_s = spec.direction * limited_dq; - motor.tau_ff_nm = spec.direction * limited_tau; - return motor; -} - -void DamiaoCanFdChain::toJointFeedback_( - const DamiaoJointSpec& spec, - DamiaoJointFeedback& feedback) const noexcept -{ - feedback.q_rad = - spec.direction * feedback.q_rad + spec.zero_offset_rad; - feedback.dq_rad_s = spec.direction * feedback.dq_rad_s; - feedback.tau_nm = spec.direction * feedback.tau_nm; -} - -Result DamiaoCanFdChain::exchange( - const DamiaoMitCommand* joint_commands, - const std::size_t command_count, - DamiaoJointFeedback* joint_feedback, - const std::size_t feedback_count, - const std::chrono::steady_clock::time_point deadline) -{ - std::lock_guard lock(io_mutex_); - if (!joint_commands || !joint_feedback || - command_count != joints_.size() || - feedback_count != joints_.size()) { - { - std::lock_guard status_lock(status_mutex_); - ++statistics_.rejected_commands; - } - return invalidArgument("Damiao exchange dimensions do not match configured joints"); - } - const auto current = state_.load(); - if (current != DamiaoChainState::Armed && - current != DamiaoChainState::Active) { - return notReady("Damiao chain is not armed"); - } - if (std::chrono::steady_clock::now() >= deadline) { - { - std::lock_guard status_lock(status_mutex_); - ++statistics_.deadline_misses; - } - latchFaultAndDisable_( - "Damiao exchange deadline expired before send"); - return Result::failure(ArmErrorCode::Timeout, lastError()); - } - - tx_frames_.clear(); - std::uint64_t saturation_count = 0; - for (std::size_t i = 0; i < joints_.size(); ++i) { - bool safety_saturated = false; - const auto motor_command = - toMotorCommand_(joints_[i], joint_commands[i], safety_saturated); - CanFrame frame; - const auto encoded = DamiaoMitCodec::encodeMit( - joints_[i].command_id, joints_[i].model, motor_command, - options_.is_fd, options_.bitrate_switch, frame); - if (!encoded) { - { - std::lock_guard status_lock(status_mutex_); - ++statistics_.rejected_commands; - } - return invalidArgument("Damiao command failed protocol validation"); - } - if (safety_saturated || - encoded.saturation_mask != DAMIAO_SATURATION_NONE) { - ++saturation_count; - } - tx_frames_.push_back(frame); - } - if (!bus_->discardPendingFrames()) { - latchFaultAndDisable_( - "failed to drain stale Damiao feedback before command"); - return commandFailed(lastError()); - } - if (std::chrono::steady_clock::now() >= deadline) { - { - std::lock_guard status_lock(status_mutex_); - ++statistics_.deadline_misses; - } - latchFaultAndDisable_( - "Damiao exchange deadline expired before command commit"); - return Result::failure(ArmErrorCode::Timeout, lastError()); - } - if (!sendFrames_(tx_frames_, deadline)) { - latchFaultAndDisable_( - "failed to send Damiao MIT command batch before deadline"); - return commandFailed(lastError()); - } - if (std::chrono::steady_clock::now() >= deadline) { - { - std::lock_guard status_lock(status_mutex_); - ++statistics_.deadline_misses; - } - latchFaultAndDisable_( - "Damiao MIT command batch exceeded its deadline"); - return Result::failure(ArmErrorCode::Timeout, lastError()); - } - - const auto receive_result = - receiveCycle_(joint_feedback, feedback_count, deadline); - { - std::lock_guard status_lock(status_mutex_); - ++statistics_.exchanges; - statistics_.protocol_saturations += saturation_count; - } - if (!receive_result.ok()) { - latchFaultAndDisable_(receive_result.message); - return Result::failure(receive_result.code, lastError()); - } - state_.store(DamiaoChainState::Active); - return Result::success(); -} - -Result DamiaoCanFdChain::sendModeAll_( - const DamiaoMode mode, - const std::chrono::steady_clock::time_point deadline, - const bool expect_feedback) -{ - tx_frames_.clear(); - for (const auto& joint : joints_) { - CanFrame frame; - const auto error = DamiaoMitCodec::encodeMode( - joint.command_id, mode, options_.is_fd, - options_.bitrate_switch, frame); - if (error != DamiaoCodecError::None) { - return invalidArgument("failed to encode Damiao lifecycle command"); - } - tx_frames_.push_back(frame); - } - if (expect_feedback && !bus_->discardPendingFrames()) { - return commandFailed( - "failed to drain stale Damiao lifecycle feedback"); - } - if (std::chrono::steady_clock::now() >= deadline) { - return Result::failure( - ArmErrorCode::Timeout, - "Damiao lifecycle deadline expired before command commit"); - } - if (!sendFrames_(tx_frames_, deadline)) { - return commandFailed( - "failed to send Damiao lifecycle command before deadline"); - } - if (std::chrono::steady_clock::now() >= deadline) { - return Result::failure( - ArmErrorCode::Timeout, - "Damiao lifecycle command exceeded its deadline"); - } - if (!expect_feedback) { - return Result::success(); - } - return receiveCycle_( - feedback_scratch_.data(), feedback_scratch_.size(), deadline); -} - -Result DamiaoCanFdChain::sendModeOne_( - const std::size_t joint_index, - const DamiaoMode mode, - const std::chrono::steady_clock::time_point deadline) -{ - if (joint_index >= joints_.size()) { - return invalidArgument("Damiao joint index is out of range"); - } - CanFrame frame; - const auto error = DamiaoMitCodec::encodeMode( - joints_[joint_index].command_id, mode, options_.is_fd, - options_.bitrate_switch, frame); - if (error != DamiaoCodecError::None) { - return invalidArgument("failed to encode Damiao lifecycle command"); - } - tx_frames_.assign(1, frame); - if (!bus_->discardPendingFrames()) { - return commandFailed( - "failed to drain stale Damiao lifecycle feedback"); - } - if (std::chrono::steady_clock::now() >= deadline) { - return Result::failure( - ArmErrorCode::Timeout, - "Damiao lifecycle deadline expired before command commit"); - } - if (!sendFrames_(tx_frames_, deadline)) { - return commandFailed( - "failed to send Damiao lifecycle command before deadline"); - } - if (std::chrono::steady_clock::now() >= deadline) { - return Result::failure( - ArmErrorCode::Timeout, - "Damiao lifecycle command exceeded its deadline"); - } - - std::fill(feedback_seen_.begin(), feedback_seen_.end(), false); - while (std::chrono::steady_clock::now() < deadline) { - rx_frames_.clear(); - int32_t count = 1; - if (bus_->receive(&rx_frames_, &count) != msgs::ErrorCode::OK) { - continue; - } - for (const auto& received : rx_frames_) { - if (received.id != joints_[joint_index].feedback_id) { - continue; - } - DamiaoJointFeedback feedback; - if (DamiaoMitCodec::decodeFeedback( - received, joints_[joint_index].feedback_id, - joints_[joint_index].model, feedback) != - DamiaoCodecError::None || - feedback.reported_motor_id != - joints_[joint_index].reported_motor_id || - !feedbackTransportAndHealthValid_( - received, joints_[joint_index], feedback)) { - return commandFailed("invalid Damiao lifecycle feedback"); - } - return Result::success(); - } - } - return Result::failure( - ArmErrorCode::Timeout, "Damiao lifecycle feedback timed out"); -} - -Result DamiaoCanFdChain::receiveCycle_( - DamiaoJointFeedback* feedback, - const std::size_t feedback_count, - const std::chrono::steady_clock::time_point deadline) -{ - if (!feedback || feedback_count != joints_.size()) { - return invalidArgument("Damiao feedback dimensions do not match"); - } - - std::fill(feedback_seen_.begin(), feedback_seen_.end(), false); - std::size_t received_count = 0; - while (received_count < joints_.size() && - std::chrono::steady_clock::now() < deadline) { - rx_frames_.clear(); - int32_t count = 1; - if (bus_->receive(&rx_frames_, &count) != msgs::ErrorCode::OK) { - continue; - } - for (const auto& frame : rx_frames_) { - const auto index = jointIndexForFeedbackId_(frame.id); - if (index == joints_.size()) { - std::lock_guard status_lock(status_mutex_); - ++statistics_.unknown_feedback; - continue; - } - if (feedback_seen_[index]) { - std::lock_guard status_lock(status_mutex_); - ++statistics_.duplicate_feedback; - continue; - } - DamiaoJointFeedback decoded; - if (DamiaoMitCodec::decodeFeedback( - frame, joints_[index].feedback_id, - joints_[index].model, decoded) != - DamiaoCodecError::None || - decoded.reported_motor_id != - joints_[index].reported_motor_id || - !feedbackTransportAndHealthValid_( - frame, joints_[index], decoded)) { - return commandFailed("Damiao feedback failed validation"); - } - toJointFeedback_(joints_[index], decoded); - feedback[index] = decoded; - feedback_seen_[index] = true; - ++received_count; - } - } - if (received_count != joints_.size()) { - std::lock_guard status_lock(status_mutex_); - ++statistics_.deadline_misses; - return Result::failure( - ArmErrorCode::Timeout, - "Damiao feedback cycle missed its deadline"); - } - return Result::success(); -} - -bool DamiaoCanFdChain::feedbackTransportAndHealthValid_( - const CanFrame& frame, - const DamiaoJointSpec& joint, - const DamiaoJointFeedback& feedback) const noexcept -{ - if (frame.is_fd != options_.is_fd) { - return false; - } - if (options_.is_fd && options_.bitrate_switch && - !frame.bitrate_switch) { - return false; - } - if (joint.healthy_status_mask == 0U) { - // An empty whitelist is tolerated only while the actuator hardware - // gate is closed, so passive software/configuration checks can run. - return !options_.hardware_enabled; - } - if (feedback.status > 0x0FU || - (joint.healthy_status_mask & - static_cast(1U << feedback.status)) == 0U) { - return false; - } - return feedback.driver_temperature_raw <= - joint.max_driver_temperature_raw && - feedback.motor_temperature_raw <= - joint.max_motor_temperature_raw; -} - -bool DamiaoCanFdChain::sendFrames_( - const std::vector& frames, - const std::chrono::steady_clock::time_point deadline) noexcept -{ - if (frames.empty() || - frames.size() > static_cast( - std::numeric_limits::max())) { - return false; - } - int32_t count = static_cast(frames.size()); - return bus_->sendUntil(frames, &count, deadline) == - msgs::ErrorCode::OK && - count == static_cast(frames.size()); -} - -bool DamiaoCanFdChain::sendFramesBestEffort_( - const std::vector& frames) noexcept -{ - if (frames.empty() || - frames.size() > static_cast( - std::numeric_limits::max())) { - return false; - } - int32_t count = static_cast(frames.size()); - return bus_->send(frames, &count) == msgs::ErrorCode::OK && - count == static_cast(frames.size()); -} - -Result DamiaoCanFdChain::disable() noexcept -{ - std::lock_guard lock(io_mutex_); - if (state_.load() == DamiaoChainState::Closed || - state_.load() == DamiaoChainState::Initialized || - state_.load() == DamiaoChainState::Stopped) { - return Result::success(); - } - const bool disabled = bestEffortZeroAndDisable_(); - if (!disabled) { - setError_( - "failed to send all Damiao zero/disable safety frames"); - state_.store(DamiaoChainState::FaultLatched); - return commandFailed(lastError()); - } - if (state_.load() != DamiaoChainState::FaultLatched) { - state_.store(DamiaoChainState::Passive); - } - return Result::success(); -} - -Result DamiaoCanFdChain::latchFault(const std::string& reason) noexcept -{ - std::lock_guard lock(io_mutex_); - if (!latchFaultAndDisable_(reason)) { - return commandFailed(lastError()); - } - return Result::success(); -} - -bool DamiaoCanFdChain::latchFaultAndDisable_( - const std::string& reason) noexcept -{ - state_.store(DamiaoChainState::FaultLatched); - setError_(reason); - if (bestEffortZeroAndDisable_()) { - return true; - } - setError_( - reason + - "; failed to send all Damiao zero/disable safety frames"); - return false; -} - -bool DamiaoCanFdChain::bestEffortZeroAndDisable_() noexcept -{ - if (!bus_ || !options_.hardware_enabled) { - return true; - } - - const auto current = state_.load(); - if (current != DamiaoChainState::Passive && - current != DamiaoChainState::Armed && - current != DamiaoChainState::Active && - current != DamiaoChainState::FaultLatched) { - return true; - } - - bool all_sent = true; - if (current == DamiaoChainState::Armed || - current == DamiaoChainState::Active || - current == DamiaoChainState::FaultLatched) { - tx_frames_.clear(); - for (const auto& joint : joints_) { - DamiaoMitCommand zero; - CanFrame frame; - if (DamiaoMitCodec::encodeMit( - joint.command_id, joint.model, zero, - options_.is_fd, options_.bitrate_switch, frame)) { - tx_frames_.push_back(frame); - } - } - if (!tx_frames_.empty()) { - all_sent = sendFramesBestEffort_(tx_frames_) && all_sent; - } - } - - tx_frames_.clear(); - for (const auto& joint : joints_) { - CanFrame frame; - if (DamiaoMitCodec::encodeMode( - joint.command_id, DamiaoMode::Disable, - options_.is_fd, options_.bitrate_switch, frame) == - DamiaoCodecError::None) { - tx_frames_.push_back(frame); - } - } - if (!tx_frames_.empty()) { - all_sent = sendFramesBestEffort_(tx_frames_) && all_sent; - } - return all_sent; -} - -void DamiaoCanFdChain::stop() noexcept -{ - std::lock_guard lock(io_mutex_); - const auto current = state_.load(); - if (current == DamiaoChainState::Closed || - current == DamiaoChainState::Stopped) { - return; - } - if (!bestEffortZeroAndDisable_()) { - setError_( - "failed to send all Damiao shutdown safety frames"); - } - if (bus_) { - bus_->stop(); - } - state_.store(DamiaoChainState::Stopped); -} - -std::size_t DamiaoCanFdChain::jointIndexForFeedbackId_( - const std::uint32_t id) const noexcept -{ - for (std::size_t i = 0; i < joints_.size(); ++i) { - if (joints_[i].feedback_id == id) { - return i; - } - } - return joints_.size(); -} - -void DamiaoCanFdChain::setError_(const std::string& error) noexcept -{ - try { - std::lock_guard lock(status_mutex_); - last_error_ = error; - } catch (...) { - } -} - -DamiaoChainStatistics DamiaoCanFdChain::statistics() const -{ - std::lock_guard lock(status_mutex_); - return statistics_; -} - -std::string DamiaoCanFdChain::lastError() const -{ - std::lock_guard lock(status_mutex_); - return last_error_; -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp b/cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp deleted file mode 100644 index 58e40e69..00000000 --- a/cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp +++ /dev/null @@ -1,254 +0,0 @@ -#include "arm/ume_robot_arm/include/damiao_mit_codec.h" - -#include -#include -#include - -namespace cmvr::device { -namespace { - -constexpr unsigned kPositionBits = 16; -constexpr unsigned kVelocityBits = 12; -constexpr unsigned kGainBits = 12; -constexpr unsigned kTorqueBits = 12; -constexpr std::uint32_t kCanStandardMaxId = 0x7FFU; - -bool finiteCommand(const DamiaoMitCommand& command) noexcept -{ - return std::isfinite(command.q_rad) && - std::isfinite(command.dq_rad_s) && - std::isfinite(command.kp) && - std::isfinite(command.kd) && - std::isfinite(command.tau_ff_nm); -} - -std::uint8_t modeByte(const DamiaoMode mode) noexcept -{ - switch (mode) { - case DamiaoMode::ClearFault: - return 0xFBU; - case DamiaoMode::Enable: - return 0xFCU; - case DamiaoMode::Disable: - return 0xFDU; - case DamiaoMode::SetZero: - return 0xFEU; - } - return 0; -} - -} // namespace - -bool DamiaoMotorLimits::valid() const noexcept -{ - return std::isfinite(q_max_rad) && q_max_rad > 0.0 && - std::isfinite(dq_max_rad_s) && dq_max_rad_s > 0.0 && - std::isfinite(tau_max_nm) && tau_max_nm > 0.0; -} - -DamiaoMotorLimits DamiaoMitCodec::limitsFor( - const DamiaoMotorModel model) noexcept -{ - switch (model) { - case DamiaoMotorModel::DM4310: - return {12.5, 30.0, 10.0}; - case DamiaoMotorModel::DM4310_48V: - return {12.5, 50.0, 10.0}; - case DamiaoMotorModel::DM4340: - return {12.5, 8.0, 28.0}; - case DamiaoMotorModel::DM4340_48V: - return {12.5, 10.0, 28.0}; - case DamiaoMotorModel::DM6006: - return {12.5, 45.0, 20.0}; - case DamiaoMotorModel::DM8006: - return {12.5, 45.0, 40.0}; - case DamiaoMotorModel::DM8009: - return {12.5, 45.0, 54.0}; - case DamiaoMotorModel::DM10010L: - return {12.5, 25.0, 200.0}; - case DamiaoMotorModel::DM10010: - return {12.5, 20.0, 200.0}; - case DamiaoMotorModel::DMH3510: - return {12.5, 280.0, 1.0}; - case DamiaoMotorModel::DMH6215: - return {12.5, 45.0, 10.0}; - case DamiaoMotorModel::DMG6220: - return {12.5, 45.0, 10.0}; - case DamiaoMotorModel::Unknown: - default: - return {}; - } -} - -std::uint16_t DamiaoMitCodec::floatToUint( - const double value, - const double minimum, - const double maximum, - const unsigned bits, - bool& saturated) noexcept -{ - saturated = value < minimum || value > maximum; - if (!std::isfinite(value) || !std::isfinite(minimum) || - !std::isfinite(maximum) || maximum <= minimum || - bits == 0 || bits > 16) { - saturated = true; - return 0; - } - - const double clamped = std::clamp(value, minimum, maximum); - const std::uint32_t levels = (std::uint32_t{1} << bits) - 1U; - const double normalized = (clamped - minimum) / (maximum - minimum); - return static_cast(normalized * levels); -} - -double DamiaoMitCodec::uintToFloat( - const std::uint16_t value, - const double minimum, - const double maximum, - const unsigned bits) noexcept -{ - if (!std::isfinite(minimum) || !std::isfinite(maximum) || - maximum <= minimum || bits == 0 || bits > 16) { - return 0.0; - } - const double span = maximum - minimum; - const double levels = static_cast(std::uint32_t{1} << bits); - return (static_cast(value) + 1.0) * span / levels + minimum; -} - -DamiaoEncodeResult DamiaoMitCodec::encodeMit( - const std::uint32_t command_id, - const DamiaoMotorModel model, - const DamiaoMitCommand& command, - const bool is_fd, - const bool bitrate_switch, - CanFrame& frame) noexcept -{ - DamiaoEncodeResult result; - const auto limits = limitsFor(model); - if (!limits.valid()) { - result.error = DamiaoCodecError::UnknownModel; - return result; - } - if (!finiteCommand(command)) { - result.error = DamiaoCodecError::NonFiniteInput; - return result; - } - if (command_id > kCanStandardMaxId) { - result.error = DamiaoCodecError::InvalidCanId; - return result; - } - - bool saturated = false; - const auto q = floatToUint( - command.q_rad, -limits.q_max_rad, limits.q_max_rad, - kPositionBits, saturated); - if (saturated) result.saturation_mask |= DAMIAO_SATURATION_Q; - - const auto dq = floatToUint( - command.dq_rad_s, -limits.dq_max_rad_s, limits.dq_max_rad_s, - kVelocityBits, saturated); - if (saturated) result.saturation_mask |= DAMIAO_SATURATION_DQ; - - const auto kp = floatToUint( - command.kp, 0.0, kKpMax, kGainBits, saturated); - if (saturated) result.saturation_mask |= DAMIAO_SATURATION_KP; - - const auto kd = floatToUint( - command.kd, 0.0, kKdMax, kGainBits, saturated); - if (saturated) result.saturation_mask |= DAMIAO_SATURATION_KD; - - const auto tau = floatToUint( - command.tau_ff_nm, -limits.tau_max_nm, limits.tau_max_nm, - kTorqueBits, saturated); - if (saturated) result.saturation_mask |= DAMIAO_SATURATION_TAU; - - frame = {}; - frame.id = command_id; - frame.len = 8; - frame.is_fd = is_fd; - frame.bitrate_switch = is_fd && bitrate_switch; - frame.data[0] = static_cast((q >> 8U) & 0xFFU); - frame.data[1] = static_cast(q & 0xFFU); - frame.data[2] = static_cast((dq >> 4U) & 0xFFU); - frame.data[3] = static_cast( - ((dq & 0xFU) << 4U) | ((kp >> 8U) & 0xFU)); - frame.data[4] = static_cast(kp & 0xFFU); - frame.data[5] = static_cast((kd >> 4U) & 0xFFU); - frame.data[6] = static_cast( - ((kd & 0xFU) << 4U) | ((tau >> 8U) & 0xFU)); - frame.data[7] = static_cast(tau & 0xFFU); - return result; -} - -DamiaoCodecError DamiaoMitCodec::decodeFeedback( - const CanFrame& frame, - const std::uint32_t expected_feedback_id, - const DamiaoMotorModel model, - DamiaoJointFeedback& feedback) noexcept -{ - feedback = {}; - const auto limits = limitsFor(model); - if (!limits.valid()) { - return DamiaoCodecError::UnknownModel; - } - if (frame.is_error_frame || frame.is_remote_frame || - frame.is_extended_id || frame.error_state_indicator || - frame.len != 8) { - return DamiaoCodecError::InvalidFrame; - } - if (frame.id != expected_feedback_id) { - return DamiaoCodecError::UnexpectedFeedbackId; - } - - const std::uint16_t q = - static_cast( - (static_cast(frame.data[1]) << 8U) | - frame.data[2]); - const std::uint16_t dq = - static_cast( - (static_cast(frame.data[3]) << 4U) | - (frame.data[4] >> 4U)); - const std::uint16_t tau = - static_cast( - ((static_cast(frame.data[4]) & 0xFU) << 8U) | - frame.data[5]); - - feedback.reported_motor_id = frame.data[0] & 0x0FU; - feedback.status = frame.data[0] >> 4U; - feedback.driver_temperature_raw = frame.data[6]; - feedback.motor_temperature_raw = frame.data[7]; - feedback.q_rad = - uintToFloat(q, -limits.q_max_rad, limits.q_max_rad, kPositionBits); - feedback.dq_rad_s = - uintToFloat(dq, -limits.dq_max_rad_s, limits.dq_max_rad_s, - kVelocityBits); - feedback.tau_nm = - uintToFloat(tau, -limits.tau_max_nm, limits.tau_max_nm, - kTorqueBits); - feedback.rx_monotonic_ns = frame.rx_monotonic_ns; - feedback.valid = true; - return DamiaoCodecError::None; -} - -DamiaoCodecError DamiaoMitCodec::encodeMode( - const std::uint32_t command_id, - const DamiaoMode mode, - const bool is_fd, - const bool bitrate_switch, - CanFrame& frame) noexcept -{ - if (command_id > kCanStandardMaxId) { - return DamiaoCodecError::InvalidCanId; - } - frame = {}; - frame.id = command_id; - frame.len = 8; - frame.is_fd = is_fd; - frame.bitrate_switch = is_fd && bitrate_switch; - std::memset(frame.data, 0xFF, 7); - frame.data[7] = modeByte(mode); - return DamiaoCodecError::None; -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/ume_robot_arm/src/ume_robot_arm.cpp b/cmvr-es/devices/arm/ume_robot_arm/src/ume_robot_arm.cpp deleted file mode 100644 index 5ba7a926..00000000 --- a/cmvr-es/devices/arm/ume_robot_arm/src/ume_robot_arm.cpp +++ /dev/null @@ -1,1035 +0,0 @@ -#include "arm/ume_robot_arm/include/ume_robot_arm.h" - -#include -#include -#include -#include -#include -#include - -#include - -#include "algorithms/kinematics/ik_solver/ik_solver_factory.h" -#include "canbus/can_client/socket/socket_can_client_raw.h" -#include "common/base/logging/logger.h" -#include "common/math/transform_math.h" - -namespace cmvr::device { -namespace { - -DamiaoMotorModel toDriverModel(const config::DamiaoMotorModel model) -{ - switch (model) { - case config::DAMIAO_MOTOR_MODEL_DM4310: - return DamiaoMotorModel::DM4310; - case config::DAMIAO_MOTOR_MODEL_DM4310_48V: - return DamiaoMotorModel::DM4310_48V; - case config::DAMIAO_MOTOR_MODEL_DM4340: - return DamiaoMotorModel::DM4340; - case config::DAMIAO_MOTOR_MODEL_DM4340_48V: - return DamiaoMotorModel::DM4340_48V; - case config::DAMIAO_MOTOR_MODEL_DM6006: - return DamiaoMotorModel::DM6006; - case config::DAMIAO_MOTOR_MODEL_DM8006: - return DamiaoMotorModel::DM8006; - case config::DAMIAO_MOTOR_MODEL_DM8009: - return DamiaoMotorModel::DM8009; - case config::DAMIAO_MOTOR_MODEL_DM10010L: - return DamiaoMotorModel::DM10010L; - case config::DAMIAO_MOTOR_MODEL_DM10010: - return DamiaoMotorModel::DM10010; - case config::DAMIAO_MOTOR_MODEL_DMH3510: - return DamiaoMotorModel::DMH3510; - case config::DAMIAO_MOTOR_MODEL_DMH6215: - return DamiaoMotorModel::DMH6215; - case config::DAMIAO_MOTOR_MODEL_DMG6220: - return DamiaoMotorModel::DMG6220; - case config::DAMIAO_MOTOR_MODEL_UNKNOWN: - default: - return DamiaoMotorModel::Unknown; - } -} - -constexpr std::uint8_t kAllJointsValid = 0xFFU; - -} // namespace - -UmeRobotArm::UmeRobotArm(const config::RobotArmConfig& cfg) - : cfg_(cfg), - ume_cfg_(cfg.has_ume() ? cfg.ume() - : config::UmeRobotArmBackendConfig{}) -{ - id_ = cfg_.id(); - normalizeConfig_(); - canbus_ = std::make_shared(ume_cfg_.can()); - buildModelAndChain_(); -} - -UmeRobotArm::UmeRobotArm( - const config::RobotArmConfig& cfg, - std::shared_ptr canbus) - : cfg_(cfg), - ume_cfg_(cfg.has_ume() ? cfg.ume() - : config::UmeRobotArmBackendConfig{}), - canbus_(std::move(canbus)) -{ - id_ = cfg_.id(); - normalizeConfig_(); - buildModelAndChain_(); -} - -UmeRobotArm::~UmeRobotArm() -{ - stop(); -} - -void UmeRobotArm::normalizeConfig_() -{ - if (ume_cfg_.control_frequency_hz() == 0U) { - ume_cfg_.set_control_frequency_hz(800U); - } - const auto period_ns = static_cast( - 1000000000ULL / ume_cfg_.control_frequency_hz()); - loop_period_ns_.store(period_ns); - - if (ume_cfg_.cycle_deadline_us() == 0U) { - const auto period_us = - 1000000U / ume_cfg_.control_frequency_hz(); - ume_cfg_.set_cycle_deadline_us( - std::max(100U, period_us * 4U / 5U)); - } - cycle_deadline_us_ = ume_cfg_.cycle_deadline_us(); - - if (ume_cfg_.feedback_watchdog_ms() == 0U) { - ume_cfg_.set_feedback_watchdog_ms(20U); - } - feedback_watchdog_ms_ = ume_cfg_.feedback_watchdog_ms(); - if (ume_cfg_.shutdown_timeout_ms() == 0U) { - ume_cfg_.set_shutdown_timeout_ms(50U); - } - shutdown_timeout_ms_ = ume_cfg_.shutdown_timeout_ms(); - - auto* can = ume_cfg_.mutable_can(); - if (!can->has_enable_fd()) { - can->set_enable_fd(true); - } - if (!can->has_bitrate_switch()) { - can->set_bitrate_switch(true); - } - if (!can->has_receive_own_messages()) { - can->set_receive_own_messages(false); - } - if (!can->has_receive_timeout_us() || - can->receive_timeout_us() == 0U) { - can->set_receive_timeout_us( - std::max(50U, cycle_deadline_us_ / 4U)); - } - if (!can->has_send_timeout_us() || - can->send_timeout_us() == 0U) { - can->set_send_timeout_us( - std::max(50U, cycle_deadline_us_ / 4U)); - } -} - -bool UmeRobotArm::buildModelAndChain_() -{ - if (id_.empty()) { - recordFault_("UME RobotArm id is empty"); - return false; - } - if (!cfg_.has_ume()) { - recordFault_("UME RobotArm backend config is missing"); - return false; - } - if (ume_cfg_.joints_size() != - static_cast(UmeArmSample::kDof)) { - recordFault_("one UME RobotArm must configure exactly eight joints"); - return false; - } - - model_.name = id_; - model_.manufacturer = "UME"; - model_.dof = UmeArmSample::kDof; - joint_specs_.clear(); - joint_specs_.reserve(UmeArmSample::kDof); - model_.joint_names.reserve(UmeArmSample::kDof); - model_.joint_limits.reserve(UmeArmSample::kDof); - - for (const auto& joint : ume_cfg_.joints()) { - if (joint.reported_motor_id() > 0x0FU || - joint.max_driver_temperature_raw() > 0xFFU || - joint.max_motor_temperature_raw() > 0xFFU || - std::any_of( - joint.healthy_feedback_status().begin(), - joint.healthy_feedback_status().end(), - [](const std::uint32_t status) { - return status > 0x0FU; - })) { - recordFault_( - "Damiao feedback identity/health values exceed protocol " - "field widths"); - return false; - } - DamiaoJointSpec spec; - spec.joint_name = joint.joint_name(); - spec.command_id = joint.command_id(); - spec.feedback_id = joint.feedback_id(); - spec.reported_motor_id = - static_cast(joint.reported_motor_id()); - spec.model = toDriverModel(joint.model()); - spec.direction = joint.direction(); - spec.zero_offset_rad = joint.zero_offset_rad(); - spec.joint_lower_rad = joint.joint_lower_rad(); - spec.joint_upper_rad = joint.joint_upper_rad(); - spec.max_velocity_rad_s = joint.max_velocity_rad_s(); - spec.max_torque_nm = joint.max_torque_nm(); - for (const auto status : joint.healthy_feedback_status()) { - if (status <= 0x0FU) { - spec.healthy_status_mask |= - static_cast(1U << status); - } - } - spec.max_driver_temperature_raw = - joint.max_driver_temperature_raw() <= 0xFFU - ? static_cast( - joint.max_driver_temperature_raw()) - : 0U; - spec.max_motor_temperature_raw = - joint.max_motor_temperature_raw() <= 0xFFU - ? static_cast( - joint.max_motor_temperature_raw()) - : 0U; - joint_specs_.push_back(spec); - - model_.joint_names.push_back(spec.joint_name); - JointLimit limit; - limit.lower = spec.joint_lower_rad; - limit.upper = spec.joint_upper_rad; - limit.max_velocity = spec.max_velocity_rad_s; - limit.max_torque = spec.max_torque_nm; - model_.joint_limits.push_back(limit); - } - - DamiaoChainOptions options; - options.is_fd = ume_cfg_.can().enable_fd(); - options.bitrate_switch = ume_cfg_.can().bitrate_switch(); - options.hardware_enabled = ume_cfg_.hardware_enabled(); - chain_ = std::make_unique( - canbus_, joint_specs_, options); - return true; -} - -bool UmeRobotArm::init() -{ - std::lock_guard lock(lifecycle_mutex_); - if (initialized_.load()) { - return true; - } - if (!chain_ || fault_latched_.load()) { - return false; - } - if (!ume_cfg_.can().enable_fd() || - !ume_cfg_.can().bitrate_switch()) { - recordFault_("UME requires SocketCAN-FD with bitrate switching"); - return false; - } - const auto control_frequency_hz = - ume_cfg_.control_frequency_hz(); - const auto configured_period_us = - control_frequency_hz == 0U - ? 0U - : 1000000U / control_frequency_hz; - if (control_frequency_hz < 50U || - control_frequency_hz > 2000U || - configured_period_us == 0U || - cycle_deadline_us_ > configured_period_us) { - recordFault_( - "UME control rate/deadline must be 50..2000 Hz with " - "cycle_deadline_us no greater than one period"); - return false; - } - if (feedback_watchdog_ms_ * 1000ULL < - static_cast(cycle_deadline_us_)) { - recordFault_( - "UME feedback watchdog is shorter than the cycle deadline"); - return false; - } - if (ume_cfg_.can().receive_own_messages()) { - recordFault_("UME must not receive its own CAN command frames"); - return false; - } - if (ume_cfg_.can().receive_timeout_us() > - cycle_deadline_us_) { - recordFault_( - "SocketCAN receive timeout exceeds the UME cycle deadline"); - return false; - } - if (ume_cfg_.can().send_timeout_us() > - cycle_deadline_us_) { - recordFault_( - "SocketCAN send timeout exceeds the UME cycle deadline"); - return false; - } - const auto minimum_shutdown_us = - static_cast(configured_period_us) + - static_cast(cycle_deadline_us_) + - 6ULL * ume_cfg_.can().send_timeout_us(); - if (static_cast(shutdown_timeout_ms_) * 1000ULL < - minimum_shutdown_us) { - recordFault_( - "UME shutdown timeout is shorter than the bounded loop and " - "zero/disable transport budget"); - return false; - } - - auto result = chain_->init(); - if (result.ok()) { - result = chain_->openPassive(); - } - if (!result.ok()) { - recordFault_(result.message); - return false; - } - - if (cfg_.kinematics().algorithm_case() != - config::ArmKinematicsConfig::ALGORITHM_NOT_SET) { - ik_solver_ = cmvr::IKSolverFactory::create(cfg_.kinematics()); - if (!ik_solver_ || !ik_solver_->init()) { - recordFault_("failed to initialize optional UME kinematics"); - chain_->stop(); - return false; - } - } - - initialized_.store(true); - CMVR_LOG(INFO) << "[UmeRobotArm] initialized passive arm '" << id_ - << "', hardware_enabled=" - << ume_cfg_.hardware_enabled(); - return true; -} - -bool UmeRobotArm::start() -{ - std::lock_guard lock(lifecycle_mutex_); - if (!initialized_.load() || fault_latched_.load()) { - return false; - } - if (running_.exchange(true)) { - return true; - } - try { - control_thread_ = std::thread(&UmeRobotArm::controlLoop_, this); - } catch (const std::exception& error) { - running_.store(false); - recordFault_(std::string("failed to start UME loop: ") + error.what()); - return false; - } - return true; -} - -bool UmeRobotArm::stop() -{ - std::lock_guard lock(lifecycle_mutex_); - const auto stop_started = std::chrono::steady_clock::now(); - running_.store(false); - powered_on_.store(false); - torque_mode_.store(false); - command_ready_.store(false); - Result disable_result = Result::success(); - if (chain_) { - disable_result = chain_->disable(); - } - if (control_thread_.joinable()) { - control_thread_.join(); - } - if (chain_) { - chain_->stop(); - } - initialized_.store(false); - const auto elapsed = - std::chrono::steady_clock::now() - stop_started; - if (!disable_result.ok()) { - recordFault_( - "UME shutdown could not enqueue every zero/disable frame: " + - disable_result.message); - return false; - } - if (elapsed > std::chrono::milliseconds(shutdown_timeout_ms_)) { - recordFault_("UME shutdown exceeded configured timeout"); - return false; - } - return true; -} - -DeviceHealthSnapshot UmeRobotArm::healthSnapshot() -{ - DeviceHealthSnapshot health; - if (fault_latched_.load() || - emergency_stopped_.load()) { - health.state = DeviceHealthState::Fault; - } else if (!initialized_.load()) { - health.state = DeviceHealthState::Unknown; - } else if (!running_.load()) { - health.state = DeviceHealthState::Degraded; - } else { - health.state = DeviceHealthState::Healthy; - } - std::lock_guard lock(status_mutex_); - health.error_message = last_error_; - return health; -} - -ArmState UmeRobotArm::getRobotState() const -{ - ArmState state; - state.timestamp = - static_cast(monotonicNowNs_()) / 1000000000.0; - state.robot_mode = getRobotMode(); - state.safety_mode = getSafetyMode(); - state.control_mode = getControlMode(); - state.connected = initialized_.load(); - state.powered_on = powered_on_.load(); - state.brake_released = powered_on_.load(); - state.moving = powered_on_.load(); - state.protective_stopped = protective_stopped_.load(); - state.emergency_stopped = emergency_stopped_.load(); - state.fault = fault_latched_.load(); - state.actual_joint_state = getJointState(); - state.actual_tcp_pose = getTcpPose(); - return state; -} - -JointGroupState UmeRobotArm::getJointState() const -{ - UmeArmSample sample; - readSample(sample); - JointGroupState state; - state.position.assign(sample.q.begin(), sample.q.end()); - state.velocity.assign(sample.dq.begin(), sample.dq.end()); - state.effort.assign( - sample.tau_measured.begin(), sample.tau_measured.end()); - state.sequence = sample.sequence; - state.sample_monotonic_ns = sample.sample_monotonic_ns; - const bool valid = sample.valid_mask == kAllJointsValid; - state.position_valid = valid; - state.velocity_valid = valid; - state.effort_valid = valid; - return state; -} - -Result UmeRobotArm::readSample(UmeArmSample& sample) const -{ - std::lock_guard lock(sample_mutex_); - sample = latest_sample_; - if (sample.valid_mask != kAllJointsValid) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "UME joint feedback snapshot is not complete"); - } - return Result::success(); -} - -CartesianPose UmeRobotArm::getTcpPose(const FrameType frame) const -{ - (void)frame; - if (!ik_solver_) { - return {}; - } - const auto state = getJointState(); - if (!state.position_valid) { - return {}; - } - Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); - std::lock_guard lock(kinematics_mutex_); - if (!ik_solver_->fk(state.position, transform, true)) { - return {}; - } - return common::math::matrixToPose(transform); -} - -RobotMode UmeRobotArm::getRobotMode() const -{ - if (fault_latched_.load()) { - return RobotMode::Fault; - } - if (!initialized_.load()) { - return RobotMode::Disconnected; - } - if (powered_on_.load()) { - return RobotMode::Running; - } - if (!running_.load()) { - return RobotMode::Stopped; - } - return RobotMode::Idle; -} - -SafetyMode UmeRobotArm::getSafetyMode() const -{ - if (emergency_stopped_.load()) { - return SafetyMode::EmergencyStop; - } - if (protective_stopped_.load()) { - return SafetyMode::ProtectiveStop; - } - if (fault_latched_.load()) { - return SafetyMode::Fault; - } - return SafetyMode::Normal; -} - -ControlMode UmeRobotArm::getControlMode() const -{ - return torque_mode_.load() ? ControlMode::Torque : ControlMode::None; -} - -Result UmeRobotArm::torqueOn() -{ - std::lock_guard lock(lifecycle_mutex_); - if (!initialized_.load() || !running_.load() || !chain_) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "UME arm is not initialized and running"); - } - if (fault_latched_.load() || - emergency_stopped_.load() || - protective_stopped_.load()) { - return Result::failure( - ArmErrorCode::RobotInFault, - "UME safety latch prevents torque-on"); - } - if (!torque_mode_.load() || !command_ready_.load()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "start torque mode and publish a command before torque-on"); - } - const auto age_ns = - monotonicNowNs_() - command_time_ns_.load(); - if (age_ns < 0 || - age_ns > static_cast( - command_watchdog_ms_.load()) * 1000000LL) { - return Result::failure( - ArmErrorCode::Timeout, - "initial UME torque command is stale"); - } - - const auto result = chain_->arm( - std::chrono::steady_clock::now() + - std::chrono::microseconds(cycle_deadline_us_)); - if (!result.ok()) { - if (chain_->state() == DamiaoChainState::FaultLatched) { - recordFault_(result.message); - } - return result; - } - powered_on_.store(true); - return Result::success(); -} - -Result UmeRobotArm::torqueOff() -{ - std::lock_guard lock(lifecycle_mutex_); - powered_on_.store(false); - if (!chain_) { - return Result::success(); - } - const auto result = chain_->disable(); - if (!result.ok()) { - recordFault_( - "UME torque-off could not enqueue every zero/disable frame: " + - result.message); - } - return result; -} - -Result UmeRobotArm::requirePassive_(const std::string& operation) const -{ - if (!initialized_.load() || !chain_) { - return Result::failure( - ArmErrorCode::RobotNotReady, - operation + " requires an initialized UME arm"); - } - if (powered_on_.load()) { - return Result::failure( - ArmErrorCode::CommandRejected, - operation + " requires torque-off"); - } - return Result::success(); -} - -Result UmeRobotArm::calibrateZeroQ(const std::string& joint_name) -{ - std::lock_guard lock(lifecycle_mutex_); - const auto passive = requirePassive_("calibrateZeroQ"); - if (!passive.ok()) { - return passive; - } - const auto it = std::find( - model_.joint_names.begin(), model_.joint_names.end(), joint_name); - if (it == model_.joint_names.end()) { - return Result::failure( - ArmErrorCode::InvalidArgument, - "unknown UME joint: " + joint_name); - } - return chain_->setZero( - static_cast( - std::distance(model_.joint_names.begin(), it)), - std::chrono::steady_clock::now() + - std::chrono::microseconds(cycle_deadline_us_)); -} - -Result UmeRobotArm::emergencyStop() -{ - emergency_stopped_.store(true); - powered_on_.store(false); - Result stop_result = Result::success(); - if (chain_) { - stop_result = - chain_->latchFault("UME software emergency stop"); - } - recordFault_( - stop_result.ok() - ? "UME software emergency stop" - : "UME software emergency stop; zero/disable failed: " + - stop_result.message); - return stop_result; -} - -Result UmeRobotArm::protectiveStop() -{ - protective_stopped_.store(true); - powered_on_.store(false); - Result stop_result = Result::success(); - if (chain_) { - stop_result = - chain_->latchFault("UME protective stop"); - } - recordFault_( - stop_result.ok() - ? "UME protective stop" - : "UME protective stop; zero/disable failed: " + - stop_result.message); - return stop_result; -} - -Result UmeRobotArm::setSpeedScaling(const double scaling) -{ - (void)scaling; - return unsupported_("setSpeedScaling"); -} - -Result UmeRobotArm::moveJ( - const JointPositionCommand&, const MotionOptions&) -{ - return unsupported_("moveJ"); -} - -Result UmeRobotArm::speedJ( - const JointVelocityCommand&, double, double) -{ - return unsupported_("speedJ"); -} - -Result UmeRobotArm::stopJ(double) -{ - return unsupported_("stopJ"); -} - -Result UmeRobotArm::moveL( - const CartesianPose&, const MotionOptions&, FrameType) -{ - return unsupported_("moveL"); -} - -Result UmeRobotArm::speedL( - const CartesianVelocity&, double, double, FrameType) -{ - return unsupported_("speedL"); -} - -Result UmeRobotArm::stopL(std::optional) -{ - return unsupported_("stopL"); -} - -Result UmeRobotArm::stopMotion() -{ - return torqueOff(); -} - -Result UmeRobotArm::startServoMode(const ServoOptions&) -{ - return unsupported_("startServoMode"); -} - -Result UmeRobotArm::servoJ(const JointPositionCommand&) -{ - return unsupported_("servoJ"); -} - -Result UmeRobotArm::servoL(const CartesianPose&, FrameType) -{ - return unsupported_("servoL"); -} - -Result UmeRobotArm::servoSpeedJ(const JointVelocityCommand&) -{ - return unsupported_("servoSpeedJ"); -} - -Result UmeRobotArm::servoSpeedL( - const CartesianVelocity&, FrameType) -{ - return unsupported_("servoSpeedL"); -} - -Result UmeRobotArm::stopServoMode() -{ - return unsupported_("stopServoMode"); -} - -Result UmeRobotArm::startTorqueMode( - const TorqueServoOptions& options) -{ - if (!std::isfinite(options.period) || - options.period < 0.00025 || - options.period > 0.02 || - options.period * 1000000.0 < - static_cast(cycle_deadline_us_) || - options.command_watchdog_ms == 0U || - static_cast(options.command_watchdog_ms) * 0.001 < - options.period) { - return Result::failure( - ArmErrorCode::InvalidArgument, - "invalid UME torque servo timing options"); - } - std::lock_guard lock(lifecycle_mutex_); - if (!initialized_.load() || !running_.load()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "UME arm is not initialized and running"); - } - if (powered_on_.load()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "cannot change UME torque timing while powered"); - } - loop_period_ns_.store(static_cast( - std::llround(options.period * 1000000000.0))); - command_watchdog_ms_.store(options.command_watchdog_ms); - torque_mode_.store(true); - command_ready_.store(false); - return Result::success(); -} - -Result UmeRobotArm::servoTorque( - const JointTorqueCommand& target) -{ - if (!target.validForModel(model_)) { - return Result::failure( - ArmErrorCode::InvalidDof, - "UME torque command must contain eight joints"); - } - if (!torque_mode_.load() || fault_latched_.load()) { - return Result::failure( - ArmErrorCode::RobotNotReady, - "UME torque mode is not ready"); - } - for (std::size_t i = 0; i < target.torque.size(); ++i) { - if (!std::isfinite(target.torque[i]) || - std::abs(target.torque[i]) > - joint_specs_[i].max_torque_nm) { - return Result::failure( - ArmErrorCode::OutOfJointLimit, - "UME torque command exceeds configured joint limits"); - } - } - { - std::lock_guard lock(command_mutex_); - std::copy( - target.torque.begin(), target.torque.end(), - latest_torque_command_.begin()); - } - command_time_ns_.store(monotonicNowNs_()); - command_sequence_.fetch_add(1U); - command_ready_.store(true); - return Result::success(); -} - -Result UmeRobotArm::stopTorqueMode() -{ - const auto result = torqueOff(); - torque_mode_.store(false); - command_ready_.store(false); - return result; -} - -Result UmeRobotArm::connect(const std::string&, int) -{ - return unsupported_("connect"); -} - -Result UmeRobotArm::disconnect() -{ - return unsupported_("disconnect"); -} - -Result UmeRobotArm::brakeRelease() -{ - return unsupported_("brakeRelease"); -} - -Result UmeRobotArm::shutdown() -{ - return stop() ? Result::success() - : Result::failure( - ArmErrorCode::CommandFailed, - "failed to stop UME arm"); -} - -Result UmeRobotArm::clearFault() -{ - std::lock_guard lock(lifecycle_mutex_); - const auto passive = requirePassive_("clearFault"); - if (!passive.ok()) { - return passive; - } - if (emergency_stopped_.load()) { - return Result::failure( - ArmErrorCode::RobotInEmergencyStop, - "restart is required after a UME emergency stop"); - } - const auto result = chain_->clearFault( - std::chrono::steady_clock::now() + - std::chrono::microseconds(cycle_deadline_us_)); - if (result.ok()) { - fault_latched_.store(false); - protective_stopped_.store(false); - std::lock_guard status_lock(status_mutex_); - last_error_.clear(); - } - return result; -} - -Result UmeRobotArm::unlockProtectiveStop() -{ - if (powered_on_.load()) { - return Result::failure( - ArmErrorCode::CommandRejected, - "torque-off is required before unlocking a protective stop"); - } - if (fault_latched_.load()) { - return Result::failure( - ArmErrorCode::RobotInFault, - "clear the UME actuator fault before unlocking"); - } - protective_stopped_.store(false); - return Result::success(); -} - -Result UmeRobotArm::loadProgram(const std::string&) -{ - return unsupported_("loadProgram"); -} - -Result UmeRobotArm::playProgram() -{ - return unsupported_("playProgram"); -} - -Result UmeRobotArm::pauseProgram() -{ - return unsupported_("pauseProgram"); -} - -Result UmeRobotArm::stopProgram() -{ - return unsupported_("stopProgram"); -} - -std::vector UmeRobotArm::ik( - const std::string& base_link, - const std::string& ee_link, - const CartesianPose& pose) -{ - (void)base_link; - (void)ee_link; - if (!ik_solver_) { - return {}; - } - auto seed = getJointState().position; - if (seed.size() != getDof()) { - seed.assign(getDof(), 0.0); - } - std::lock_guard lock(kinematics_mutex_); - ik_solver_->update_joints_state(seed); - if (!ik_solver_->ik( - common::math::poseToMatrix(pose), seed, true)) { - return {}; - } - return seed; -} - -CartesianPose UmeRobotArm::fk( - const std::string& base_link, - const std::string& ee_link) -{ - (void)base_link; - (void)ee_link; - return fk(true); -} - -CartesianPose UmeRobotArm::fk(const bool is_tcp) -{ - if (!ik_solver_) { - return {}; - } - const auto state = getJointState(); - if (!state.position_valid) { - return {}; - } - Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); - std::lock_guard lock(kinematics_mutex_); - if (!ik_solver_->fk(state.position, transform, is_tcp)) { - return {}; - } - return common::math::matrixToPose(transform); -} - -void UmeRobotArm::controlLoop_() noexcept -{ - std::array commands{}; - std::array feedback{}; - auto next_tick = std::chrono::steady_clock::now(); - - while (running_.load()) { - const auto period = - std::chrono::nanoseconds(loop_period_ns_.load()); - next_tick += period; - - if (powered_on_.load()) { - const auto now_ns = monotonicNowNs_(); - const auto command_age_ns = - now_ns - command_time_ns_.load(); - if (!command_ready_.load() || - command_age_ns < 0 || - command_age_ns > - static_cast( - command_watchdog_ms_.load()) * 1000000LL) { - powered_on_.store(false); - Result stop_result = Result::success(); - if (chain_) { - stop_result = chain_->latchFault( - "UME torque command watchdog expired"); - } - recordFault_( - stop_result.ok() - ? "UME torque command watchdog expired" - : "UME torque command watchdog expired; " - "zero/disable failed: " + - stop_result.message); - } else { - { - std::lock_guard lock(command_mutex_); - for (std::size_t i = 0; i < commands.size(); ++i) { - commands[i] = {}; - commands[i].tau_ff_nm = - latest_torque_command_[i]; - } - } - const auto cycle_start = - std::chrono::steady_clock::now(); - const auto result = chain_->exchange( - commands.data(), commands.size(), - feedback.data(), feedback.size(), - cycle_start + - std::chrono::microseconds(cycle_deadline_us_)); - if (!result.ok()) { - powered_on_.store(false); - recordFault_(result.message); - } else { - const auto snapshot_now_ns = monotonicNowNs_(); - UmeArmSample sample; - sample.sample_monotonic_ns = snapshot_now_ns; - bool feedback_fresh = true; - for (std::size_t i = 0; i < feedback.size(); ++i) { - sample.q[i] = feedback[i].q_rad; - sample.dq[i] = feedback[i].dq_rad_s; - sample.tau_measured[i] = feedback[i].tau_nm; - sample.motor_rx_time_ns[i] = - feedback[i].rx_monotonic_ns; - if (feedback[i].valid) { - sample.valid_mask |= - static_cast(1U << i); - } - const auto age = - snapshot_now_ns - - feedback[i].rx_monotonic_ns; - if (!feedback[i].valid || - feedback[i].rx_monotonic_ns <= 0 || - age < 0 || - age > static_cast( - feedback_watchdog_ms_) * - 1000000LL) { - feedback_fresh = false; - } - } - if (!feedback_fresh || - sample.valid_mask != kAllJointsValid) { - powered_on_.store(false); - const auto stop_result = chain_->latchFault( - "UME feedback watchdog expired"); - recordFault_( - stop_result.ok() - ? "UME feedback watchdog expired" - : "UME feedback watchdog expired; " - "zero/disable failed: " + - stop_result.message); - } else { - std::lock_guard lock(sample_mutex_); - sample.sequence = - latest_sample_.sequence + 1U; - latest_sample_ = sample; - } - } - } - } - - const auto now = std::chrono::steady_clock::now(); - if (next_tick <= now) { - next_tick = now; - } else { - std::this_thread::sleep_until(next_tick); - } - } -} - -void UmeRobotArm::recordFault_( - const std::string& message) noexcept -{ - fault_latched_.store(true); - powered_on_.store(false); - try { - std::lock_guard lock(status_mutex_); - last_error_ = message; - } catch (...) { - // Health reporting is best effort; safety latches are already set. - } -} - -Result UmeRobotArm::unsupported_( - const std::string& operation) -{ - return Result::failure( - ArmErrorCode::UnsupportedCommand, - "UmeRobotArm does not support " + operation); -} - -std::int64_t UmeRobotArm::monotonicNowNs_() noexcept -{ - return std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()) - .count(); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp b/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp deleted file mode 100644 index 782ce46c..00000000 --- a/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp +++ /dev/null @@ -1,407 +0,0 @@ -#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h" - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "canbus/abstract_canbus.h" - -namespace cmvr::device { -namespace { - -class FakeCanbus final : public AbstractCanbus { -public: - std::string typeName() const override { return "FakeCanbus"; } - bool init() override - { - initialized = true; - return init_result; - } - bool start() override - { - started = start_result; - is_started_ = started; - return started; - } - bool stop() override - { - stopped = true; - started = false; - is_started_ = false; - return true; - } - - msgs::ErrorCode send(const std::vector& frames, - int32_t* frame_num) override - { - if (!started || !frame_num || - *frame_num != static_cast(frames.size())) { - return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; - } - if (!send_result) { - return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; - } - sent_batches.push_back(frames); - if (!scheduled_replies.empty()) { - for (const auto& reply : scheduled_replies.front()) { - replies.push_back(reply); - } - scheduled_replies.pop_front(); - } - return msgs::ErrorCode::OK; - } - - msgs::ErrorCode receive(std::vector* frames, - int32_t* frame_num) override - { - if (!started || !frames || !frame_num || replies.empty()) { - return msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; - } - frames->clear(); - frames->push_back(replies.front()); - replies.pop_front(); - *frame_num = 1; - return msgs::ErrorCode::OK; - } - - bool discardPendingFrames() override - { - ++drain_calls; - if (drain_delay > std::chrono::microseconds::zero()) { - std::this_thread::sleep_for(drain_delay); - } - replies.clear(); - return drain_result; - } - - std::string getErrorString(int32_t) override { return {}; } - - void enqueueReplies(std::vector batch) - { - scheduled_replies.push_back(std::move(batch)); - } - - bool init_result{true}; - bool start_result{true}; - bool initialized{false}; - bool started{false}; - bool stopped{false}; - bool drain_result{true}; - bool send_result{true}; - std::size_t drain_calls{0}; - std::chrono::microseconds drain_delay{0}; - std::vector> sent_batches; - std::deque replies; - std::deque> scheduled_replies; -}; - -DamiaoJointSpec joint(std::string name, - std::uint32_t command_id, - std::uint32_t feedback_id, - std::uint8_t reported_id, - int direction = 1) -{ - DamiaoJointSpec spec; - spec.joint_name = std::move(name); - spec.command_id = command_id; - spec.feedback_id = feedback_id; - spec.reported_motor_id = reported_id; - spec.model = DamiaoMotorModel::DM4310; - spec.direction = direction; - spec.zero_offset_rad = direction == 1 ? 0.1 : -0.2; - spec.joint_lower_rad = -2.0; - spec.joint_upper_rad = 2.0; - spec.max_velocity_rad_s = 3.0; - spec.max_torque_nm = 2.0; - spec.healthy_status_mask = 1U << 0U; - spec.max_driver_temperature_raw = 80U; - spec.max_motor_temperature_raw = 90U; - return spec; -} - -CanFrame feedback(std::uint32_t id, - std::uint8_t reported_id, - std::uint8_t status = 0U) -{ - CanFrame frame; - frame.id = id; - frame.len = 8; - frame.is_fd = true; - frame.bitrate_switch = true; - frame.rx_monotonic_ns = 100; - frame.data[0] = - static_cast((status << 4U) | reported_id); - frame.data[1] = 0x80; - frame.data[2] = 0x00; - frame.data[3] = 0x80; - frame.data[4] = 0x08; - frame.data[5] = 0x00; - frame.data[6] = 30U; - frame.data[7] = 35U; - return frame; -} - -std::chrono::steady_clock::time_point soon() -{ - return std::chrono::steady_clock::now() + - std::chrono::milliseconds(20); -} - -std::size_t countLifecycleByte( - const std::vector>& batches, - const std::uint8_t value) -{ - std::size_t count = 0; - for (const auto& batch : batches) { - for (const auto& frame : batch) { - if (frame.len == 8 && - frame.data[0] == 0xFF && - frame.data[7] == value) { - ++count; - } - } - } - return count; -} - -TEST(DamiaoCanFdChainTest, PassiveOpenNeverEnablesHardware) -{ - auto bus = std::make_shared(); - DamiaoCanFdChain chain( - bus, {joint("J1", 1, 0x11, 1)}, - DamiaoChainOptions{true, true, false}); - - ASSERT_TRUE(chain.init().ok()); - ASSERT_TRUE(chain.openPassive().ok()); - EXPECT_EQ(chain.state(), DamiaoChainState::Passive); - EXPECT_TRUE(bus->sent_batches.empty()); - - const auto arm_result = chain.arm(soon()); - EXPECT_FALSE(arm_result.ok()); - EXPECT_EQ(arm_result.code, ArmErrorCode::CommandRejected); - EXPECT_TRUE(bus->sent_batches.empty()); -} - -TEST(DamiaoCanFdChainTest, ExplicitArmAndExchangeUseUniqueConfiguredFeedback) -{ - auto bus = std::make_shared(); - DamiaoCanFdChain chain( - bus, - {joint("J1", 1, 0x11, 1), - joint("J2", 2, 0x12, 2, -1)}, - DamiaoChainOptions{true, true, true}); - ASSERT_TRUE(chain.init().ok()); - ASSERT_TRUE(chain.openPassive().ok()); - - // A stale invalid frame is already queued before this request. The drain - // must remove it; only replies generated by the subsequent send may be - // accepted. - bus->replies.push_back(feedback(0x11, 1, 2)); - bus->enqueueReplies({ - feedback(0x12, 2), - feedback(0x11, 1), - }); - ASSERT_TRUE(chain.arm(soon()).ok()); - EXPECT_EQ(chain.state(), DamiaoChainState::Armed); - EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 2U); - - bus->enqueueReplies({ - feedback(0x11, 1), - feedback(0x12, 2), - }); - DamiaoMitCommand commands[2]{}; - commands[0].tau_ff_nm = 1.0; - commands[1].tau_ff_nm = -1.0; - DamiaoJointFeedback states[2]{}; - ASSERT_TRUE(chain.exchange( - commands, 2, states, 2, soon()).ok()); - EXPECT_EQ(chain.state(), DamiaoChainState::Active); - EXPECT_TRUE(states[0].valid); - EXPECT_TRUE(states[1].valid); - // J2 has direction=-1 and offset=-0.2. - EXPECT_NEAR(states[1].q_rad, -0.2003814697265625, 1e-12); - EXPECT_NEAR(states[1].dq_rad_s, -0.0146484375, 1e-12); - EXPECT_NEAR(states[1].tau_nm, -0.0048828125, 1e-12); -} - -TEST(DamiaoCanFdChainTest, MissedFeedbackLatchesFaultAndNeverReenables) -{ - auto bus = std::make_shared(); - DamiaoCanFdChain chain( - bus, {joint("J1", 1, 0x11, 1)}, - DamiaoChainOptions{true, true, true}); - ASSERT_TRUE(chain.init().ok()); - ASSERT_TRUE(chain.openPassive().ok()); - bus->enqueueReplies({feedback(0x11, 1)}); - ASSERT_TRUE(chain.arm(soon()).ok()); - - DamiaoMitCommand command; - DamiaoJointFeedback state; - const auto result = chain.exchange( - &command, 1, &state, 1, - std::chrono::steady_clock::now() + - std::chrono::milliseconds(1)); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, ArmErrorCode::Timeout); - EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched); - EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 1U); - EXPECT_GE(countLifecycleByte(bus->sent_batches, 0xFD), 1U); - - // Clearing the fault is explicit and leaves the chain passive. - bus->enqueueReplies({feedback(0x11, 1)}); - ASSERT_TRUE(chain.clearFault(soon()).ok()); - EXPECT_EQ(chain.state(), DamiaoChainState::Passive); - EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 1U); -} - -TEST(DamiaoCanFdChainTest, DuplicateFeedbackCannotSatisfyAGroupCycle) -{ - auto bus = std::make_shared(); - DamiaoCanFdChain chain( - bus, - {joint("J1", 1, 0x11, 1), - joint("J2", 2, 0x12, 2)}, - DamiaoChainOptions{true, true, true}); - ASSERT_TRUE(chain.init().ok()); - ASSERT_TRUE(chain.openPassive().ok()); - bus->enqueueReplies({ - feedback(0x11, 1), - feedback(0x12, 2), - }); - ASSERT_TRUE(chain.arm(soon()).ok()); - - bus->enqueueReplies({ - feedback(0x11, 1), - feedback(0x11, 1), - }); - DamiaoMitCommand commands[2]{}; - DamiaoJointFeedback states[2]{}; - EXPECT_FALSE(chain.exchange( - commands, 2, states, 2, - std::chrono::steady_clock::now() + - std::chrono::milliseconds(1)).ok()); - EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched); - EXPECT_EQ(chain.statistics().duplicate_feedback, 1U); -} - -TEST(DamiaoCanFdChainTest, RejectsUnreviewedStatusAndClassicFrame) -{ - auto bus = std::make_shared(); - DamiaoCanFdChain chain( - bus, {joint("J1", 1, 0x11, 1)}, - DamiaoChainOptions{true, true, true}); - ASSERT_TRUE(chain.init().ok()); - ASSERT_TRUE(chain.openPassive().ok()); - bus->enqueueReplies({feedback(0x11, 1)}); - ASSERT_TRUE(chain.arm(soon()).ok()); - - bus->enqueueReplies({feedback(0x11, 1, 2)}); - DamiaoMitCommand command; - DamiaoJointFeedback state; - EXPECT_FALSE(chain.exchange( - &command, 1, &state, 1, soon()).ok()); - EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched); - - auto second_bus = std::make_shared(); - DamiaoCanFdChain second( - second_bus, {joint("J1", 1, 0x11, 1)}, - DamiaoChainOptions{true, true, true}); - ASSERT_TRUE(second.init().ok()); - ASSERT_TRUE(second.openPassive().ok()); - auto classic = feedback(0x11, 1); - classic.is_fd = false; - classic.bitrate_switch = false; - second_bus->enqueueReplies({classic}); - EXPECT_FALSE(second.arm(soon()).ok()); - EXPECT_EQ(second.state(), DamiaoChainState::FaultLatched); - - auto third_bus = std::make_shared(); - DamiaoCanFdChain third( - third_bus, {joint("J1", 1, 0x11, 1)}, - DamiaoChainOptions{true, true, true}); - ASSERT_TRUE(third.init().ok()); - ASSERT_TRUE(third.openPassive().ok()); - auto error_passive = feedback(0x11, 1); - error_passive.error_state_indicator = true; - third_bus->enqueueReplies({error_passive}); - EXPECT_FALSE(third.arm(soon()).ok()); - EXPECT_EQ(third.state(), DamiaoChainState::FaultLatched); -} - -TEST(DamiaoCanFdChainTest, HardwareEnableRequiresReviewedHealthContract) -{ - auto bus = std::make_shared(); - auto unreviewed = joint("J1", 1, 0x11, 1); - unreviewed.healthy_status_mask = 0U; - DamiaoCanFdChain chain( - bus, {unreviewed}, - DamiaoChainOptions{true, true, true}); - - const auto result = chain.init(); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, ArmErrorCode::InvalidArgument); - EXPECT_FALSE(bus->initialized); -} - -TEST(DamiaoCanFdChainTest, DisableReportsUnconfirmedSafetyFrames) -{ - auto bus = std::make_shared(); - DamiaoCanFdChain chain( - bus, {joint("J1", 1, 0x11, 1)}, - DamiaoChainOptions{true, true, true}); - ASSERT_TRUE(chain.init().ok()); - ASSERT_TRUE(chain.openPassive().ok()); - bus->enqueueReplies({feedback(0x11, 1)}); - ASSERT_TRUE(chain.arm(soon()).ok()); - - bus->send_result = false; - const auto result = chain.disable(); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, ArmErrorCode::CommandFailed); - EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched); -} - -TEST(DamiaoCanFdChainTest, ExpiredDeadlineAfterDrainNeverCommitsEnable) -{ - auto bus = std::make_shared(); - DamiaoCanFdChain chain( - bus, {joint("J1", 1, 0x11, 1)}, - DamiaoChainOptions{true, true, true}); - ASSERT_TRUE(chain.init().ok()); - ASSERT_TRUE(chain.openPassive().ok()); - - bus->drain_delay = std::chrono::milliseconds(3); - bus->enqueueReplies({feedback(0x11, 1)}); - const auto result = chain.arm( - std::chrono::steady_clock::now() + - std::chrono::milliseconds(1)); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, ArmErrorCode::Timeout); - EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched); - EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 0U); - EXPECT_GE(countLifecycleByte(bus->sent_batches, 0xFD), 1U); -} - -TEST(DamiaoCanFdChainTest, ConfigurationRejectsAmbiguousMappings) -{ - auto bus = std::make_shared(); - DamiaoCanFdChain chain( - bus, - {joint("J1", 1, 0x11, 1), - joint("J1", 2, 0x12, 2)}, - DamiaoChainOptions{true, true, false}); - const auto result = chain.init(); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, ArmErrorCode::InvalidArgument); - EXPECT_FALSE(bus->initialized); -} - -} // namespace -} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp b/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp deleted file mode 100644 index 5a3bcef1..00000000 --- a/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp +++ /dev/null @@ -1,150 +0,0 @@ -#include "arm/ume_robot_arm/include/damiao_mit_codec.h" - -#include -#include -#include - -#include - -namespace cmvr::device { -namespace { - -void expectPayload(const CanFrame& frame, - const std::array& expected) -{ - ASSERT_EQ(frame.len, expected.size()); - for (std::size_t i = 0; i < expected.size(); ++i) { - EXPECT_EQ(frame.data[i], expected[i]) << "byte " << i; - } -} - -TEST(DamiaoMitCodecTest, MatchesLegacyPythonGoldenVectors) -{ - CanFrame frame; - DamiaoMitCommand zero; - auto result = DamiaoMitCodec::encodeMit( - 1, DamiaoMotorModel::DM4310, zero, true, true, frame); - ASSERT_TRUE(result); - EXPECT_EQ(result.saturation_mask, DAMIAO_SATURATION_NONE); - EXPECT_TRUE(frame.is_fd); - EXPECT_TRUE(frame.bitrate_switch); - expectPayload(frame, {0x7F, 0xFF, 0x7F, 0xF0, - 0x00, 0x00, 0x07, 0xFF}); - - DamiaoMitCommand nontrivial; - nontrivial.kp = 100.0; - nontrivial.kd = 1.0; - nontrivial.q_rad = 1.25; - nontrivial.dq_rad_s = -2.5; - nontrivial.tau_ff_nm = 3.0; - result = DamiaoMitCodec::encodeMit( - 1, DamiaoMotorModel::DM4310, nontrivial, false, false, frame); - ASSERT_TRUE(result); - expectPayload(frame, {0x8C, 0xCC, 0x75, 0x43, - 0x33, 0x33, 0x3A, 0x65}); -} - -TEST(DamiaoMitCodecTest, ReportsProtocolSaturationWithoutHidingIt) -{ - DamiaoMitCommand command; - command.q_rad = 100.0; - command.dq_rad_s = -100.0; - command.kp = 600.0; - command.kd = -1.0; - command.tau_ff_nm = 100.0; - CanFrame frame; - const auto result = DamiaoMitCodec::encodeMit( - 2, DamiaoMotorModel::DM4310, command, false, false, frame); - ASSERT_TRUE(result); - EXPECT_EQ( - result.saturation_mask, - DAMIAO_SATURATION_Q | DAMIAO_SATURATION_DQ | - DAMIAO_SATURATION_KP | DAMIAO_SATURATION_KD | - DAMIAO_SATURATION_TAU); -} - -TEST(DamiaoMitCodecTest, RejectsNonFiniteInput) -{ - DamiaoMitCommand command; - command.tau_ff_nm = std::numeric_limits::quiet_NaN(); - CanFrame frame; - const auto result = DamiaoMitCodec::encodeMit( - 1, DamiaoMotorModel::DM4310, command, false, false, frame); - EXPECT_FALSE(result); - EXPECT_EQ(result.error, DamiaoCodecError::NonFiniteInput); -} - -TEST(DamiaoMitCodecTest, EncodesLifecycleFramesWithoutEnablingImplicitly) -{ - CanFrame frame; - ASSERT_EQ(DamiaoMitCodec::encodeMode( - 3, DamiaoMode::Enable, true, true, frame), - DamiaoCodecError::None); - expectPayload(frame, {0xFF, 0xFF, 0xFF, 0xFF, - 0xFF, 0xFF, 0xFF, 0xFC}); - - ASSERT_EQ(DamiaoMitCodec::encodeMode( - 3, DamiaoMode::Disable, true, true, frame), - DamiaoCodecError::None); - EXPECT_EQ(frame.data[7], 0xFD); - ASSERT_EQ(DamiaoMitCodec::encodeMode( - 3, DamiaoMode::SetZero, true, true, frame), - DamiaoCodecError::None); - EXPECT_EQ(frame.data[7], 0xFE); - ASSERT_EQ(DamiaoMitCodec::encodeMode( - 3, DamiaoMode::ClearFault, true, true, frame), - DamiaoCodecError::None); - EXPECT_EQ(frame.data[7], 0xFB); -} - -TEST(DamiaoMitCodecTest, DecodesLegacyFeedbackAndRequiresConfiguredId) -{ - CanFrame frame; - frame.id = 0x11; - frame.len = 8; - frame.is_fd = true; - frame.bitrate_switch = true; - frame.rx_monotonic_ns = 1234567; - frame.data[0] = 0xA1; - frame.data[1] = 0x80; - frame.data[2] = 0x00; - frame.data[3] = 0x80; - frame.data[4] = 0x08; - frame.data[5] = 0x00; - frame.data[6] = 40; - frame.data[7] = 41; - - DamiaoJointFeedback feedback; - EXPECT_EQ(DamiaoMitCodec::decodeFeedback( - frame, 0x12, DamiaoMotorModel::DM4310, feedback), - DamiaoCodecError::UnexpectedFeedbackId); - EXPECT_FALSE(feedback.valid); - - ASSERT_EQ(DamiaoMitCodec::decodeFeedback( - frame, 0x11, DamiaoMotorModel::DM4310, feedback), - DamiaoCodecError::None); - EXPECT_TRUE(feedback.valid); - EXPECT_EQ(feedback.reported_motor_id, 1); - EXPECT_EQ(feedback.status, 0x0A); - EXPECT_EQ(feedback.driver_temperature_raw, 40); - EXPECT_EQ(feedback.motor_temperature_raw, 41); - EXPECT_EQ(feedback.rx_monotonic_ns, 1234567); - EXPECT_NEAR(feedback.q_rad, 0.0003814697265625, 1e-12); - EXPECT_NEAR(feedback.dq_rad_s, 0.0146484375, 1e-12); - EXPECT_NEAR(feedback.tau_nm, 0.0048828125, 1e-12); -} - -TEST(DamiaoMitCodecTest, ContainsAllLegacyMotorRanges) -{ - EXPECT_DOUBLE_EQ( - DamiaoMitCodec::limitsFor(DamiaoMotorModel::DM8009).tau_max_nm, - 54.0); - EXPECT_DOUBLE_EQ( - DamiaoMitCodec::limitsFor(DamiaoMotorModel::DMH3510).dq_max_rad_s, - 280.0); - EXPECT_FALSE( - DamiaoMitCodec::limitsFor(DamiaoMotorModel::Unknown).valid()); -} - -} // namespace -} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp b/cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp deleted file mode 100644 index e7241ebc..00000000 --- a/cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp +++ /dev/null @@ -1,338 +0,0 @@ -#include "arm/ume_robot_arm/include/ume_robot_arm.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "canbus/abstract_canbus.h" -#include "common/io/proto_file_io.h" - -#ifndef CMVR_UME_ARM_CONFIG_PATH -#define CMVR_UME_ARM_CONFIG_PATH "" -#endif - -namespace cmvr::device { -namespace { - -class LoopbackDamiaoBus final : public AbstractCanbus { -public: - std::string typeName() const override { return "LoopbackDamiaoBus"; } - bool init() override - { - std::lock_guard lock(mutex); - initialized = true; - return true; - } - bool start() override - { - std::lock_guard lock(mutex); - started = true; - is_started_ = true; - return true; - } - bool stop() override - { - std::lock_guard lock(mutex); - started = false; - is_started_ = false; - return true; - } - - msgs::ErrorCode send( - const std::vector& frames, - int32_t* frame_num) override - { - std::lock_guard lock(mutex); - if (!started || !frame_num || - *frame_num != static_cast(frames.size())) { - return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; - } - if (!send_result) { - return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; - } - sent_batches.push_back(frames); - for (const auto& frame : frames) { - if (frame.id < 1U || frame.id > 8U) { - continue; - } - CanFrame reply; - reply.id = 0x10U + frame.id; - reply.len = 8U; - reply.is_fd = true; - reply.bitrate_switch = true; - reply.rx_monotonic_ns = - std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()) - .count(); - reply.data[0] = static_cast(frame.id); - reply.data[1] = 0x80U; - reply.data[2] = 0x00U; - reply.data[3] = 0x80U; - reply.data[4] = 0x08U; - reply.data[5] = 0x00U; - replies.push_back(reply); - } - return msgs::ErrorCode::OK; - } - - msgs::ErrorCode receive( - std::vector* frames, - int32_t* frame_num) override - { - std::lock_guard lock(mutex); - if (!started || !frames || !frame_num || replies.empty()) { - return msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; - } - frames->clear(); - frames->push_back(replies.front()); - replies.pop_front(); - *frame_num = 1; - return msgs::ErrorCode::OK; - } - - bool discardPendingFrames() override - { - std::lock_guard lock(mutex); - replies.clear(); - return started; - } - - std::string getErrorString(int32_t) override { return {}; } - - void setSendResult(const bool result) - { - std::lock_guard lock(mutex); - send_result = result; - } - - std::size_t lifecycleCount(const std::uint8_t byte) const - { - std::lock_guard lock(mutex); - std::size_t count = 0; - for (const auto& batch : sent_batches) { - for (const auto& frame : batch) { - if (frame.len == 8U && - frame.data[0] == 0xFFU && - frame.data[7] == byte) { - ++count; - } - } - } - return count; - } - - bool initialized{false}; - bool started{false}; - bool send_result{true}; - std::deque replies; - std::vector> sent_batches; - mutable std::mutex mutex; -}; - -config::RobotArmConfig configFor(const bool hardware_enabled) -{ - config::RobotArmConfig cfg; - cfg.set_id("ume_right"); - auto* ume = cfg.mutable_ume(); - ume->set_hardware_enabled(hardware_enabled); - ume->set_control_frequency_hz(800U); - ume->set_cycle_deadline_us(1000U); - ume->set_feedback_watchdog_ms(20U); - auto* can = ume->mutable_can(); - can->set_interface_name("fake-can"); - can->set_enable_fd(true); - can->set_bitrate_switch(true); - can->set_send_timeout_us(100U); - can->set_receive_timeout_us(100U); - can->set_receive_own_messages(false); - for (std::uint32_t i = 1; i <= 8U; ++i) { - auto* joint = ume->add_joints(); - joint->set_joint_name("RJ" + std::to_string(i)); - joint->set_command_id(i); - joint->set_feedback_id(0x10U + i); - joint->set_reported_motor_id(i); - joint->set_model(config::DAMIAO_MOTOR_MODEL_DM4310); - joint->set_direction(1); - joint->set_joint_lower_rad(-2.0); - joint->set_joint_upper_rad(2.0); - joint->set_max_velocity_rad_s(3.0); - joint->set_max_torque_nm(2.0); - joint->add_healthy_feedback_status(0U); - joint->set_max_driver_temperature_raw(80U); - joint->set_max_motor_temperature_raw(90U); - } - return cfg; -} - -bool waitUntil( - const std::function& predicate, - const std::chrono::milliseconds timeout) -{ - const auto deadline = std::chrono::steady_clock::now() + timeout; - while (std::chrono::steady_clock::now() < deadline) { - if (predicate()) { - return true; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - return predicate(); -} - -TEST(UmeRobotArmTest, LifecycleIsPassiveUntilExplicitFreshTorqueCommand) -{ - auto bus = std::make_shared(); - UmeRobotArm arm(configFor(true), bus); - - ASSERT_TRUE(arm.init()); - EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U); - ASSERT_TRUE(arm.start()); - EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U); - - TorqueServoOptions options; - options.period = 0.00125; - options.command_watchdog_ms = 100U; - ASSERT_TRUE(arm.startTorqueMode(options).ok()); - JointTorqueCommand command; - command.torque.assign(8U, 0.0); - ASSERT_TRUE(arm.servoTorque(command).ok()); - ASSERT_TRUE(arm.torqueOn().ok()); - - ASSERT_TRUE(waitUntil( - [&arm] { return arm.getJointState().sequence > 0U; }, - std::chrono::milliseconds(30))); - const auto state = arm.getJointState(); - EXPECT_TRUE(state.position_valid); - EXPECT_TRUE(state.velocity_valid); - EXPECT_TRUE(state.effort_valid); - EXPECT_EQ(state.position.size(), 8U); - EXPECT_EQ(bus->lifecycleCount(0xFCU), 8U); - EXPECT_EQ(arm.getControlMode(), ControlMode::Torque); - - EXPECT_TRUE(arm.stop()); - EXPECT_GE(bus->lifecycleCount(0xFDU), 8U); -} - -TEST(UmeRobotArmTest, StaleCommandLatchesFaultAndNeverReenables) -{ - auto bus = std::make_shared(); - UmeRobotArm arm(configFor(true), bus); - ASSERT_TRUE(arm.init()); - ASSERT_TRUE(arm.start()); - - TorqueServoOptions options; - options.period = 0.001; - options.command_watchdog_ms = 2U; - ASSERT_TRUE(arm.startTorqueMode(options).ok()); - JointTorqueCommand command; - command.torque.assign(8U, 0.0); - ASSERT_TRUE(arm.servoTorque(command).ok()); - ASSERT_TRUE(arm.torqueOn().ok()); - ASSERT_TRUE(waitUntil( - [&arm] { return arm.isFault(); }, - std::chrono::milliseconds(50))); - - EXPECT_FALSE(arm.busy()); - EXPECT_EQ(bus->lifecycleCount(0xFCU), 8U); - EXPECT_GE(bus->lifecycleCount(0xFDU), 8U); - EXPECT_EQ(arm.healthSnapshot().state, DeviceHealthState::Fault); -} - -TEST(UmeRobotArmTest, HardwareGateRejectsEnableWithoutWritingIt) -{ - auto bus = std::make_shared(); - UmeRobotArm arm(configFor(false), bus); - ASSERT_TRUE(arm.init()); - ASSERT_TRUE(arm.start()); - ASSERT_TRUE(arm.startTorqueMode(TorqueServoOptions{}).ok()); - JointTorqueCommand command; - command.torque.assign(8U, 0.0); - ASSERT_TRUE(arm.servoTorque(command).ok()); - - const auto result = arm.torqueOn(); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, ArmErrorCode::CommandRejected); - EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U); -} - -TEST(UmeRobotArmTest, PositionServoIsExplicitlyUnsupported) -{ - auto bus = std::make_shared(); - UmeRobotArm arm(configFor(false), bus); - JointPositionCommand command; - command.position.assign(8U, 0.0); - const auto result = arm.servoJ(command); - EXPECT_EQ(result.code, ArmErrorCode::UnsupportedCommand); -} - -TEST(UmeRobotArmTest, RejectsCycleDeadlineLongerThanControlPeriod) -{ - auto cfg = configFor(false); - cfg.mutable_ume()->set_cycle_deadline_us(2000U); - auto bus = std::make_shared(); - UmeRobotArm arm(cfg, bus); - - EXPECT_FALSE(arm.init()); - EXPECT_FALSE(bus->initialized); - EXPECT_EQ( - arm.healthSnapshot().state, - DeviceHealthState::Fault); -} - -TEST(UmeRobotArmTest, RejectsReportedMotorIdBeforeNarrowingConversion) -{ - auto cfg = configFor(false); - cfg.mutable_ume()->mutable_joints(0)->set_reported_motor_id(257U); - auto bus = std::make_shared(); - UmeRobotArm arm(cfg, bus); - - EXPECT_FALSE(arm.init()); - EXPECT_FALSE(bus->initialized); - EXPECT_EQ(arm.healthSnapshot().state, DeviceHealthState::Fault); -} - -TEST(UmeRobotArmTest, EmergencyStopReportsUnconfirmedDisable) -{ - auto bus = std::make_shared(); - UmeRobotArm arm(configFor(true), bus); - ASSERT_TRUE(arm.init()); - ASSERT_TRUE(arm.start()); - - bus->setSendResult(false); - const auto result = arm.emergencyStop(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, ArmErrorCode::CommandFailed); - EXPECT_TRUE(arm.isEmergencyStopped()); - EXPECT_TRUE(arm.isFault()); - EXPECT_NE( - arm.healthSnapshot().error_message.find("zero/disable failed"), - std::string::npos); -} - -TEST(UmeRobotArmTest, CheckedInDualArmConfigParsesAndKeepsHardwareDisabled) -{ - config::ArmRootConfig root; - ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( - CMVR_UME_ARM_CONFIG_PATH, &root)); - ASSERT_EQ(root.arm().robot_arms_size(), 2); - for (const auto& arm : root.arm().robot_arms()) { - ASSERT_TRUE(arm.has_ume()); - EXPECT_EQ(arm.ume().joints_size(), 8); - EXPECT_FALSE(arm.ume().hardware_enabled()); - EXPECT_TRUE(arm.ume().can().enable_fd()); - EXPECT_TRUE(arm.ume().can().bitrate_switch()); - EXPECT_GT(arm.ume().can().send_timeout_us(), 0U); - EXPECT_FALSE(arm.ume().can().receive_own_messages()); - } -} - -} // namespace -} // namespace cmvr::device diff --git a/cmvr-es/devices/biohead/abstract_biohead.h b/cmvr-es/devices/biohead/abstract_biohead.h index 26ac0d35..aaf28658 100644 --- a/cmvr-es/devices/biohead/abstract_biohead.h +++ b/cmvr-es/devices/biohead/abstract_biohead.h @@ -1,11 +1,6 @@ #ifndef ABSTRACT_BIOHEAD_H #define ABSTRACT_BIOHEAD_H #pragma once - -#include -#include -#include - #include "../abstract_device.h" namespace cmvr::device { @@ -63,8 +58,6 @@ namespace cmvr::device { // 抽象头部类 class AbstractBiohead : public AbstractDevice { public: - using OperationalToken = std::uint64_t; - AbstractBiohead() = default; ~AbstractBiohead() override = default; @@ -83,140 +76,20 @@ 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_; - 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; - } + std::atomic emergency_stop_requested = false; + - 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 dd0d31a1..61d301ba 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h +++ b/cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h @@ -4,12 +4,10 @@ #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 { @@ -21,7 +19,7 @@ namespace cmvr::device { class BioHeadRobot : public AbstractBiohead { public: explicit BioHeadRobot(const config::BioHeadRobotConfig &config); - ~BioHeadRobot() override; + ~BioHeadRobot() override = default; std::string typeName() const override { return "BioHeadRobot"; } bool init() override; @@ -32,25 +30,7 @@ namespace cmvr::device { void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) override; void speakstart() override; void speakstop() override; - 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 speakthread(); void expressionHappy()override; void expressionSurprised()override; @@ -63,23 +43,11 @@ namespace cmvr::device { private: // 内部方法 void parseConfig(const config::BioHeadRobotConfig &config); - bool sendServoCommands( - const std::vector& targets, - uint16_t duration_ms, - bool force = false); - bool sendRawIfCurrent( - OperationalToken token, - const std::vector& raw_data); + void sendServoCommands( const std::vector& targets, uint16_t duration_ms); uint16_t angleToRaw(double angle); double normalizeToAngle(double normalized, size_t index); - 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); + void sendExpression(const std::vector& device_64_angles, const std::vector& device_65_angles, int step_ms); @@ -101,9 +69,8 @@ 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 7fd504a6..5e86e722 100644 --- a/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp @@ -21,11 +21,6 @@ BioHeadRobot::BioHeadRobot(const config::BioHeadRobotConfig &config) { } -BioHeadRobot::~BioHeadRobot() -{ - speakstop(); -} - bool BioHeadRobot::init() { @@ -117,36 +112,13 @@ 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."; - (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); - }); + sendServoCommands(current_joints_, 100); // 快速下发当前角度 } void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, double vel, double acc) { @@ -188,10 +160,8 @@ void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, do for (size_t i = 0; i < joints.size(); ++i) { CMVR_LOG(INFO) << "Joint[" << i << "] = " << joints[i]; // 打印每个关节的角度 } - const uint16_t duration = vel > 0.0 - ? static_cast(1000.0 / vel) - : 0U; - (void)sendServoCommands(joints, duration); + uint16_t duration = static_cast(1000.0 / vel); + sendServoCommands(joints, duration); } @@ -238,30 +208,17 @@ void BioHeadRobot::streamFacialPose(FacialExpressionState& expression_state, dou CMVR_LOG(INFO) << "嘴角3=： " << ": " << joints[15]; CMVR_LOG(INFO) << "嘴角4=： " << ": " << joints[16]; - const uint16_t duration = vel > 0.0 - ? static_cast(1000.0 / vel) - : 0U; - (void)sendServoCommands(joints, duration); + uint16_t duration = static_cast(1000.0 / vel); + 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 operationalActivityCurrent_(token); + return; } // 检查 channels 中是否有 65:8 和 65:9 @@ -272,9 +229,12 @@ bool BioHeadRobot::startSpeaking(const OperationalToken token) } if (!found8 || !found9) { CMVR_LOG(ERROR) << "[BioHeadRobot] Required servo channels not found (addr 65 ch 8/9). speakstart aborted."; - return false; + return; } + // 启动线程 + speak_running_.store(true); + // 清理旧线程（若有） if (speak_thread_ && speak_thread_->joinable()) { try { @@ -285,24 +245,19 @@ bool BioHeadRobot::startSpeaking(const OperationalToken token) speak_thread_.reset(); } - 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; - } + speak_thread_ = std::make_shared(&BioHeadRobot::speakthread, this); CMVR_LOG(INFO) << "[BioHeadRobot] speak thread started."; - return true; } void BioHeadRobot::speakstop() { - std::lock_guard lock(speak_mutex_); - speak_running_.store(false, std::memory_order_release); + { + if (!speak_running_.load()) { + CMVR_LOG(INFO) << "[BioHeadRobot] speak thread not running."; + return; + } + speak_running_.store(false); + } + // 唤醒线程（如果在 wait 中） // join 并清理线程对象 if (speak_thread_) { @@ -320,20 +275,7 @@ void BioHeadRobot::speakstop() { CMVR_LOG(INFO) << "[BioHeadRobot] speak thread stopped."; } -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) { +void BioHeadRobot::speakthread() { CMVR_LOG(INFO) << "[BioHeadRobot] speakthread running."; // 固定参数 @@ -371,11 +313,7 @@ void BioHeadRobot::speakthread(const OperationalToken token) { } // 以当前角度为基准 - std::vector base; - { - std::lock_guard lock(stateMutex_); - base = current_joints_; - } + std::vector base = current_joints_; if (base.size() != channels_.size()) { base.resize(channels_.size(), 90.0); } @@ -408,8 +346,7 @@ void BioHeadRobot::speakthread(const OperationalToken token) { double current_random_factor = 0.0; const double random_update_interval = 0.2; // 每0.2秒更新一次随机扰动 - while (speak_running_.load(std::memory_order_acquire) && - operationalActivityCurrent_(token)) { + while (speak_running_.load()) { auto now = std::chrono::steady_clock::now(); double t = std::chrono::duration_cast>(now - start).count(); @@ -515,9 +452,7 @@ void BioHeadRobot::speakthread(const OperationalToken token) { } // 下发 - if (!sendRawIfCurrent(token, raw_data)) { - break; - } + serial_->sendRawServoData(raw_data); // 控制循环频率 std::this_thread::sleep_for(std::chrono::milliseconds(step_ms)); @@ -548,23 +483,12 @@ void BioHeadRobot::speakthread(const OperationalToken token) { } } - 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); + serial_->sendRawServoData(restore_data); + CMVR_LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose."; } -bool BioHeadRobot::sendExpression( - const OperationalToken token, - const std::vector& device_64_angles, - const std::vector& device_65_angles, - const int step_ms) -{ +void BioHeadRobot::sendExpression(const std::vector& device_64_angles, const std::vector& device_65_angles, int step_ms) { std::vector raw_data; // 处理设备64角度 @@ -587,20 +511,8 @@ bool BioHeadRobot::sendExpression( raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节 } - 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; - } - } + serial_->sendRawServoData(raw_data); + std::this_thread::sleep_for(std::chrono::seconds(5)); // 恢复到原始角度 // 设备64角度（10通道） @@ -630,78 +542,50 @@ bool BioHeadRobot::sendExpression( raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节 } - return sendRawIfCurrent(token, raw_data_neutral); + serial_->sendRawServoData(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}; - return sendExpression(token, device_64_angles, device_65_angles, 0); + sendExpression(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}; - return sendExpression(token, device_64_angles, device_65_angles, 0); + sendExpression(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}; - return sendExpression(token, device_64_angles, device_65_angles, 0); + sendExpression(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}; - return sendExpression(token, device_64_angles, device_65_angles, 0); + sendExpression(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}; - return sendExpression(token, device_64_angles, device_65_angles, 0); + sendExpression(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}; - return sendExpression(token, device_64_angles, device_65_angles, 0); + sendExpression(device_64_angles, device_65_angles, 0); } -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; - } +void BioHeadRobot::sendServoCommands(const std::vector& targets, uint16_t duration_ms) { std::vector addrs, chs; std::vector raws; @@ -714,14 +598,17 @@ bool BioHeadRobot::sendServoCommands( 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() && !force) { - return true; + if (raws.empty()) { + return; } std::vector raw_data; // 原始格式处理 @@ -753,41 +640,7 @@ bool BioHeadRobot::sendServoCommands( - 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; + serial_->sendRawServoData(raw_data); } @@ -798,3 +651,4 @@ uint16_t BioHeadRobot::angleToRaw(double angle) { } // namespace cmvr::device + diff --git a/cmvr-es/devices/camera/abstract_camera.h b/cmvr-es/devices/camera/abstract_camera.h index 7d28efca..a3082602 100644 --- a/cmvr-es/devices/camera/abstract_camera.h +++ b/cmvr-es/devices/camera/abstract_camera.h @@ -131,12 +131,6 @@ namespace cmvr::device { virtual bool startStreaming() {return true;} virtual void stopStreaming() {} - virtual bool startOperationalActivity() { return start(); } - // Stops activity started by CameraService::StartCamera without - // tearing down the device lifecycle. Implementations must return true - // only after the camera is quiescent and a later start() can resume it - // without another init(). Unsupported backends fail closed. - virtual bool stopOperationalActivity() { return false; } virtual bool controlPtz(PtzCommand command, bool stop, int speed) { (void)command; (void)stop; diff --git a/cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h b/cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h index 39adf98e..0fd6e27f 100644 --- a/cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h +++ b/cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h @@ -36,7 +36,6 @@ public: bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override; bool startStreaming() override; void stopStreaming() override; - bool stopOperationalActivity() override; bool controlPtz(PtzCommand command, bool stop, int speed) override; bool executeJsonCommand(const std::string& request_json, std::string& response_json) override; bool requestKeyFrame() override; @@ -57,7 +56,7 @@ private: void releaseSdk_(); bool login_(); bool startPreview_(); - bool stopPreview_(); + void stopPreview_(); bool requestKeyFrame_(); void stopRecordingUnlocked_(); void fillIntrinsics_(Rs2Intrinsics& intrinsics) const; diff --git a/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp b/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp index 265d7e62..e2d92821 100644 --- a/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp +++ b/cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp @@ -323,7 +323,7 @@ bool HikvisionCamera::start() if (state_.is_opened) { return true; } - if (user_id_ < 0 && !login_()) { + if (!login_()) { return false; } if (!startPreview_()) { @@ -348,7 +348,7 @@ bool HikvisionCamera::stop() state_.is_streaming = false; stream_count_ = 0; resetStreamState_(); - (void)stopPreview_(); + stopPreview_(); if (user_id_ >= 0) { NET_DVR_Logout(user_id_); user_id_ = -1; @@ -544,23 +544,6 @@ void HikvisionCamera::stopStreaming() } } -bool HikvisionCamera::stopOperationalActivity() -{ - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - - if (stream_count_ != 0 || state_.is_streaming || state_.is_recording) { - return false; - } - if (!stopPreview_()) { - setError_(sdkError_("NET_DVR_StopRealPlay")); - return false; - } - state_.is_opened = false; - return real_handle_ < 0 && !state_.is_streaming && - !state_.is_recording; -} - bool HikvisionCamera::controlPtz(PtzCommand command, bool stop, int speed) { std::lock_guard lock(ctrl_mtx_); @@ -934,7 +917,7 @@ bool HikvisionCamera::startPreview_() return true; } -bool HikvisionCamera::stopPreview_() +void HikvisionCamera::stopPreview_() { const int preview_handle = real_handle_; { @@ -947,12 +930,9 @@ bool HikvisionCamera::stopPreview_() awaiting_key_frame_ = false; } if (preview_handle >= 0) { - if (!NET_DVR_StopRealPlay(preview_handle)) { - return false; - } + NET_DVR_StopRealPlay(preview_handle); real_handle_ = -1; } - return true; } void HikvisionCamera::fillIntrinsics_(Rs2Intrinsics& intrinsics) const diff --git a/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp b/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp index fc18e9e5..1aa72441 100644 --- a/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp +++ b/cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp @@ -269,30 +269,11 @@ bool testCallbackPublicationLifecycle() CHECK_TRUE(frame.sequence == 0); CHECK_TRUE(frame.codec_config_generation == 3); - camera.stopStreaming(); - CHECK_TRUE(camera.stopOperationalActivity()); - CHECK_TRUE(g_stop_callback_count.load() == 1); - cmvr::device::CameraState stopped_state{}; - camera.getState(stopped_state); - CHECK_TRUE(stopped_state.is_initialized); - CHECK_TRUE(!stopped_state.is_opened); - CHECK_TRUE(!stopped_state.is_streaming); - - // Operational stop keeps the SDK/login lifecycle reusable. start() only - // recreates the preview pipeline and can stream again without init(). - CHECK_TRUE(camera.startOperationalActivity()); - CHECK_TRUE(camera.startStreaming()); - emitIFrame(); - CHECK_TRUE(camera.waitEncodedFrame( - frame, cursor, std::chrono::milliseconds(50))); - CHECK_TRUE(frame.stream_epoch == 3); - camera.stopStreaming(); - // The fake StopRealPlay invokes the SDK callback synchronously. stop() // owns ctrl_mtx_ here, proving the callback neither takes that mutex nor // publishes after the preview handle has been invalidated. CHECK_TRUE(camera.stop()); - CHECK_TRUE(g_stop_callback_count.load() == 2); + CHECK_TRUE(g_stop_callback_count.load() == 1); CHECK_TRUE(!camera.waitEncodedFrame( frame, cursor, std::chrono::milliseconds(10))); return true; diff --git a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h index d8c0bfde..501b0da8 100644 --- a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h +++ b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h @@ -50,8 +50,6 @@ public: void getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) override; bool startStreaming() override; void stopStreaming() override; - bool startOperationalActivity() override; - bool stopOperationalActivity() override; bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override; private: @@ -93,8 +91,6 @@ private: uint64_t last_frame_id_{0}; bool has_last_frame_id_{false}; size_t stream_frame_index_{0}; - std::size_t stream_count_{0}; - bool operational_active_{false}; bool streaming_{false}; std::shared_ptr rgb_encoder_; }; diff --git a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp index 54afbdfe..416dc7bb 100644 --- a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp +++ b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp @@ -150,9 +150,6 @@ bool MujocoCamera::start() bool MujocoCamera::stop() { std::lock_guard lock(mtx_); - operational_active_ = false; - streaming_ = false; - stream_count_ = 0U; state_.is_streaming = false; state_.is_opened = false; destroyOffscreen_(); @@ -216,7 +213,6 @@ bool MujocoCamera::startStreaming() return false; } std::lock_guard lock(mtx_); - ++stream_count_; streaming_ = true; state_.is_streaming = true; return true; @@ -225,41 +221,10 @@ bool MujocoCamera::startStreaming() void MujocoCamera::stopStreaming() { std::lock_guard lock(mtx_); - if (stream_count_ > 0U) { - --stream_count_; - } - if (stream_count_ == 0U) { - streaming_ = false; - state_.is_streaming = operational_active_; - stream_frame_index_ = 0; - rgb_encoder_.reset(); - } -} - -bool MujocoCamera::startOperationalActivity() -{ - if (!start()) { - return false; - } - std::lock_guard lock(mtx_); - operational_active_ = true; - state_.is_streaming = true; - return true; -} - -bool MujocoCamera::stopOperationalActivity() -{ - std::lock_guard lock(mtx_); - if (stream_count_ != 0U || state_.is_recording) { - return false; - } - operational_active_ = false; streaming_ = false; state_.is_streaming = false; - state_.is_opened = false; stream_frame_index_ = 0; rgb_encoder_.reset(); - return true; } bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) diff --git a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp index a3ab20be..67a6c083 100644 --- a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp +++ b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp @@ -53,40 +53,4 @@ TEST(MujocoCameraTest, CapturesOffscreenRgbdFrame) EXPECT_GT(intrinsics.fy, 0.0f); } -TEST(MujocoCameraTest, OperationalStopCanResumeWithoutReinitializing) -{ - std::uint64_t frame_id = 0; - cmvr::device::MujocoCamera camera( - [&frame_id](std::vector& rgb, - std::vector& depth, - int& width, - int& height, - std::uint64_t& returned_frame_id) { - width = 2; - height = 2; - rgb.assign(12U, 127U); - depth.assign(4U, 1.0F); - returned_frame_id = ++frame_id; - return true; - }); - - ASSERT_TRUE(camera.init()); - ASSERT_TRUE(camera.startOperationalActivity()); - ASSERT_TRUE(camera.startStreaming()); - EXPECT_FALSE(camera.stopOperationalActivity()); - camera.stopStreaming(); - EXPECT_TRUE(camera.stopOperationalActivity()); - - cmvr::device::CameraState state{}; - camera.getState(state); - EXPECT_TRUE(state.is_initialized); - EXPECT_FALSE(state.is_opened); - EXPECT_FALSE(state.is_streaming); - - ASSERT_TRUE(camera.startOperationalActivity()); - camera.getState(state); - EXPECT_TRUE(state.is_opened); - EXPECT_TRUE(state.is_streaming); -} - } // namespace diff --git a/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h b/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h index 09bad9e8..ec700d96 100644 --- a/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h +++ b/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h @@ -5,10 +5,6 @@ #ifndef REALSENSE_CAMERA_H #define REALSENSE_CAMERA_H -#include -#include -#include - #include "camera/abstract_camera.h" #include "common/base/ring_buffer.h" #include "devices/camera/common/include/camera_stream_encoder.h" @@ -42,7 +38,6 @@ namespace cmvr::device{ bool startStreaming() override; void stopStreaming() override; - bool stopOperationalActivity() override; Eigen::Vector3f get3DPointFromPixel(int u, int v) override; @@ -50,11 +45,6 @@ namespace cmvr::device{ rs2::frameset get_frameset(bool align); void streaming_worker_(); void recording_worker_(); - bool collectStreamingWorker_(std::chrono::milliseconds timeout); - bool collectRecordingWorker_(std::chrono::milliseconds timeout); - void markStreamingWorkerStopped_() noexcept; - void markRecordingWorkerStopped_() noexcept; - void setWorkerError_(const std::string& message) noexcept; private: int fps_; int width_; @@ -89,10 +79,6 @@ namespace cmvr::device{ std::string current_video_path_; std::mutex ctrl_mtx_{}; - std::mutex stream_lifecycle_mtx_{}; - std::mutex stream_stop_mtx_{}; - std::condition_variable stream_stop_cv_{}; - std::condition_variable recording_stop_cv_{}; std::unique_ptr video_writer_; std::shared_ptr stream_thread_;//采集线程 std::shared_ptr encode_thread_;//采集线程 @@ -116,12 +102,8 @@ namespace cmvr::device{ size_t recordingIndex_ = 0; size_t getImageIndex_ = 0; - std::atomic stream_requested_{false}; - std::atomic recording_requested_{false}; - std::atomic stream_worker_exited_{true}; - std::atomic recording_worker_exited_{true}; - std::atomic is_streaming_running{false}; - std::atomic is_recording_running{false}; + bool is_streaming_running = false; + bool is_recording_running = false; int stream_count_ = 0; cv::Mat latest_depth_; diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index a5da58d3..e9db35ff 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp @@ -8,10 +8,6 @@ using namespace std; using namespace cmvr::device; -namespace { -constexpr auto kStreamStopTimeout = std::chrono::seconds(2); -} - // 检查系统中是否存在指定序列号的 RealSense 设备 bool checkRealSenseCamera(const std::string& serialNumber = "") { @@ -311,39 +307,31 @@ bool RealsenseCamera::start() { bool RealsenseCamera::stop() { //先停止录制再关闭摄像头 - bool is_recording = false; - { - std::lock_guard lock(ctrl_mtx_); - is_recording = state_.is_recording; - } - if (is_recording) { + if (state_.is_recording) { try { stopRecording(); } catch (const std::exception& e) { CMVR_LOG(WARNING) << "[RealsenseCamera] (stop): stopRecording failed: " << e.what(); } } - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + std::shared_ptr stream_thread_to_join; { std::lock_guard lock(ctrl_mtx_); clear_error_(); - if (!state_.is_opened || !state_.is_initialized) { - state_.is_opened = false; - return true; - } - state_.is_streaming = false; stream_count_ = 0; - stream_requested_.store(false, std::memory_order_release); + state_.is_streaming = false; + state_.is_recording = false; + stream_thread_to_join = stream_thread_; + stream_thread_.reset(); } - - if (!collectStreamingWorker_(kStreamStopTimeout)) { - std::lock_guard lock(ctrl_mtx_); - state_.is_error = true; - state_.error_message = "timed out waiting for camera stream to stop"; - return false; + if (stream_thread_to_join && stream_thread_to_join->joinable()) { + stream_thread_to_join->join(); } - std::lock_guard lock(ctrl_mtx_); + if (!state_.is_opened || !state_.is_initialized) { + state_.is_opened = false; + return true; + } try { pipe_.stop(); } catch (const std::exception& e) { @@ -411,8 +399,6 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { } void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { - std::lock_guard control_lock(ctrl_mtx_); - clear_error_(); intrinsics.cx = intrinsics_.ppx; intrinsics.cy = intrinsics_.ppy; intrinsics.fx = intrinsics_.fx; @@ -459,8 +445,6 @@ void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { } void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) { - std::lock_guard control_lock(ctrl_mtx_); - clear_error_(); intrinsics.cx = intrinsics_.ppx; intrinsics.cy = intrinsics_.ppy; intrinsics.fx = intrinsics_.fx; @@ -513,49 +497,24 @@ void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsic void RealsenseCamera::startRecording(const std::string &video_path) { - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); - { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (mode_ != VIDEO_MODE) { - state_.is_error = true; - state_.error_message = "startRecording only supports VIDEO_MODE"; - return; - } - if (!state_.is_opened) { - state_.is_error = true; - state_.error_message = "camera not opened"; - return; - } - if (state_.is_recording || - recording_requested_.load(std::memory_order_acquire)) { - state_.is_error = true; - state_.error_message = "already recording"; - return; - } - } - - if (!collectRecordingWorker_(kStreamStopTimeout)) { - setWorkerError_("previous camera recording did not stop"); - return; - } - bool collect_stale_stream = false; - { - std::lock_guard lock(ctrl_mtx_); - collect_stale_stream = stream_thread_ && - stream_worker_exited_.load(std::memory_order_acquire); - } - if (collect_stale_stream && - !collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_("previous camera stream did not stop"); - return; - } - - std::unique_lock lock(ctrl_mtx_); - if (!state_.is_opened || state_.is_recording) { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (mode_ != VIDEO_MODE) { state_.is_error = true; - state_.error_message = state_.is_recording - ? "already recording" : "camera not opened"; + state_.error_message = "startRecording only supports VIDEO_MODE"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + return; + } + if (!state_.is_opened) { + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; + return; + } + if (state_.is_recording) { + state_.is_error = true; + state_.error_message = "already recording"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (startRecording): " << state_.error_message; return; } @@ -569,11 +528,8 @@ void RealsenseCamera::startRecording(const std::string &video_path) { stream_ = nullptr; format_context_ = nullptr; state_.is_recording = false; - recording_requested_.store(false, std::memory_order_release); - current_video_path_.clear(); }; - bool created_stream_worker = false; try { current_video_path_ = video_path; std::string temp_path = current_video_path_ + ".temp"; // 临时文件 @@ -664,67 +620,63 @@ void RealsenseCamera::startRecording(const std::string &video_path) { cleanup_recording_resources(); return; } + //不在录像也不在流传输，但是采集线程没有退出时。 + if (!state_.is_streaming && !state_.is_recording) { + if (stream_thread_) { + if (stream_thread_->joinable()) { + stream_thread_->join(); + is_streaming_running = false; + } + stream_thread_.reset(); + } + } // 开启录像 state_.is_recording = true; - recording_requested_.store(true, std::memory_order_release); //开启流采集线程 if (!stream_thread_) { - stream_worker_exited_.store(false, std::memory_order_release); stream_thread_ = make_shared(&RealsenseCamera::streaming_worker_, this); - created_stream_worker = true; + //延时100ms，等待流线程获取图像 + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } // 启动录像线程 frame_count_ = 0; - recording_worker_exited_.store(false, std::memory_order_release); + if (recording_thread_) { + if (recording_thread_->joinable()) { + recording_thread_->join(); + is_recording_running = false; + } + recording_thread_.reset(); + } recording_thread_ = make_shared(&RealsenseCamera::recording_worker_, this); } catch (const std::exception& e) { - recording_requested_.store(false, std::memory_order_release); - recording_worker_exited_.store(true, std::memory_order_release); - recording_stop_cv_.notify_all(); - if (!stream_thread_) { - stream_worker_exited_.store(true, std::memory_order_release); - stream_stop_cv_.notify_all(); - } cleanup_recording_resources(); state_.is_error = true; state_.error_message = "[RealsenseCamera] (startRecording): " + std::string(e.what()); CMVR_LOG(ERROR) << state_.error_message; - lock.unlock(); - if (created_stream_worker && - !collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_( - "failed to collect camera stream after recording start failure"); - } } } void RealsenseCamera::stopRecording() { - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); - bool has_recording_worker = false; - { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (mode_ != VIDEO_MODE) { - state_.is_error = true; - state_.error_message = "stopRecording only supports VIDEO_MODE"; - return; - } - has_recording_worker = static_cast(recording_thread_); - if (!state_.is_recording && !has_recording_worker) { - return; - } - recording_requested_.store(false, std::memory_order_release); + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (mode_ != VIDEO_MODE) { + state_.is_error = true; + state_.error_message = "stopRecording only supports VIDEO_MODE"; + CMVR_LOG(ERROR) << "[RealsenseCamera] (stopRecording): " << state_.error_message; + return; } - if (has_recording_worker && - !collectRecordingWorker_(kStreamStopTimeout)) { - setWorkerError_("timed out waiting for camera recording to stop"); - throw std::runtime_error( - "timed out waiting for camera recording to stop"); + if (!state_.is_recording) { + CMVR_LOG(WARNING) << "[RealsenseCamera] (stopRecording): not recording"; + return; } - std::unique_lock lock(ctrl_mtx_); + // 1. 停止录像线程 state_.is_recording = false; + if (recording_thread_ && recording_thread_->joinable()) { + recording_thread_->join(); + recording_thread_.reset(); + } // 2. 清理FFmpeg资源 if (packet_) { @@ -746,26 +698,14 @@ void RealsenseCamera::stopRecording() { stream_ = nullptr; // 3. 重命名临时文件为目标文件 - const std::string completed_video_path = current_video_path_; - const std::string temp_path = completed_video_path + ".temp"; - if (!completed_video_path.empty() && - rename(temp_path.c_str(), completed_video_path.c_str()) != 0) { + std::string temp_path = current_video_path_ + ".temp"; + if (rename(temp_path.c_str(), current_video_path_.c_str()) != 0) { state_.is_error = true; - state_.error_message = "failed to rename temp file: " + temp_path + - " -> " + completed_video_path; + state_.error_message = "failed to rename temp file: " + temp_path + " -> " + current_video_path_; CMVR_LOG(ERROR) << "[RealsenseCamera] (stopRecording): " << state_.error_message; + return; } current_video_path_.clear(); - - const bool collect_stream = stream_count_ == 0 && - static_cast(stream_thread_); - lock.unlock(); - if (collect_stream && - !collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_("timed out waiting for camera stream to stop"); - throw std::runtime_error( - "timed out waiting for camera stream to stop"); - } } void RealsenseCamera::pauseRecording() { @@ -794,15 +734,14 @@ void RealsenseCamera::streaming_worker_() { const int frame_interval = 1000 / fps_; bool success = false; - is_streaming_running.store(true, std::memory_order_release); + is_streaming_running = true; uint64_t frame_sequence = 0; const uint64_t stream_epoch = static_cast( std::chrono::duration_cast( std::chrono::steady_clock::now().time_since_epoch()).count()); // 处于流传输或者录像状态时就不退出线程 - while (stream_requested_.load(std::memory_order_acquire) || - recording_requested_.load(std::memory_order_acquire)) { + while (state_.is_streaming || state_.is_recording) { // 记录当前帧处理开始时间 const auto frame_start_time = std::chrono::steady_clock::now(); @@ -811,13 +750,15 @@ void RealsenseCamera::streaming_worker_() { rs2::frame color_frame = frames.get_color_frame(); rs2::frame depth_frame = frames.get_depth_frame(); if (!color_frame) { - setWorkerError_("missing color frame"); - recording_requested_.store(false, std::memory_order_release); + state_.is_error = true; + state_.error_message = "missing color frame"; + CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; break; } if (stream_mode_ == RGBD_MODE && !depth_frame) { - setWorkerError_("missing depth frame in RGBD mode"); - recording_requested_.store(false, std::memory_order_release); + state_.is_error = true; + state_.error_message = "missing depth frame in RGBD mode"; + CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; break; } @@ -902,7 +843,7 @@ void RealsenseCamera::streaming_worker_() { } } - markStreamingWorkerStopped_(); + is_streaming_running = false; // 线程结束时清空队列 stream_frame_buffer_->clear(); recordingIndex_ = 0; @@ -913,21 +854,20 @@ void RealsenseCamera::streaming_worker_() { // 线程结束时清空队列 stream_frame_buffer_->clear(); // 确保线程状态正确更新 - recording_requested_.store(false, std::memory_order_release); - setWorkerError_(e.what()); - markStreamingWorkerStopped_(); - CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_ error:" - << e.what(); + is_streaming_running = false; + state_.is_error = true; + state_.error_message = e.what(); + CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_ error:" << state_.error_message; } } void RealsenseCamera::recording_worker_() { - is_recording_running.store(true, std::memory_order_release); + is_recording_running = true; const int frame_interval = 1000 / fps_; bool is_first_key = false; try { //保证当前采集线程正常运行 - while (recording_requested_.load(std::memory_order_acquire)) { + while (state_.is_recording && is_streaming_running) { // 等待缓冲区有数据 if (stream_frame_buffer_->empty()) { std::this_thread::sleep_for(std::chrono::milliseconds(frame_interval)); @@ -992,12 +932,11 @@ void RealsenseCamera::recording_worker_() { av_write_trailer(format_context_); } catch (const std::exception& e) { - setWorkerError_(e.what()); CMVR_LOG(ERROR) << "Recording thread error: " << e.what(); } - recording_requested_.store(false, std::memory_order_release); - markRecordingWorkerStopped_(); + is_recording_running = false; + state_.is_recording = false; } void RealsenseCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index) { @@ -1031,222 +970,49 @@ bool RealsenseCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& bool RealsenseCamera::startStreaming() { - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); - - { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (!state_.is_opened) { - state_.is_error = true; - state_.error_message = "camera not opened"; - return false; - } - if (stream_count_ > 0) { - if (!stream_thread_ || - stream_worker_exited_.load(std::memory_order_acquire)) { - state_.is_error = true; - state_.error_message = "camera stream worker exited"; - return false; - } - ++stream_count_; - state_.is_streaming = true; - stream_requested_.store(true, std::memory_order_release); - return true; - } - if (stream_thread_ && - !stream_worker_exited_.load(std::memory_order_acquire)) { - ++stream_count_; - state_.is_streaming = true; - stream_requested_.store(true, std::memory_order_release); - return true; - } - } - - if (!collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_("previous camera stream did not stop"); - return false; - } - std::lock_guard lock(ctrl_mtx_); - if (!state_.is_opened) { - state_.is_error = true; - state_.error_message = "camera not opened"; - return false; + //不在录像也不在流传输，但是采集线程没有退出时。 + if (!state_.is_streaming && !state_.is_recording) { + if (stream_thread_) { + if (stream_thread_->joinable()) { + stream_thread_->join(); + is_streaming_running = false; + } + stream_thread_.reset(); + } } - state_.is_streaming = true; - stream_requested_.store(true, std::memory_order_release); - stream_worker_exited_.store(false, std::memory_order_release); - try { + //开启流采集线程 + if (!stream_thread_) { + state_.is_streaming = true; stream_thread_ = make_shared(&RealsenseCamera::streaming_worker_, this); - } catch (const std::exception& error) { - stream_requested_.store(false, std::memory_order_release); - stream_worker_exited_.store(true, std::memory_order_release); - stream_stop_cv_.notify_all(); - state_.is_streaming = false; - state_.is_error = true; - state_.error_message = - std::string("failed to start camera stream worker: ") + - error.what(); - return false; + + //延时100ms，等待流线程获取图像 + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - stream_count_ = 1; + stream_count_++; return true; } void RealsenseCamera::stopStreaming() { - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + std::shared_ptr stream_thread_to_join; { std::lock_guard lock(ctrl_mtx_); - if (stream_count_ == 0) { - return; + if (stream_count_ > 0) { + stream_count_--; } - --stream_count_; - if (stream_count_ != 0) { - return; - } - state_.is_streaming = false; - stream_requested_.store(false, std::memory_order_release); - if (recording_requested_.load(std::memory_order_acquire)) { - return; + if (stream_count_ == 0) + { + // 当前已经没有正在使用的流了，编码采集线程状态修改 + state_.is_streaming = false; + if (!state_.is_recording) { + stream_thread_to_join = stream_thread_; + stream_thread_.reset(); + } } } - if (!collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_("timed out waiting for camera stream to stop"); - throw std::runtime_error("timed out waiting for camera stream to stop"); - } -} - -bool RealsenseCamera::stopOperationalActivity() -{ - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); - { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - - if (stream_count_ != 0 || state_.is_streaming || - state_.is_recording || - recording_requested_.load(std::memory_order_acquire)) { - return false; - } - stream_requested_.store(false, std::memory_order_release); - recording_requested_.store(false, std::memory_order_release); - } - - if (!collectRecordingWorker_(kStreamStopTimeout)) { - setWorkerError_("timed out waiting for camera recording to stop"); - return false; - } - if (!collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_("timed out waiting for camera stream to stop"); - return false; - } - - std::lock_guard lock(ctrl_mtx_); - if (state_.is_opened) { - try { - pipe_.stop(); - } catch (const std::exception& error) { - state_.is_error = true; - state_.error_message = - std::string("failed to stop operational pipeline: ") + - error.what(); - return false; - } - } - align_.reset(); - pipe_ = rs2::pipeline(); - state_.is_opened = false; - return !state_.is_streaming && !state_.is_recording; -} - -bool RealsenseCamera::collectStreamingWorker_( - const std::chrono::milliseconds timeout) -{ - std::shared_ptr worker; - { - std::lock_guard lock(ctrl_mtx_); - worker = stream_thread_; - } - if (!worker) { - return true; - } - - { - std::unique_lock lock(stream_stop_mtx_); - if (!stream_stop_cv_.wait_for(lock, timeout, [this] { - return stream_worker_exited_.load(std::memory_order_acquire); - })) { - return false; - } - } - if (worker->joinable()) { - worker->join(); - } - std::lock_guard lock(ctrl_mtx_); - if (stream_thread_ == worker) { - stream_thread_.reset(); - } - return true; -} - -void RealsenseCamera::markStreamingWorkerStopped_() noexcept -{ - is_streaming_running.store(false, std::memory_order_release); - stream_worker_exited_.store(true, std::memory_order_release); - stream_stop_cv_.notify_all(); -} - -bool RealsenseCamera::collectRecordingWorker_( - const std::chrono::milliseconds timeout) -{ - std::shared_ptr worker; - { - std::lock_guard lock(ctrl_mtx_); - worker = recording_thread_; - } - if (!worker) { - return true; - } - - { - std::unique_lock lock(stream_stop_mtx_); - if (!recording_stop_cv_.wait_for(lock, timeout, [this] { - return recording_worker_exited_.load( - std::memory_order_acquire); - })) { - return false; - } - } - if (worker->joinable()) { - worker->join(); - } - std::lock_guard lock(ctrl_mtx_); - if (recording_thread_ == worker) { - recording_thread_.reset(); - } - return true; -} - -void RealsenseCamera::markRecordingWorkerStopped_() noexcept -{ - { - std::lock_guard lock(ctrl_mtx_); - state_.is_recording = false; - } - is_recording_running.store(false, std::memory_order_release); - recording_worker_exited_.store(true, std::memory_order_release); - recording_stop_cv_.notify_all(); -} - -void RealsenseCamera::setWorkerError_(const std::string& message) noexcept -{ - try { - std::lock_guard lock(ctrl_mtx_); - state_.is_error = true; - state_.error_message = message; - } catch (...) { - // Error reporting from a worker must never terminate the process. + if (stream_thread_to_join && stream_thread_to_join->joinable()) { + stream_thread_to_join->join(); } } diff --git a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h b/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h index 621cde81..14194a28 100644 --- a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h +++ b/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h @@ -6,7 +6,6 @@ #define CMVR_ES_UVC_CAMERA_H #include -#include #include #include "common/base/ring_buffer.h" @@ -46,7 +45,6 @@ namespace cmvr::device { bool startStreaming() override; void stopStreaming() override; - bool stopOperationalActivity() override; private: void streaming_worker_(); void recording_worker_(); @@ -60,11 +58,6 @@ namespace cmvr::device { int64_t& capture_utc_ns); bool convert_capture_frame_to_bgr_(const AVFrame* frame, cv::Mat& bgr_frame); static int interrupt_capture_(void* opaque); - bool collectStreamingWorker_(std::chrono::milliseconds timeout); - bool collectRecordingWorker_(std::chrono::milliseconds timeout); - void markStreamingWorkerStopped_() noexcept; - void markRecordingWorkerStopped_() noexcept; - void setWorkerError_(const std::string& message) noexcept; int fps_; int width_; @@ -82,10 +75,6 @@ namespace cmvr::device { std::string current_video_path_; std::mutex ctrl_mtx_{}; - std::mutex stream_lifecycle_mtx_{}; - std::mutex stream_stop_mtx_{}; - std::condition_variable stream_stop_cv_{}; - std::condition_variable recording_stop_cv_{}; std::shared_ptr stream_thread_; std::shared_ptr recording_thread_; std::shared_ptr capture_thread_; @@ -122,12 +111,8 @@ namespace cmvr::device { size_t recordingIndex_ = 0; size_t getImageIndex_ = 0; - std::atomic stream_requested_{false}; - std::atomic recording_requested_{false}; - std::atomic stream_worker_exited_{true}; - std::atomic recording_worker_exited_{true}; - std::atomic is_streaming_running{false}; - std::atomic is_recording_running{false}; + bool is_streaming_running = false; + bool is_recording_running = false; int stream_count_ = 0; config::UVCCameraConfig camera_; diff --git a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp index c5e63112..2105d234 100644 --- a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp +++ b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp @@ -20,7 +20,6 @@ using namespace cmvr::device; #define USE_LIST_IMAGE 1 namespace { -constexpr auto kStreamStopTimeout = std::chrono::seconds(2); std::string ffmpeg_error_string(const int error_code) { @@ -115,6 +114,7 @@ bool is_complete_mjpeg_packet(const AVPacket* packet) } } // namespace + UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera) { id_ = camera_.id(); @@ -263,18 +263,9 @@ bool UVCCamera::start() { bool UVCCamera::stop() { //先停止录制再关闭摄像头 - bool is_recording = false; - { - std::lock_guard lock(ctrl_mtx_); - is_recording = state_.is_recording || recording_thread_ || packet_ || - format_context_ || stream_; - } - if (is_recording) { + if (state_.is_recording || recording_thread_ || packet_ || format_context_ || stream_) { stopRecording(); } - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); - bool collect_stream = false; - { std::lock_guard lock(ctrl_mtx_); clear_error_(); try { @@ -284,10 +275,18 @@ bool UVCCamera::stop() { } if (mode_ == VIDEO_MODE){ state_.is_streaming = false; - stream_count_ = 0; - stream_requested_.store(false, std::memory_order_release); - collect_stream = static_cast(stream_thread_); + if (stream_thread_) { + if (stream_thread_->joinable()) { + stream_thread_->join(); + } + stream_thread_.reset(); + } + is_streaming_running = false; } + + close_capture_(); + state_.is_opened = false; + return true; } catch (exception &e) { CMVR_LOG(ERROR) << "[UVCCamera] (stop): " << e.what(); @@ -295,20 +294,6 @@ bool UVCCamera::stop() { state_.error_message = e.what(); return false; } - } - - if (collect_stream && !collectStreamingWorker_(kStreamStopTimeout)) { - std::lock_guard lock(ctrl_mtx_); - state_.is_error = true; - state_.error_message = "timed out waiting for camera stream to stop"; - return false; - } - close_capture_(); - { - std::lock_guard lock(ctrl_mtx_); - state_.is_opened = false; - } - return true; } void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) @@ -783,7 +768,6 @@ bool UVCCamera::convert_capture_frame_to_bgr_(const AVFrame* frame, cv::Mat& bgr } void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { - std::lock_guard lock(ctrl_mtx_); state_.is_error = true; state_.error_message = "getDepthImage unsupported usage"; CMVR_LOG(ERROR) << "[UVCCamera] (getDepthImage): " << state_.error_message; @@ -791,7 +775,6 @@ void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { } void UVCCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) { - std::lock_guard lock(ctrl_mtx_); state_.is_error = true; state_.error_message = "getRGBDImages unsupported usage"; CMVR_LOG(ERROR) << "[UVCCamera] (getRGBDImages): " << state_.error_message; @@ -816,56 +799,27 @@ void UVCCamera::cleanup_recording_resources_() { } void UVCCamera::startRecording(const std::string &video_path) { - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); - { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (mode_ != VIDEO_MODE) { - state_.is_error = true; - state_.error_message = "startRecording only supports VIDEO_MODE"; - CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; - return; - } - if (!state_.is_opened) { - state_.is_error = true; - state_.error_message = "camera not opened"; - CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; - return; - } - if (state_.is_recording || - recording_requested_.load(std::memory_order_acquire)) { - state_.is_error = true; - state_.error_message = "already recording"; - CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; - return; - } - } - - if (!collectRecordingWorker_(kStreamStopTimeout)) { - setWorkerError_("previous camera recording did not stop"); - return; - } - bool collect_stale_stream = false; - { - std::lock_guard lock(ctrl_mtx_); - collect_stale_stream = stream_thread_ && - stream_worker_exited_.load(std::memory_order_acquire); - } - if (collect_stale_stream && - !collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_("previous camera stream did not stop"); - return; - } - - std::unique_lock lock(ctrl_mtx_); - if (!state_.is_opened || state_.is_recording) { + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (mode_ != VIDEO_MODE) { state_.is_error = true; - state_.error_message = state_.is_recording - ? "already recording" : "camera not opened"; + state_.error_message = "startRecording only supports VIDEO_MODE"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + return; + } + if (!state_.is_opened) { + state_.is_error = true; + state_.error_message = "camera not opened"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; + return; + } + if (state_.is_recording) { + state_.is_error = true; + state_.error_message = "already recording"; + CMVR_LOG(ERROR) << "[UVCCamera] (startRecording): " << state_.error_message; return; } - bool created_stream_worker = false; try { current_video_path_ = video_path; std::string temp_path = current_video_path_ + ".temp"; // 临时文件 @@ -958,102 +912,81 @@ void UVCCamera::startRecording(const std::string &video_path) { cleanup_recording_resources_(); return; } + //不在录像也不在流传输，但是采集线程没有退出时。 + if (!state_.is_streaming && !state_.is_recording) { + if (stream_thread_) { + if (stream_thread_->joinable()) { + stream_thread_->join(); + is_streaming_running = false; + } + stream_thread_.reset(); + } + } // 开启录像 state_.is_recording = true; - recording_requested_.store(true, std::memory_order_release); //开启流采集线程 if (!stream_thread_) { - stream_worker_exited_.store(false, std::memory_order_release); stream_thread_ = make_shared(&UVCCamera::streaming_worker_, this); - created_stream_worker = true; + //延时100ms，等待流线程获取图像 + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } // 启动录像线程 + if (recording_thread_) { + if (recording_thread_->joinable()) { + recording_thread_->join(); + is_recording_running = false; + } + recording_thread_.reset(); + } frame_count_ = 0; - recording_worker_exited_.store(false, std::memory_order_release); recording_thread_ = make_shared(&UVCCamera::recording_worker_, this); } catch (const std::exception& e) { - recording_requested_.store(false, std::memory_order_release); - recording_worker_exited_.store(true, std::memory_order_release); - recording_stop_cv_.notify_all(); - if (!stream_thread_) { - stream_worker_exited_.store(true, std::memory_order_release); - stream_stop_cv_.notify_all(); - } cleanup_recording_resources_(); state_.is_recording = false; state_.is_error = true; state_.error_message = "[UVCCamera] (startRecording): " + std::string(e.what()); CMVR_LOG(ERROR) << state_.error_message; - const bool collect_created_stream = created_stream_worker && - !stream_requested_.load(std::memory_order_acquire); - lock.unlock(); - if (collect_created_stream && - !collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_( - "failed to collect camera stream after recording start failure"); - } } } void UVCCamera::stopRecording() { - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); - bool has_recording_worker = false; - { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (mode_ != VIDEO_MODE) { - state_.is_error = true; - state_.error_message = "stopRecording only supports VIDEO_MODE"; - CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message; - return; - } - has_recording_worker = static_cast(recording_thread_); - const bool has_recording_resources = has_recording_worker || packet_ || - format_context_ || stream_; - if (!state_.is_recording && !has_recording_resources) { - CMVR_LOG(WARNING) << "[UVCCamera] (stopRecording): not recording"; - return; - } - recording_requested_.store(false, std::memory_order_release); + std::lock_guard lock(ctrl_mtx_); + clear_error_(); + if (mode_ != VIDEO_MODE) { + state_.is_error = true; + state_.error_message = "stopRecording only supports VIDEO_MODE"; + CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message; + return; + } + const bool has_recording_resources = + recording_thread_ || packet_ || format_context_ || stream_; + if (!state_.is_recording && !has_recording_resources) { + CMVR_LOG(WARNING) << "[UVCCamera] (stopRecording): not recording"; + return; } - if (has_recording_worker && - !collectRecordingWorker_(kStreamStopTimeout)) { - setWorkerError_("timed out waiting for camera recording to stop"); - throw std::runtime_error( - "timed out waiting for camera recording to stop"); - } - - std::unique_lock lock(ctrl_mtx_); + // 1. 停止录像线程 state_.is_recording = false; + if (recording_thread_) { + if (recording_thread_->joinable()) { + recording_thread_->join(); + } + recording_thread_.reset(); + } // 2. 清理FFmpeg资源 cleanup_recording_resources_(); - // 3. 重命名临时文件为目标文件。即使重命名失败，也必须继续 - // 回收仅由录像使用的采集线程。 - const std::string completed_video_path = current_video_path_; - const std::string temp_path = completed_video_path + ".temp"; - const bool rename_failed = !completed_video_path.empty() && - rename(temp_path.c_str(), completed_video_path.c_str()) != 0; - if (rename_failed) { + // 3. 重命名临时文件为目标文件 + std::string temp_path = current_video_path_ + ".temp"; + if (rename(temp_path.c_str(), current_video_path_.c_str()) != 0) { state_.is_error = true; - state_.error_message = "failed to rename temp file: " + temp_path + - " -> " + completed_video_path; + state_.error_message = "failed to rename temp file: " + temp_path + " -> " + current_video_path_; CMVR_LOG(ERROR) << "[UVCCamera] (stopRecording): " << state_.error_message; + return; } current_video_path_.clear(); - - const bool collect_stream = stream_count_ == 0 && - static_cast(stream_thread_); - lock.unlock(); - if (collect_stream && - !collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_("timed out waiting for camera stream to stop"); - throw std::runtime_error( - "timed out waiting for camera stream to stop"); - } } void UVCCamera::pauseRecording() { @@ -1067,9 +1000,10 @@ void UVCCamera::resumeRecording() { void UVCCamera::streaming_worker_() { AVFrame* frame = av_frame_alloc(); if (!frame) { - setWorkerError_("failed to allocate streaming frame"); - markStreamingWorkerStopped_(); - CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: failed to allocate streaming frame"; + is_streaming_running = false; + state_.is_error = true; + state_.error_message = "failed to allocate streaming frame"; + CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message; return; } try { @@ -1077,7 +1011,7 @@ void UVCCamera::streaming_worker_() { const int frame_interval = 1000 / fps_; bool success = false; - is_streaming_running.store(true, std::memory_order_release); + is_streaming_running = true; uint64_t capture_sequence = 0; int64_t frame_capture_monotonic_ns = 0; int64_t frame_capture_utc_ns = 0; @@ -1095,9 +1029,9 @@ void UVCCamera::streaming_worker_() { int64_t encode_us = 0; int64_t processing_us = 0; auto timing_window_start = std::chrono::steady_clock::now(); + // 处于流传输或者录像状态时就不退出线程 - while (stream_requested_.load(std::memory_order_acquire) || - recording_requested_.load(std::memory_order_acquire)) { + while (state_.is_streaming || state_.is_recording) { // 记录当前帧处理开始时间 const auto frame_start_time = std::chrono::steady_clock::now(); if (!wait_for_capture_frame_(frame, @@ -1109,12 +1043,11 @@ void UVCCamera::streaming_worker_() { std::lock_guard capture_lock(capture_frame_mutex_); capture_error = capture_error_; } - const std::string error_message = capture_error.empty() + state_.is_error = true; + state_.error_message = capture_error.empty() ? "timed out waiting for camera frame" : capture_error; - setWorkerError_(error_message); - recording_requested_.store(false, std::memory_order_release); - CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << error_message; + CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message; break; } const auto capture_end_time = std::chrono::steady_clock::now(); @@ -1202,7 +1135,7 @@ void UVCCamera::streaming_worker_() { } - markStreamingWorkerStopped_(); + is_streaming_running = false; // 线程结束时清空队列 stream_frame_buffer_->clear(); recordingIndex_ = 0; @@ -1213,23 +1146,23 @@ void UVCCamera::streaming_worker_() { // 线程结束时清空队列 stream_frame_buffer_->clear(); // 确保线程状态正确更新 - recording_requested_.store(false, std::memory_order_release); - setWorkerError_(e.what()); - markStreamingWorkerStopped_(); - CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << e.what(); + is_streaming_running = false; + state_.is_error = true; + state_.error_message = e.what(); + CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << state_.error_message; } av_frame_free(&frame); } void UVCCamera::recording_worker_() { - is_recording_running.store(true, std::memory_order_release); + is_recording_running = true; const int frame_interval = 1000 / fps_; bool is_first_key = false; int64_t first_capture_monotonic_ns = 0; int64_t last_packet_pts = AV_NOPTS_VALUE; try { //保证当前采集线程正常运行 - while (recording_requested_.load(std::memory_order_acquire)) { + while (state_.is_recording && is_streaming_running) { // 等待缓冲区有数据 if (stream_frame_buffer_->empty()) { std::this_thread::sleep_for(std::chrono::milliseconds(frame_interval)); @@ -1302,12 +1235,11 @@ void UVCCamera::recording_worker_() { av_write_trailer(format_context_); } catch (const std::exception& e) { - setWorkerError_(e.what()); CMVR_LOG(ERROR) << "录像线程错误: " << e.what(); } - recording_requested_.store(false, std::memory_order_release); - markRecordingWorkerStopped_(); + is_recording_running = false; + state_.is_recording = false; } void UVCCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index) { @@ -1341,213 +1273,50 @@ bool UVCCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_ bool UVCCamera::startStreaming() { - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); + std::lock_guard lock(ctrl_mtx_); + //不在录像也不在流传输，但是采集线程没有退出时。 + if (!state_.is_streaming && !state_.is_recording) { + if (stream_thread_) { + if (stream_thread_->joinable()) { + stream_thread_->join(); + is_streaming_running = false; + } + stream_thread_.reset(); + } + } - { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - if (!state_.is_opened) { + // A recording session may already own the encoder worker. The streaming + // state still has to reflect the new client lease so stopping recording + // does not terminate the worker while clients are consuming frames. + state_.is_streaming = true; + //开启流采集线程 + if (!stream_thread_) { + try { + stream_thread_ = make_shared(&UVCCamera::streaming_worker_, this); + } catch (const std::exception& e) { + state_.is_streaming = stream_count_ > 0; state_.is_error = true; - state_.error_message = "camera not opened"; + state_.error_message = "failed to start streaming worker: " + std::string(e.what()); + CMVR_LOG(ERROR) << "[UVCCamera] (startStreaming): " << state_.error_message; return false; } - // A running worker is shared by all streaming leases and recording. - // Adding another lease must not wait for that worker to exit. - if (stream_count_ > 0) { - if (!stream_thread_ || - stream_worker_exited_.load(std::memory_order_acquire)) { - state_.is_error = true; - state_.error_message = "camera stream worker exited"; - return false; - } - ++stream_count_; - state_.is_streaming = true; - stream_requested_.store(true, std::memory_order_release); - return true; - } - if (stream_thread_ && - !stream_worker_exited_.load(std::memory_order_acquire)) { - ++stream_count_; - state_.is_streaming = true; - stream_requested_.store(true, std::memory_order_release); - return true; - } + //延时100ms，等待流线程获取图像 + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - if (!collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_("previous camera stream did not stop"); - return false; - } - - std::lock_guard lock(ctrl_mtx_); - if (!state_.is_opened) { - state_.is_error = true; - state_.error_message = "camera not opened"; - return false; - } - state_.is_streaming = true; - stream_requested_.store(true, std::memory_order_release); - stream_worker_exited_.store(false, std::memory_order_release); - try { - stream_thread_ = make_shared(&UVCCamera::streaming_worker_, this); - } catch (const std::exception& error) { - stream_requested_.store(false, std::memory_order_release); - stream_worker_exited_.store(true, std::memory_order_release); - stream_stop_cv_.notify_all(); - state_.is_streaming = false; - state_.is_error = true; - state_.error_message = - std::string("failed to start camera stream worker: ") + - error.what(); - return false; - } - stream_count_ = 1; + stream_count_++; return true; } void UVCCamera::stopStreaming() { - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); - { - std::lock_guard lock(ctrl_mtx_); - if (stream_count_ == 0) { - return; - } + std::lock_guard lock(ctrl_mtx_); + if (stream_count_ > 0) { --stream_count_; - if (stream_count_ != 0) { - return; - } + } + if (stream_count_ == 0) + { + // 当前已经没有正在使用的流了，编码采集线程状态修改 state_.is_streaming = false; - stream_requested_.store(false, std::memory_order_release); - if (recording_requested_.load(std::memory_order_acquire)) { - return; - } - } - if (!collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_("timed out waiting for camera stream to stop"); - throw std::runtime_error("timed out waiting for camera stream to stop"); - } -} - -bool UVCCamera::stopOperationalActivity() -{ - std::lock_guard lifecycle_lock(stream_lifecycle_mtx_); - { - std::lock_guard lock(ctrl_mtx_); - clear_error_(); - - if (stream_count_ != 0 || state_.is_streaming || - state_.is_recording || - recording_requested_.load(std::memory_order_acquire)) { - return false; - } - stream_requested_.store(false, std::memory_order_release); - recording_requested_.store(false, std::memory_order_release); - } - - if (!collectRecordingWorker_(kStreamStopTimeout)) { - setWorkerError_("timed out waiting for camera recording to stop"); - return false; - } - if (!collectStreamingWorker_(kStreamStopTimeout)) { - setWorkerError_("timed out waiting for camera stream to stop"); - return false; - } - - close_capture_(); - { - std::lock_guard lock(ctrl_mtx_); - state_.is_opened = false; - } - return !capture_running_.load(std::memory_order_acquire); -} - -bool UVCCamera::collectStreamingWorker_( - const std::chrono::milliseconds timeout) -{ - std::shared_ptr worker; - { - std::lock_guard lock(ctrl_mtx_); - worker = stream_thread_; - } - if (!worker) { - return true; - } - - { - std::unique_lock lock(stream_stop_mtx_); - if (!stream_stop_cv_.wait_for(lock, timeout, [this] { - return stream_worker_exited_.load(std::memory_order_acquire); - })) { - return false; - } - } - if (worker->joinable()) { - worker->join(); - } - std::lock_guard lock(ctrl_mtx_); - if (stream_thread_ == worker) { - stream_thread_.reset(); - } - return true; -} - -void UVCCamera::markStreamingWorkerStopped_() noexcept -{ - is_streaming_running.store(false, std::memory_order_release); - stream_worker_exited_.store(true, std::memory_order_release); - stream_stop_cv_.notify_all(); -} - -bool UVCCamera::collectRecordingWorker_( - const std::chrono::milliseconds timeout) -{ - std::shared_ptr worker; - { - std::lock_guard lock(ctrl_mtx_); - worker = recording_thread_; - } - if (!worker) { - return true; - } - - { - std::unique_lock lock(stream_stop_mtx_); - if (!recording_stop_cv_.wait_for(lock, timeout, [this] { - return recording_worker_exited_.load( - std::memory_order_acquire); - })) { - return false; - } - } - if (worker->joinable()) { - worker->join(); - } - std::lock_guard lock(ctrl_mtx_); - if (recording_thread_ == worker) { - recording_thread_.reset(); - } - return true; -} - -void UVCCamera::markRecordingWorkerStopped_() noexcept -{ - { - std::lock_guard lock(ctrl_mtx_); - state_.is_recording = false; - } - is_recording_running.store(false, std::memory_order_release); - recording_worker_exited_.store(true, std::memory_order_release); - recording_stop_cv_.notify_all(); -} - -void UVCCamera::setWorkerError_(const std::string& message) noexcept -{ - try { - std::lock_guard lock(ctrl_mtx_); - state_.is_error = true; - state_.error_message = message; - } catch (...) { - // Error reporting from a worker must never terminate the process. } } diff --git a/cmvr-es/devices/canbus/CMakeLists.txt b/cmvr-es/devices/canbus/CMakeLists.txt index 0ce2cd40..76b2a460 100644 --- a/cmvr-es/devices/canbus/CMakeLists.txt +++ b/cmvr-es/devices/canbus/CMakeLists.txt @@ -31,21 +31,6 @@ 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 @@ -108,3 +93,4 @@ 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 dc683f4b..b8649125 100644 --- a/cmvr-es/devices/canbus/abstract_canbus.h +++ b/cmvr-es/devices/canbus/abstract_canbus.h @@ -3,14 +3,6 @@ // #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" @@ -22,26 +14,20 @@ namespace cmvr::device { */ struct CanFrame { /// Message id - uint32_t id{0}; + uint32_t id; /// Message length - 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}; + uint8_t len; + /// Message content + uint8_t data[8]; + /// Time stamp + struct timeval timestamp; /** * @brief Constructor */ - CanFrame() = default; + CanFrame() : id(0), len(0), timestamp{0} { + std::memset(data, 0, sizeof(data)); + } /** * @brief CanFrame string including essential information about the message. @@ -51,15 +37,10 @@ namespace cmvr::device { std::stringstream output_stream(""); output_stream << "id:0x" << Byte::byte_to_hex(id) << ",len:" << static_cast(len) << ",data:"; - const auto printable_len = - std::min(len, sizeof(data)); - for (std::size_t i = 0; i < printable_len; ++i) { + for (uint8_t i = 0; i < len; ++i) { output_stream << Byte::byte_to_hex(data[i]); } - output_stream << ",fd:" << is_fd - << ",brs:" << bitrate_switch - << ",extended:" << is_extended_id - << ",error:" << is_error_frame << ","; + output_stream << ","; return output_stream.str(); } }; @@ -86,28 +67,6 @@ 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. @@ -116,9 +75,7 @@ namespace cmvr::device { virtual cmvr::msgs::ErrorCode sendSingleFrame( const std::vector &frames) { if (frames.size() != 1U) { - CMVR_LOG(ERROR) << "frames size not equal to 1, actual frame size: " - << frames.size(); - return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + CMVR_LOG(FATAL) << "frames size not equal to 1, actual frame size: " << frames.size(); } int32_t n = 1; return send(frames, &n); @@ -134,17 +91,6 @@ 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 61b7157f..21d9340e 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,13 +12,9 @@ #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; @@ -28,25 +24,8 @@ namespace cmvr { auto channel_id = cfg.channel_id(); port_ = static_cast(channel_id); interface_ = CANCardParameter::NATIVE; - 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(); + + enable_can_err_check_ = false; } @@ -70,7 +49,7 @@ namespace cmvr { } SocketCanClientRaw::~SocketCanClientRaw() { - if (dev_handler_ >= 0) { + if (dev_handler_) { stop(); } } @@ -80,8 +59,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 @@ -112,71 +91,17 @@ 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. 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); + // 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; 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; } @@ -190,40 +115,14 @@ namespace cmvr { interface_prefix = "can"; } - 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; - } + const std::string can_name = absl::StrCat(interface_prefix, port_); 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); + CMVR_LOG(ERROR) << "ioctl error"; 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 addr.can_family = AF_CAN; @@ -232,10 +131,8 @@ namespace cmvr { sizeof(addr)); if (ret < 0) { - CMVR_LOG(ERROR) << "bind socket to CAN interface failed: " - << std::strerror(errno); + CMVR_LOG(ERROR) << "bind socket to network interface error code: " << ret; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; - stop(); return false; } @@ -245,11 +142,10 @@ namespace cmvr { } bool SocketCanClientRaw::stop() { - is_started_ = false; - if (dev_handler_ >= 0) { - const int fd = dev_handler_; - dev_handler_ = -1; - int ret = close(fd); + if (is_started_) { + is_started_ = false; + + int ret = close(dev_handler_); if (ret < 0) { CMVR_LOG(ERROR) << "close error code:" << ret << ", " << getErrorString(ret); return false; @@ -263,190 +159,48 @@ 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(ERROR) << "frame_num is null"; - return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + CMVR_LOG(FATAL) << "frame_num is null"; } - 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 (frames.size() != static_cast(*frame_num)) { + CMVR_LOG(FATAL) << "frames size does not match 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; } - if (std::chrono::steady_clock::now() >= send_deadline) { - *frame_num = 0; - return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; - } - - // 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; + 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; } - } - - 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; + if (frames[i].id > CAN_STANDARD_MAX_ID) { + send_frames_[i].can_id = (frames[i].id & CAN_EFF_MASK) | CAN_EFF_FLAG; } 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; + 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); - 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; - } + // 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; } } - *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; @@ -459,109 +213,39 @@ namespace cmvr { return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM; } - 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) { + for (int32_t i = 0; i < *frame_num && i < MAX_CAN_RECV_FRAME_LEN; ++i) { CanFrame cf; - struct canfd_frame raw {}; - const auto ret = ::read(dev_handler_, &raw, CANFD_MTU); + auto ret = read(dev_handler_, &recv_frames_[i], sizeof(recv_frames_[i])); if (ret < 0) { - if (errno == EAGAIN || errno == EWOULDBLOCK || - errno == EINTR) { - return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; - } - CMVR_LOG(ERROR) << "receive CAN message failed: " - << std::strerror(errno); + 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 << ")."; return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; } - if (ret != CAN_MTU && ret != CANFD_MTU) { - CMVR_LOG(ERROR) << "unexpected SocketCAN MTU: " << ret; - return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; - } - - 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; + 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 = raw_id & CAN_SFF_MASK; + cf.id = (recv_frames_[i].can_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); + // 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); frames->push_back(cf); - ++(*frame_num); } return ErrorCode::OK; } - 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); + std::string SocketCanClientRaw::getErrorString(const int32_t /*status*/) { + return ""; } } } 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 ca2c01ab..7f478c86 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,13 +12,11 @@ #include #include -#include #include #include #include #include -#include #include #include @@ -53,10 +51,6 @@ 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 @@ -66,7 +60,6 @@ namespace cmvr { */ cmvr::msgs::ErrorCode receive(std::vector *const frames, int32_t *const frame_num) override; - bool discardPendingFrames() override; /** * @brief Get the error string. @@ -74,23 +67,14 @@ namespace cmvr { */ std::string getErrorString(const int32_t status) override; private: - int dev_handler_{-1}; + int dev_handler_ = 0; cmvr::msgs::CANCardParameter::CANChannelId port_; cmvr::msgs::CANCardParameter::CANInterface interface_; - 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}; + can_frame send_frames_[MAX_CAN_SEND_FRAME_LEN]; + can_frame recv_frames_[MAX_CAN_RECV_FRAME_LEN]; // 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 745600eb..391be9d3 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,226 +1,44 @@ +#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; -#include -#include -#include -#include -#include -#include -#include + TEST(SocketCanClientRawTest, simple_test) { + CANCardParameter param; + param.set_brand(CANCardParameter::SOCKET_CAN_RAW); + param.set_channel_id(CANCardParameter::CHANNEL_ID_ZERO); -#include + cmvr::config::SocketCanConfig cfg; + cfg.set_channel_id(0); + SocketCanClientRaw socket_can_client(cfg); -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]); + // 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(); } } - -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 d46f9223..ed42ba19 100644 --- a/cmvr-es/devices/canbus/common/canbus_consts.h +++ b/cmvr-es/devices/canbus/common/canbus_consts.h @@ -26,14 +26,9 @@ namespace cmvr { namespace device { const int32_t CAN_FRAME_SIZE = 8; - 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_SEND_FRAME_LEN = 1; 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 a68048e8..9cdb53fb 100644 --- a/cmvr-es/devices/dexhand/abstract_dexhand.h +++ b/cmvr-es/devices/dexhand/abstract_dexhand.h @@ -156,17 +156,6 @@ 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 91cb479f..163f12c8 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,8 +49,6 @@ 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; @@ -124,7 +122,6 @@ 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 0d4724d2..d15d4cf6 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,28 +450,6 @@ 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."; } @@ -611,12 +589,6 @@ 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 { @@ -750,10 +722,9 @@ 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_ || polling_paused_for_stop_all_) { + if (!requested_polling_) { polling_cv_.wait(lock, [this]() { - return !polling_thread_running_.load(std::memory_order_acquire) || - (requested_polling_ && !polling_paused_for_stop_all_); + return !polling_thread_running_.load(std::memory_order_acquire) || requested_polling_; }); next_poll_deadline = std::chrono::steady_clock::now(); continue; @@ -775,8 +746,7 @@ void PX6AXGen3::pollingLoop() { } polling_cv_.wait_until(lock, next_poll_deadline, [this]() { - return !polling_thread_running_.load(std::memory_order_acquire) || - polling_paused_for_stop_all_; + return !polling_thread_running_.load(std::memory_order_acquire); }); } } @@ -788,15 +758,6 @@ 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/CMakeLists.txt b/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt index 0c340a57..1e522d43 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt @@ -7,30 +7,3 @@ add_library(cmvr_es::device::rh56dftp_dexhand ALIAS rh56dftp_dexhand) target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware cmvr_es::proto -lmodbus) install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - add_executable(rh56dftp_dexhand_stop_all_test - tests/rh56dftp_dexhand_stop_all_test.cpp - ) - target_link_libraries(rh56dftp_dexhand_stop_all_test PRIVATE - cmvr_es::device::rh56dftp_dexhand - gtest - gtest_main - pthread - ) - add_test( - NAME rh56dftp_dexhand_stop_all_test - COMMAND rh56dftp_dexhand_stop_all_test - ) - set(_rh56_stop_all_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _rh56_stop_all_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(rh56dftp_dexhand_stop_all_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_rh56_stop_all_test_environment}" - ) -endif() 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 6e19729b..388790f8 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h @@ -28,17 +28,14 @@ namespace cmvr::device { class ModbusController { public: ModbusController() = default; - virtual ~ModbusController(); + ~ModbusController(); - virtual bool open(const std::string& ip, int port); - virtual void close(); - virtual bool isOpen() const; + bool open(const std::string& ip, int port); + void close(); + bool isOpen() const; - virtual bool writeRegisters(int address, const uint16_t* values, int count); - virtual bool readRegisterBlock( - int start_address, - int count, - std::vector& values); + bool writeRegisters(int address, const uint16_t* values, int count); + bool readRegisterBlock(int start_address, int count, std::vector& values); private: void closeUnlocked(); @@ -61,9 +58,6 @@ 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"; } @@ -74,8 +68,6 @@ 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; @@ -118,18 +110,6 @@ 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 3d0875bd..44737981 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp @@ -21,11 +21,6 @@ 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; @@ -330,16 +325,8 @@ void ModbusController::closeUnlocked() { } RH56DFTPDexhand::RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& 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"); - } + : controller_(std::make_unique()), + dexhandCfg_(cfg) { id_ = dexhandCfg_.id(); ip_address_ = dexhandCfg_.ip(); if (dexhandCfg_.port() > 0) { @@ -365,9 +352,6 @@ 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; @@ -403,7 +387,6 @@ 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(); @@ -411,14 +394,8 @@ bool RH56DFTPDexhand::stop() { tactile_thread_.join(); } - { - // 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 (controller_) { + controller_->close(); } if (state() != Status::FAULT) { @@ -456,124 +433,13 @@ 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(), @@ -672,14 +538,6 @@ 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()) { @@ -728,12 +586,9 @@ 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() || - operational_paused_.load(std::memory_order_acquire)) { + if (requested_polling_mask_.none()) { polling_cv_.wait(lock, [this]() { - return !tactile_thread_running_.load(std::memory_order_acquire) || - (!operational_paused_.load(std::memory_order_acquire) && - requested_polling_mask_.any()); + return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_.any(); }); next_poll_deadline = std::chrono::steady_clock::now(); continue; @@ -756,9 +611,7 @@ void RH56DFTPDexhand::tactilePollingLoop() { } polling_cv_.wait_until(lock, next_poll_deadline, [this, mask]() { - return !tactile_thread_running_.load(std::memory_order_acquire) || - operational_paused_.load(std::memory_order_acquire) || - requested_polling_mask_ != mask; + return !tactile_thread_running_.load(std::memory_order_acquire) || requested_polling_mask_ != mask; }); } } @@ -814,9 +667,6 @@ 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/dexhand/rh56dftp_dexhand/tests/rh56dftp_dexhand_stop_all_test.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/tests/rh56dftp_dexhand_stop_all_test.cpp deleted file mode 100644 index 9414588c..00000000 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/tests/rh56dftp_dexhand_stop_all_test.cpp +++ /dev/null @@ -1,348 +0,0 @@ -#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace cmvr::device { -namespace { - -using namespace std::chrono_literals; - -class FakeModbusController final : public ModbusController { -public: - bool open(const std::string&, int) override - { - std::lock_guard lock(mutex_); - open_ = true; - return true; - } - - void close() override - { - std::lock_guard lock(mutex_); - open_ = false; - } - - bool isOpen() const override - { - std::lock_guard lock(mutex_); - return open_; - } - - bool writeRegisters( - int, - const uint16_t* values, - const int count) override - { - std::unique_lock lock(mutex_); - ++write_calls_; - write_started_ = true; - condition_.notify_all(); - condition_.wait(lock, [this] { return !block_write_; }); - if (!write_succeeds_) { - return false; - } - actual_angles_.assign(values, values + count); - return true; - } - - bool readRegisterBlock( - const int address, - const int count, - std::vector& values) override - { - std::unique_lock lock(mutex_); - ++read_calls_; - read_started_ = true; - condition_.notify_all(); - condition_.wait(lock, [this] { return !block_read_; }); - const std::vector* source = &actual_angles_; - std::vector sampled_angles; - if (address == 1546 && !actual_angle_samples_.empty()) { - const auto sample = actual_angle_samples_.front(); - actual_angle_samples_.pop_front(); - sampled_angles.assign(sample.begin(), sample.end()); - source = &sampled_angles; - } - values.assign(static_cast(count), 0U); - for (std::size_t index = 0; - index < values.size() && index < source->size(); - ++index) { - values[index] = (*source)[index]; - } - return read_succeeds_; - } - - void setActualAngles(const std::array& values) - { - std::lock_guard lock(mutex_); - actual_angles_.assign(values.begin(), values.end()); - actual_angle_samples_.clear(); - } - - void setActualAngleSamples( - std::deque> samples) - { - std::lock_guard lock(mutex_); - actual_angle_samples_ = std::move(samples); - } - - void setWriteSucceeds(const bool succeeds) - { - std::lock_guard lock(mutex_); - write_succeeds_ = succeeds; - } - - void blockNextRead() - { - std::lock_guard lock(mutex_); - block_read_ = true; - read_started_ = false; - } - - void releaseRead() - { - { - std::lock_guard lock(mutex_); - block_read_ = false; - } - condition_.notify_all(); - } - - bool waitForRead(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [this] { return read_started_; }); - } - - bool waitForReadCalls( - const int expected, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [this, expected] { return read_calls_ >= expected; }); - } - - void blockNextWrite() - { - std::lock_guard lock(mutex_); - block_write_ = true; - write_started_ = false; - } - - void releaseWrite() - { - { - std::lock_guard lock(mutex_); - block_write_ = false; - } - condition_.notify_all(); - } - - bool waitForWrite(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [this] { return write_started_; }); - } - - int readCalls() const - { - std::lock_guard lock(mutex_); - return read_calls_; - } - - int writeCalls() const - { - std::lock_guard lock(mutex_); - return write_calls_; - } - -private: - mutable std::mutex mutex_; - std::condition_variable condition_; - std::vector actual_angles_{6U, 0U}; - std::deque> actual_angle_samples_; - bool open_{false}; - bool block_read_{false}; - bool block_write_{false}; - bool read_started_{false}; - bool write_started_{false}; - bool read_succeeds_{true}; - bool write_succeeds_{true}; - int read_calls_{0}; - int write_calls_{0}; -}; - -struct TestHand { - TestHand() - { - config.set_id("rh56-test"); - config.set_ip("fake-modbus"); - auto controller = std::make_unique(); - fake = controller.get(); - hand = std::make_unique( - config, std::move(controller)); - EXPECT_TRUE(hand->init()); - } - - ~TestHand() - { - if (hand) { - hand->stop(); - } - } - - config::RH56DFTPDexHandConfig config; - FakeModbusController* fake{nullptr}; - std::unique_ptr hand; -}; - -TEST(RH56DFTPDexhandStopAllTest, IdleTactileDeviceStopsWithoutClosingLifecycle) -{ - TestHand fixture; - - EXPECT_TRUE(fixture.hand->stopOperationalActivity()); - EXPECT_EQ(fixture.hand->state(), AbstractDexHand::Status::INITIALIZED); - EXPECT_TRUE(fixture.fake->isOpen()); - - const int reads_before = fixture.fake->readCalls(); - (void)fixture.hand->getSensorData( - AbstractDexHand::FingerType::INDEX, - AbstractDexHand::TactileRegion::TIP); - EXPECT_EQ(fixture.fake->readCalls(), reads_before); -} - -TEST(RH56DFTPDexhandStopAllTest, UnreachedAngleTargetFailsClosedThenRecovers) -{ - TestHand fixture; - const std::vector target{100, 200, 300, 400, 500, 600}; - - fixture.hand->setAngles(target); - fixture.fake->setActualAngles({0, 0, 0, 0, 0, 0}); - EXPECT_FALSE(fixture.hand->stopOperationalActivity()); - - fixture.fake->setActualAngles({100, 200, 300, 400, 500, 600}); - EXPECT_TRUE(fixture.hand->stopOperationalActivity()); - EXPECT_TRUE(fixture.fake->isOpen()); -} - -TEST(RH56DFTPDexhandStopAllTest, MovingSampleAtTargetDoesNotConfirmStop) -{ - TestHand fixture; - const std::vector target{100, 200, 300, 400, 500, 600}; - - fixture.hand->setAngles(target); - fixture.fake->setActualAngleSamples({ - {100, 200, 300, 400, 500, 600}, - {103, 203, 303, 403, 503, 603}, - {106, 206, 306, 406, 506, 606}, - }); - EXPECT_FALSE(fixture.hand->stopOperationalActivity()); - - fixture.fake->setActualAngles({100, 200, 300, 400, 500, 600}); - EXPECT_TRUE(fixture.hand->stopOperationalActivity()); -} - -TEST(RH56DFTPDexhandStopAllTest, FailedAngleWriteRemainsUnconfirmed) -{ - TestHand fixture; - fixture.fake->setActualAngles({0, 0, 0, 0, 0, 0}); - fixture.fake->setWriteSucceeds(false); - - fixture.hand->setAngles({100, 200, 300, 400, 500, 600}); - - EXPECT_FALSE(fixture.hand->stopOperationalActivity()); -} - -TEST(RH56DFTPDexhandStopAllTest, StopWaitsForAdmittedAngleWrite) -{ - TestHand fixture; - fixture.fake->blockNextWrite(); - const std::vector target{100, 200, 300, 400, 500, 600}; - auto command = std::async(std::launch::async, [&] { - fixture.hand->setAngles(target); - }); - ASSERT_TRUE(fixture.fake->waitForWrite(500ms)); - - auto stop = std::async(std::launch::async, [&] { - return fixture.hand->stopOperationalActivity(); - }); - EXPECT_EQ(stop.wait_for(20ms), std::future_status::timeout); - - fixture.fake->releaseWrite(); - EXPECT_EQ(command.wait_for(500ms), std::future_status::ready); - command.get(); - ASSERT_EQ(stop.wait_for(500ms), std::future_status::ready); - EXPECT_TRUE(stop.get()); - - const int writes_before = fixture.fake->writeCalls(); - fixture.hand->setAngles(target); - EXPECT_EQ(fixture.fake->writeCalls(), writes_before); - EXPECT_TRUE(fixture.hand->resumeOperationalActivity()); - fixture.hand->setAngles(target); - EXPECT_EQ(fixture.fake->writeCalls(), writes_before + 1); -} - -TEST(RH56DFTPDexhandStopAllTest, StopWaitsForAdmittedTactileRead) -{ - TestHand fixture; - fixture.fake->blockNextRead(); - auto read = std::async(std::launch::async, [&] { - return fixture.hand->getSensorData( - AbstractDexHand::FingerType::INDEX, - AbstractDexHand::TactileRegion::TIP); - }); - ASSERT_TRUE(fixture.fake->waitForRead(500ms)); - - auto stop = std::async(std::launch::async, [&] { - return fixture.hand->stopOperationalActivity(); - }); - EXPECT_EQ(stop.wait_for(20ms), std::future_status::timeout); - - fixture.fake->releaseRead(); - EXPECT_EQ(read.wait_for(500ms), std::future_status::ready); - (void)read.get(); - ASSERT_EQ(stop.wait_for(500ms), std::future_status::ready); - EXPECT_TRUE(stop.get()); -} - -TEST(RH56DFTPDexhandStopAllTest, StopDrainsAndPausesBackgroundTactilePolling) -{ - TestHand fixture; - ASSERT_TRUE(fixture.hand->start()); - - const int reads_before_block = fixture.fake->readCalls(); - fixture.fake->blockNextRead(); - ASSERT_TRUE(fixture.fake->waitForReadCalls(reads_before_block + 1, 500ms)); - - auto stop = std::async(std::launch::async, [&] { - return fixture.hand->stopOperationalActivity(); - }); - EXPECT_EQ(stop.wait_for(20ms), std::future_status::timeout); - - fixture.fake->releaseRead(); - ASSERT_EQ(stop.wait_for(500ms), std::future_status::ready); - EXPECT_TRUE(stop.get()); - - const int reads_after_stop = fixture.fake->readCalls(); - std::this_thread::sleep_for(30ms); - EXPECT_EQ(fixture.fake->readCalls(), reads_after_stop); - - ASSERT_TRUE(fixture.hand->resumeOperationalActivity()); - EXPECT_TRUE(fixture.fake->waitForReadCalls(reads_after_stop + 1, 500ms)); -} - -} // namespace -} // namespace cmvr::device diff --git a/cmvr-es/devices/gripper/abstract_gripper.h b/cmvr-es/devices/gripper/abstract_gripper.h index d4dcfed1..728d094b 100644 --- a/cmvr-es/devices/gripper/abstract_gripper.h +++ b/cmvr-es/devices/gripper/abstract_gripper.h @@ -23,11 +23,6 @@ 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/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index a7399736..c3081330 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -17,5 +17,4 @@ add_subdirectory(drivers/ti5_canopen) add_subdirectory(drivers/mujoco) add_subdirectory(bus_runtime) add_subdirectory(drivers/ethercat_motor) - add_subdirectory(manager) diff --git a/cmvr-es/devices/motor/README.md b/cmvr-es/devices/motor/README.md deleted file mode 100644 index 46949387..00000000 --- a/cmvr-es/devices/motor/README.md +++ /dev/null @@ -1,47 +0,0 @@ -# Motor 设备模块 - -`devices/motor/` 提供电机管理、协议适配、总线 runtime 和厂商驱动。Service、 -RobotArm 和业务 Task 只依赖 `MotorManager`/`AbstractMotor` 的稳定接口，不应 -直接访问 CAN、EtherCAT、MuJoCo 或厂商 SDK。 - -返回 [Devices 模块指南](../README.md) 或 [项目总览](../../../README.md)。 - -## 目录职责 - -| 目录 | 职责 | -| --- | --- | -| `manager/` | 创建 MotorGroup，按 `motor_id`/`joint_name` 暴露 `AbstractMotor` | -| `bus_runtime/` | 连接、收发和总线生命周期 | -| `drivers/` | CANopen、EtherCAT 和 MuJoCo 等具体后端 | - -## 当前后端 - -- CAN + TI5 CANopen； -- EtherCAT + EYOU CiA 402； -- MuJoCo 仿真电机。 - -配置示例： - -- [`ti5_motors.pb.txt`](../../config/devices/motor/ti5_motors.pb.txt) -- [`ethercat_motors.pb.txt`](../../config/devices/motor/ethercat_motors.pb.txt) -- [`mujoco_motors.pb.txt`](../../config/devices/motor/mujoco_motors.pb.txt) - -对外接口与控制权语义见 [MotorService 文档](../../service/README.md#motorservice)。 - -## 安全边界 - -- MotorService 是单轴 API，不提供多轴同扫描周期的原子 commit； -- 软件 `emergencyStop` 和 Quick Stop 不具备功能安全等级； -- 真实设备必须具有经风险评估确定的硬接线急停、安全继电器和驱动器安全链； -- 新硬件配置保持 `enable: false`，完成方向、限位和故障注入验证后才能启用。 - -## 测试 - -```bash -cmake --build build --target grpc_motor_service_test -j4 -ctest --test-dir build \ - -R '^grpc_motor_service_test$' \ - --output-on-failure -``` - -该测试使用 fake motor，不替代真实总线、驱动器或安全链验证。 diff --git a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt index 458b8e46..ce09edab 100644 --- a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt +++ b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(motor_bus_runtime SHARED set(IGH_ETHERCAT_ROOT ${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0 ) + target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} diff --git a/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp index 18d54623..09c1284c 100644 --- a/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp +++ b/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp @@ -68,18 +68,16 @@ bool CanMotorBusRuntime::start() return false; } - // Start the receiver first so a protocol response cannot arrive before the - // receive path is ready. - auto ret = receiver_->Start(); + auto ret = sender_->Start(); if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_; + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_; stop(); return false; } - ret = sender_->Start(); + ret = receiver_->Start(); if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_; + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_; stop(); return false; } diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp index d5e3b47f..1e0eeb5f 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp @@ -153,11 +153,8 @@ void Cia402StatusMonitor::reportStatuswordTransition_( const StatusSample& previous, const StatusSample& current) const { - const bool device_state_changed = - !had_previous || !previous.read_ok || - (previous.statusword & 0x006F) != (current.statusword & 0x006F); const bool status_changed = - device_state_changed || + !had_previous || !previous.read_ok || previous.status_problem != current.status_problem || (previous.statusword & 0x0888) != (current.statusword & 0x0888); if (current.status_problem && status_changed) { @@ -181,20 +178,9 @@ void Cia402StatusMonitor::reportStatuswordTransition_( previous.status_problem) { CMVR_LOG(INFO) << "[Cia402StatusMonitor] [statusword] recovered" << ", node=" << static_cast(node_id) - << ", current_statusword=0x" << std::hex << std::uppercase - << std::setw(4) << std::setfill('0') << current.statusword - << std::dec << std::nouppercase << std::setfill(' ') - << ' ' << deviceStateName_(current.statusword) << ", previous_statusword=0x" << std::hex << std::uppercase << std::setw(4) << std::setfill('0') << previous.statusword << std::dec << std::nouppercase << std::setfill(' '); - } else if (device_state_changed) { - CMVR_LOG(INFO) << "[Cia402StatusMonitor] [statusword] 0x" - << std::hex << std::uppercase << std::setw(4) - << std::setfill('0') << current.statusword - << std::dec << std::nouppercase << std::setfill(' ') - << ' ' << deviceStateName_(current.statusword) - << ", node=" << static_cast(node_id); } } diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h index 56cda849..3673355a 100644 --- a/cmvr-es/devices/motor/manager/include/motor_manager.h +++ b/cmvr-es/devices/motor/manager/include/motor_manager.h @@ -54,15 +54,6 @@ public: std::vector& positions, std::vector& velocities) const; - // A MotorRobotArm owns its joints for the lifetime of the arm instance. - // Direct per-motor control must not compete with that group controller. - bool claimArmJoints(const std::string& arm_id, - const std::vector& joint_names, - std::uint64_t& claim_id, - std::string* error = nullptr); - void releaseArmJoints(std::uint64_t claim_id) noexcept; - std::string armOwnerForJoint(const std::string& joint_name) const; - static std::shared_ptr managerFor(const std::string& id); static std::shared_ptr mujocoWorldFor(const std::string& id); static void setActiveJoints(const std::string& motor_manager_id, @@ -102,13 +93,6 @@ private: mutable std::mutex motors_mutex_; std::unordered_map> motors_by_id_; std::unordered_map> motors_by_joint_; - struct ArmJointClaim { - std::string arm_id; - std::vector joint_names; - }; - std::uint64_t next_arm_claim_id_{0}; - std::unordered_map arm_claims_; - std::unordered_map arm_claim_by_joint_; bool initialized_{false}; static std::mutex registry_mutex_; diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index aef43d0b..3a6b5118 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -279,120 +279,6 @@ bool MotorManager::readFeedbacksAtomic( return false; } -bool MotorManager::claimArmJoints( - const std::string& arm_id, - const std::vector& joint_names, - std::uint64_t& claim_id, - std::string* error) -{ - claim_id = 0; - if (error) { - error->clear(); - } - if (arm_id.empty() || joint_names.empty()) { - if (error) { - *error = "arm id and joint names are required"; - } - return false; - } - - std::unordered_set unique_joints; - unique_joints.reserve(joint_names.size()); - std::lock_guard lock(motors_mutex_); - for (const auto& joint_name : joint_names) { - if (joint_name.empty() || !unique_joints.insert(joint_name).second) { - if (error) { - *error = joint_name.empty() - ? "arm joint name cannot be empty" - : "arm joint is listed more than once: " + joint_name; - } - return false; - } - if (motors_by_joint_.count(joint_name) == 0U) { - if (error) { - *error = "motor not found for arm joint: " + joint_name; - } - return false; - } - const auto existing = arm_claim_by_joint_.find(joint_name); - if (existing != arm_claim_by_joint_.end()) { - const auto owner = arm_claims_.find(existing->second); - if (error) { - *error = "motor joint is already controlled by RobotArm"; - if (owner != arm_claims_.end()) { - *error += " '" + owner->second.arm_id + "'"; - } - *error += ": " + joint_name; - } - return false; - } - } - - do { - ++next_arm_claim_id_; - } while (next_arm_claim_id_ == 0U || - arm_claims_.count(next_arm_claim_id_) != 0U); - - ArmJointClaim claim; - claim.arm_id = arm_id; - claim.joint_names.assign(unique_joints.begin(), unique_joints.end()); - const auto new_claim_id = next_arm_claim_id_; - arm_claims_.emplace(new_claim_id, std::move(claim)); - try { - for (const auto& joint_name : unique_joints) { - arm_claim_by_joint_.emplace(joint_name, new_claim_id); - } - } catch (...) { - for (auto it = arm_claim_by_joint_.begin(); - it != arm_claim_by_joint_.end();) { - if (it->second == new_claim_id) { - it = arm_claim_by_joint_.erase(it); - } else { - ++it; - } - } - arm_claims_.erase(new_claim_id); - throw; - } - claim_id = new_claim_id; - return true; -} - -void MotorManager::releaseArmJoints(const std::uint64_t claim_id) noexcept -{ - if (claim_id == 0U) { - return; - } - try { - std::lock_guard lock(motors_mutex_); - const auto claim = arm_claims_.find(claim_id); - if (claim == arm_claims_.end()) { - return; - } - for (const auto& joint_name : claim->second.joint_names) { - const auto owner = arm_claim_by_joint_.find(joint_name); - if (owner != arm_claim_by_joint_.end() && - owner->second == claim_id) { - arm_claim_by_joint_.erase(owner); - } - } - arm_claims_.erase(claim); - } catch (...) { - } -} - -std::string MotorManager::armOwnerForJoint( - const std::string& joint_name) const -{ - std::lock_guard lock(motors_mutex_); - const auto owner = arm_claim_by_joint_.find(joint_name); - if (owner == arm_claim_by_joint_.end()) { - return {}; - } - const auto claim = arm_claims_.find(owner->second); - return claim == arm_claims_.end() ? std::string{} : claim->second.arm_id; -} - std::shared_ptr MotorManager::managerFor(const std::string& id) { std::lock_guard lock(registry_mutex_); diff --git a/cmvr-es/devices/speaker/abstract_speaker.h b/cmvr-es/devices/speaker/abstract_speaker.h index cfdb62d9..ab24542d 100644 --- a/cmvr-es/devices/speaker/abstract_speaker.h +++ b/cmvr-es/devices/speaker/abstract_speaker.h @@ -21,11 +21,6 @@ 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/CMakeLists.txt b/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt index c37d546c..7ff77300 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/CMakeLists.txt @@ -7,32 +7,3 @@ add_library(cmvr_es::device::ffmpeg_speaker ALIAS ffmpeg_speaker) target_link_libraries(ffmpeg_speaker PRIVATE -lpulse-simple -lpulse cmvr_es::proto) install(TARGETS ffmpeg_speaker LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - add_executable(ffmpeg_speaker_lifecycle_test - tests/ffmpeg_speaker_lifecycle_test.cpp - ) - target_link_libraries(ffmpeg_speaker_lifecycle_test - PRIVATE - cmvr_es::device::ffmpeg_speaker - avcodec - avformat - avutil - swresample - ) - add_test( - NAME ffmpeg_speaker_lifecycle_test - COMMAND ffmpeg_speaker_lifecycle_test - ) - set(_ffmpeg_speaker_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _ffmpeg_speaker_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(ffmpeg_speaker_lifecycle_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_ffmpeg_speaker_test_environment}" - ) -endif() 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 6c95eb37..124213b3 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/include/ffmpeg_speaker.h @@ -32,7 +32,6 @@ 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; @@ -46,7 +45,6 @@ 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 1153e3a3..0a6be5fb 100644 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp +++ b/cmvr-es/devices/speaker/ffmpeg_speaker/src/ffmpeg_speaker.cpp @@ -34,7 +34,6 @@ 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; @@ -95,6 +94,7 @@ void ffmpegSpeaker::resetPlayState() // 清空所有帧 } + state_.is_initialized = false; is_streaming_input_ = false; audio_path_.clear(); @@ -102,22 +102,11 @@ 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; diff --git a/cmvr-es/devices/speaker/ffmpeg_speaker/tests/ffmpeg_speaker_lifecycle_test.cpp b/cmvr-es/devices/speaker/ffmpeg_speaker/tests/ffmpeg_speaker_lifecycle_test.cpp deleted file mode 100644 index 432a82a6..00000000 --- a/cmvr-es/devices/speaker/ffmpeg_speaker/tests/ffmpeg_speaker_lifecycle_test.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#include "include/ffmpeg_speaker.h" - -#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() -{ - cmvr::config::FFMpegSpeakerConfig config; - config.set_id("lifecycle-test-speaker"); - cmvr::device::ffmpegSpeaker speaker(config); - cmvr::device::SpeakerState state{}; - - CHECK_TRUE(speaker.init()); - speaker.getState(state); - CHECK_TRUE(state.is_initialized); - - speaker.resetPlayState(); - speaker.getState(state); - CHECK_TRUE(state.is_initialized); - - CHECK_TRUE(speaker.stopPlayback()); - speaker.getState(state); - CHECK_TRUE(state.is_initialized); - - speaker.stopStreaming(); - speaker.getState(state); - CHECK_TRUE(state.is_initialized); - - CHECK_TRUE(speaker.stop()); - speaker.getState(state); - CHECK_TRUE(!state.is_initialized); - - std::cout << "ffmpeg_speaker_lifecycle_test: PASS\n"; - return 0; -} diff --git a/cmvr-es/main.cpp b/cmvr-es/main.cpp index 81c7621d..2e422db8 100644 --- a/cmvr-es/main.cpp +++ b/cmvr-es/main.cpp @@ -1,12 +1,5 @@ #include -#include -#include -#include -#include -#include #include -#include -#include #include "common/base/logging/logger.h" #include "runtime/include/cmvr_runtime.h" @@ -22,64 +15,6 @@ bool blockShutdownSignals(sigset_t& shutdown_signals) return pthread_sigmask(SIG_BLOCK, &shutdown_signals, nullptr) == 0; } -struct CommandLineOptions { - std::string config_path; - double control_period_s{0.001}; - bool show_help{false}; -}; - -void printUsage(const char* program) -{ - 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"; -} - -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; - } - return true; -} - } // namespace namespace fs = std::filesystem; @@ -127,17 +62,8 @@ static bool setupIntelMediaEnvironment() } -int main(int argc, char* argv[]) +int main() { - CommandLineOptions options; - if (!parseCommandLine(argc, argv, options)) { - printUsage(argv[0]); - return 2; - } - if (options.show_help) { - printUsage(argv[0]); - return 0; - } if (!setupIntelMediaEnvironment()) { return EXIT_FAILURE; } @@ -148,13 +74,10 @@ int main(int argc, char* argv[]) } cmvr::Runtime runtime; - const bool initialized = options.config_path.empty() - ? runtime.init() - : runtime.init(options.config_path); - if (!initialized) { + if (!runtime.init()) { return 1; } - if (!runtime.startTasks(options.control_period_s)) { + if (!runtime.startTasks()) { return 1; } diff --git a/cmvr-es/manager/README.md b/cmvr-es/manager/README.md index 620a6288..7ef1429b 100644 --- a/cmvr-es/manager/README.md +++ b/cmvr-es/manager/README.md @@ -6,20 +6,13 @@ ## 当前管理器 -管理模块目录统一使用 `*_manager` 后缀，主管理类使用 `*Manager` 后缀。工厂、适配器、 -账本、快照和结果结构体属于管理器内部的支撑类型，保留其职责名称，不强行改成 -`*Manager`。 - | 目录 | CMake target | 职责 | | --- | --- | --- | -| [`control_authority_manager/`](control_authority_manager/) | `cmvr_es::control_authority_manager` | 控制权租约、代际、dispatch fence 和 quarantine | | [`device_manager/`](device_manager/) | `cmvr_es::device_manager` | 按配置创建、初始化、查询和批量启停设备 | -| [`safety_manager/`](safety_manager/) | `cmvr_es::safety_manager` | Sensor/Control 安全准入、StopAll、恢复和命令账本 | | [`task_manager/`](task_manager/) | `cmvr_es::task_manager` | 创建任务、校验运行模式、统一启停和调度周期任务 | -| [`media_source_manager/`](media_source_manager/) | `cmvr_es::media_source_manager`、`cmvr_es::device_media_source_adapter` | 实时媒体源注册、按需启停和多消费者分发 | +| [`media_source_hub/`](media_source_hub/) | `cmvr_es::media_source_hub`、`cmvr_es::device_media_source_adapter` | 实时媒体源注册、按需启停和多消费者分发 | -`manager/` 当前没有聚合 `CMakeLists.txt`，所有模块由 [`../CMakeLists.txt`](../CMakeLists.txt) -按依赖顺序加入。新增 manager 时必须同时更新目录、target、依赖顺序和本 README。 +`manager/` 当前没有聚合 `CMakeLists.txt`，三个子目录由 [`../CMakeLists.txt`](../CMakeLists.txt) 分别加入。新增 manager 时必须显式更新该文件。 ## 进程生命周期 @@ -37,8 +30,7 @@ - DeviceManager 构造不会自动调用全部设备的 `start()`； - 当前主退出路径没有调用 `DeviceManager::stop()`； -- `SystemService/StopAll` 只停止当前运动、控制和媒体活动，不调用 - `DeviceManager::stop()`，成功返回后可继续接受新命令； +- `SystemService/StopAll` 会调用 DeviceManager stop； - `DeviceManager::destroyInstance()` 不调用设备 stop，销毁前必须先显式停止； - `TaskManager::destroyInstance()` 会调用 `stopRunTask()`，但 manager 未处于 running 状态时该调用会直接返回； - DeviceManager 和 TaskManager 都是首次配置生效的单例，不支持热加载。 @@ -143,12 +135,12 @@ - 有顺序依赖的工作应放入同一协调任务或显式建模； - task 返回后，其内部状态并发安全由具体实现负责。 -## MediaSourceManager +## MediaSourceHub 关键文件： -- [`media_source_manager/include/media_source_manager.h`](media_source_manager/include/media_source_manager.h) -- [`media_source_manager/src/device_media_source_adapter.cpp`](media_source_manager/src/device_media_source_adapter.cpp) +- [`media_source_hub/include/media_source_hub.h`](media_source_hub/include/media_source_hub.h) +- [`media_source_hub/src/device_media_source_adapter.cpp`](media_source_hub/src/device_media_source_adapter.cpp) - [`../common/media/media_frame.h`](../common/media/media_frame.h) - [`../common/base/ring_buffer.h`](../common/base/ring_buffer.h) @@ -159,8 +151,7 @@ | 摄像头彩色流 | `/video/color` | 64 | | 麦克风主流 | `/audio/main` | 256 | -当前 gRPC RGB/麦克风流和 QUIC 彩色/麦克风轨道使用 MediaSourceManager；gRPC Depth/RGBD -仍直接读取设备帧。 +当前 gRPC RGB/麦克风流和 QUIC 彩色/麦克风轨道使用 Hub；gRPC Depth/RGBD 仍直接读取设备帧。 ### 注册新媒体源 @@ -219,7 +210,7 @@ ring generation 不等于 `TrackDescriptor::generation`，ring 的 `ReadResult.s ## 新增第四种 Manager -1. 先确认能力不是 DeviceManager、TaskManager 或 MediaSourceManager 的子职责； +1. 先确认能力不是 DeviceManager、TaskManager 或 MediaSourceHub 的子职责； 2. 定义所有权、初始化、start/stop 和线程模型； 3. 避免新增无必要的全局单例； 4. 新建独立目录、头文件、实现和 CMake target； @@ -229,13 +220,13 @@ ring generation 不等于 `TrackDescriptor::generation`，ring 的 `ReadResult.s ## 测试 -MediaSourceManager： +MediaSourceHub： ```bash -cmake --build build --target media_source_manager_test +cmake --build build --target media_source_hub_test ctest \ --test-dir build \ - -R '^media_source_manager_test$' \ + -R '^media_source_hub_test$' \ --output-on-failure ``` diff --git a/cmvr-es/manager/control_authority_manager/CMakeLists.txt b/cmvr-es/manager/control_authority_manager/CMakeLists.txt deleted file mode 100644 index f69da862..00000000 --- a/cmvr-es/manager/control_authority_manager/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -add_library(control_authority_manager STATIC - src/control_authority_manager.cpp -) -target_compile_features(control_authority_manager PUBLIC cxx_std_17) -target_include_directories(control_authority_manager - PUBLIC - ${PROJECT_SOURCE_DIR}/cmvr-es -) - -add_library( - cmvr_es::control_authority_manager - ALIAS control_authority_manager -) -install(TARGETS control_authority_manager ARCHIVE DESTINATION lib) - -if(BUILD_TESTING) - add_executable(control_authority_manager_test - tests/control_authority_manager_test.cpp - ) - target_link_libraries(control_authority_manager_test - PRIVATE - cmvr_es::control_authority_manager - gtest - gtest_main - pthread - ) - add_test( - NAME control_authority_manager_test - COMMAND control_authority_manager_test - ) - set_tests_properties(control_authority_manager_test PROPERTIES - TIMEOUT 10 - ) -endif() diff --git a/cmvr-es/manager/control_authority_manager/include/control_authority_manager.h b/cmvr-es/manager/control_authority_manager/include/control_authority_manager.h deleted file mode 100644 index ceba5057..00000000 --- a/cmvr-es/manager/control_authority_manager/include/control_authority_manager.h +++ /dev/null @@ -1,170 +0,0 @@ -#ifndef CMVR_ES_CONTROL_AUTHORITY_MANAGER_H -#define CMVR_ES_CONTROL_AUTHORITY_MANAGER_H - -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::control { - -struct ControlLeaseToken { - std::string resource_id; - std::string owner_id; - std::uint64_t generation{0}; - - bool valid() const noexcept - { - return !resource_id.empty() && - !owner_id.empty() && - generation != 0U; - } -}; - -struct ControlAcquireResult { - bool acquired{false}; - ControlLeaseToken token; - std::string detail; -}; - -class ControlDispatchGuard; - -// Process-wide, transport-independent control ownership. The generation in a -// token prevents a delayed release from an old network session from releasing -// a newer lease on the same arm. -class ControlAuthorityManager { -public: - using Duration = std::chrono::milliseconds; - - static ControlAuthorityManager& instance(); - - ControlAcquireResult tryAcquire( - const std::string& resource_id, - const std::string& owner_id, - Duration ttl); - - // Atomically invalidates a normal control lease and joins a safety - // barrier. Each safety caller receives an independent token; normal - // control remains blocked until the last safety token is released. - ControlAcquireResult preemptAcquire( - const std::string& resource_id, - const std::string& owner_id, - Duration ttl); - - // Converts the expected normal lease into a safety barrier only while it - // is still the current lease. A stale token never preempts a successor or - // joins an existing safety barrier. - ControlAcquireResult preemptAcquireIfCurrent( - const ControlLeaseToken& expected_token, - const std::string& owner_id, - Duration ttl); - - // Waits for normal lease handlers displaced by the current safety barrier - // to release their tokens and for their in-flight dispatches to finish. - // Returns false on timeout or when safety_token is no longer a holder of - // the current entry. - bool waitForPreemptedRelease( - const ControlLeaseToken& safety_token, - Duration timeout); - - // Permanently blocks the resource only if the expected normal lease is - // still current. A later safety holder may clear this fail-closed state - // only after it has independently confirmed the preempted handler exited. - bool quarantineIfCurrent( - const ControlLeaseToken& expected_token) noexcept; - - // Abandons a safety token while retaining its barrier. A retired token can - // no longer be validated, released, or used as a recovery authority. This - // lets a failed stop path discard local token ownership without silently - // reopening the resource. - bool retireSafetyHolder( - const ControlLeaseToken& safety_token) noexcept; - - // Clears retired safety holders and a tokenless quarantine after a newer, - // active safety holder has confirmed every preempted normal handler has - // exited. Other active safety holders are deliberately preserved. - bool recoverRetiredSafetyHolders( - const ControlLeaseToken& recovery_token) noexcept; - - bool renew(const ControlLeaseToken& token, Duration ttl); - bool validate(const ControlLeaseToken& token); - - // Use only around a bounded device-command submission. Never retain this - // guard while waiting for physical motion or another long-running task. - ControlDispatchGuard tryBeginDispatch( - const ControlLeaseToken& token); - void release(const ControlLeaseToken& token) noexcept; - - // Safety/control paths which do not possess a lease use this query to - // reject mutating commands. Read-only state and stop/torque-off commands - // are intentionally allowed by their callers. - bool isLeased(const std::string& resource_id); - void revoke(const std::string& resource_id) noexcept; - - // Test/process teardown hook. Runtime code should release/revoke exact - // resources instead of clearing unrelated ownership. - void clear() noexcept; - -private: - friend class ControlDispatchGuard; - - struct SafetyHolder { - std::string owner_id; - bool retired{false}; - }; - - struct Entry; - - static void quarantine_(Entry& entry) noexcept; - bool expired_(const Entry& entry) const noexcept; - static bool isSafetyHolder_( - const Entry& entry, - const ControlLeaseToken& token) noexcept; - static bool isActiveSafetyHolder_( - const Entry& entry, - const ControlLeaseToken& token) noexcept; - static bool canErase_(const Entry& entry) noexcept; - static void invalidateToDispatchFence_(Entry& entry) noexcept; - void endDispatch_(const std::shared_ptr& entry) noexcept; - - std::mutex mutex_; - std::condition_variable release_cv_; - std::unordered_map> entries_; - std::uint64_t next_generation_{0}; -}; - -// Tracks one bounded backend dispatch without retaining the process-wide -// authority lock. Safety preemption invalidates the lease immediately, while -// waitForPreemptedRelease() joins both the displaced handler and its in-flight -// dispatches before the safety operation reaches the device. -class ControlDispatchGuard final { -public: - ControlDispatchGuard() noexcept = default; - ~ControlDispatchGuard() noexcept; - - ControlDispatchGuard(ControlDispatchGuard&& other) noexcept; - ControlDispatchGuard& operator=(ControlDispatchGuard&& other) noexcept; - - ControlDispatchGuard(const ControlDispatchGuard&) = delete; - ControlDispatchGuard& operator=(const ControlDispatchGuard&) = delete; - - bool acquired() const noexcept { return entry_ != nullptr; } - -private: - friend class ControlAuthorityManager; - - ControlDispatchGuard( - ControlAuthorityManager* manager, - std::shared_ptr entry) noexcept; - void reset_() noexcept; - - ControlAuthorityManager* manager_{nullptr}; - std::shared_ptr entry_; -}; - -} // namespace cmvr::control - -#endif // CMVR_ES_CONTROL_AUTHORITY_MANAGER_H diff --git a/cmvr-es/manager/control_authority_manager/src/control_authority_manager.cpp b/cmvr-es/manager/control_authority_manager/src/control_authority_manager.cpp deleted file mode 100644 index 19063685..00000000 --- a/cmvr-es/manager/control_authority_manager/src/control_authority_manager.cpp +++ /dev/null @@ -1,682 +0,0 @@ -#include "manager/control_authority_manager/include/control_authority_manager.h" - -#include - -namespace cmvr::control { - -struct ControlAuthorityManager::Entry { - std::string resource_id; - std::string owner_id; - std::uint64_t generation{0}; - std::chrono::steady_clock::time_point deadline; - bool preemptible{true}; - bool quarantined{false}; - bool quarantined_normal_pending{false}; - bool dispatch_fence_only{false}; - std::uint64_t in_flight_dispatches{0}; - std::unordered_map safety_holders; - std::unordered_map - preempted_normal_holders; -}; - -ControlDispatchGuard::ControlDispatchGuard( - ControlAuthorityManager* const manager, - std::shared_ptr entry) noexcept - : manager_(manager), - entry_(std::move(entry)) -{ -} - -ControlDispatchGuard::~ControlDispatchGuard() noexcept -{ - reset_(); -} - -ControlDispatchGuard::ControlDispatchGuard( - ControlDispatchGuard&& other) noexcept - : manager_(std::exchange(other.manager_, nullptr)), - entry_(std::move(other.entry_)) -{ -} - -ControlDispatchGuard& ControlDispatchGuard::operator=( - ControlDispatchGuard&& other) noexcept -{ - if (this != &other) { - reset_(); - manager_ = std::exchange(other.manager_, nullptr); - entry_ = std::move(other.entry_); - } - return *this; -} - -void ControlDispatchGuard::reset_() noexcept -{ - if (entry_ == nullptr) { - return; - } - auto entry = std::move(entry_); - auto* const manager = std::exchange(manager_, nullptr); - manager->endDispatch_(entry); -} - -ControlAuthorityManager& ControlAuthorityManager::instance() -{ - static ControlAuthorityManager manager; - return manager; -} - -ControlAcquireResult ControlAuthorityManager::tryAcquire( - const std::string& resource_id, - const std::string& owner_id, - const Duration ttl) -{ - if (resource_id.empty() || owner_id.empty() || - ttl <= Duration::zero()) { - return {false, {}, "invalid control lease request"}; - } - - std::lock_guard lock(mutex_); - const auto existing = entries_.find(resource_id); - if (existing != entries_.end()) { - auto& entry = *existing->second; - if (!expired_(entry)) { - if (entry.dispatch_fence_only) { - return { - false, - {}, - "control resource still has an in-flight dispatch"}; - } - return { - false, - {}, - "control resource is already leased by " + - entry.owner_id}; - } - if (entry.in_flight_dispatches != 0U) { - invalidateToDispatchFence_(entry); - return { - false, - {}, - "control resource still has an in-flight dispatch"}; - } - entries_.erase(existing); - } - - ControlLeaseToken token; - token.resource_id = resource_id; - token.owner_id = owner_id; - token.generation = ++next_generation_; - - auto entry = std::make_shared(); - entry->resource_id = resource_id; - entry->owner_id = owner_id; - entry->generation = token.generation; - entry->deadline = std::chrono::steady_clock::now() + ttl; - entries_.emplace(resource_id, std::move(entry)); - return {true, std::move(token), {}}; -} - -ControlAcquireResult ControlAuthorityManager::preemptAcquire( - const std::string& resource_id, - const std::string& owner_id, - const Duration ttl) -{ - if (resource_id.empty() || owner_id.empty() || - ttl <= Duration::zero()) { - return {false, {}, "invalid control barrier request"}; - } - - std::lock_guard lock(mutex_); - const auto existing = entries_.find(resource_id); - if (existing != entries_.end() && - !existing->second->preemptible && - !existing->second->dispatch_fence_only) { - ControlLeaseToken token; - token.resource_id = resource_id; - token.owner_id = owner_id; - token.generation = ++next_generation_; - existing->second->safety_holders.emplace( - token.generation, - SafetyHolder{token.owner_id, false}); - return {true, std::move(token), {}}; - } - - const bool replacing_normal = - existing != entries_.end() && existing->second->preemptible; - try { - ControlLeaseToken token; - token.resource_id = resource_id; - token.owner_id = owner_id; - token.generation = ++next_generation_; - - std::unordered_map safety_holders; - safety_holders.emplace( - token.generation, - SafetyHolder{owner_id, false}); - std::string safety_owner = owner_id; - std::unordered_map - preempted_normal_holders; - if (replacing_normal) { - preempted_normal_holders.emplace( - existing->second->generation, - existing->second->owner_id); - } - - std::shared_ptr entry; - if (existing == entries_.end()) { - entry = std::make_shared(); - entry->resource_id = resource_id; - entry->owner_id.swap(safety_owner); - entry->generation = token.generation; - entry->deadline = - std::chrono::steady_clock::time_point::max(); - entry->preemptible = false; - entry->safety_holders.swap(safety_holders); - entry->preempted_normal_holders.swap( - preempted_normal_holders); - entries_.emplace(resource_id, entry); - } else { - entry = existing->second; - entry->owner_id.swap(safety_owner); - entry->generation = token.generation; - entry->deadline = - std::chrono::steady_clock::time_point::max(); - entry->preemptible = false; - entry->quarantined = false; - entry->quarantined_normal_pending = false; - entry->dispatch_fence_only = false; - entry->safety_holders.swap(safety_holders); - entry->preempted_normal_holders.swap( - preempted_normal_holders); - } - return {true, std::move(token), {}}; - } catch (...) { - if (replacing_normal && existing->second->preemptible) { - quarantine_(*existing->second); - } - throw; - } -} - -ControlAcquireResult ControlAuthorityManager::preemptAcquireIfCurrent( - const ControlLeaseToken& expected_token, - const std::string& owner_id, - const Duration ttl) -{ - if (!expected_token.valid() || owner_id.empty() || - ttl <= Duration::zero()) { - return {false, {}, "invalid conditional control barrier request"}; - } - - std::lock_guard lock(mutex_); - const auto existing = entries_.find(expected_token.resource_id); - if (existing == entries_.end() || - expired_(*existing->second) || - !existing->second->preemptible || - existing->second->owner_id != expected_token.owner_id || - existing->second->generation != expected_token.generation) { - return { - false, - {}, - "expected control lease is no longer current"}; - } - - try { - ControlLeaseToken token; - token.resource_id = expected_token.resource_id; - token.owner_id = owner_id; - token.generation = ++next_generation_; - - std::unordered_map safety_holders; - safety_holders.emplace( - token.generation, - SafetyHolder{owner_id, false}); - std::string safety_owner = owner_id; - std::unordered_map - preempted_normal_holders; - preempted_normal_holders.emplace( - existing->second->generation, - existing->second->owner_id); - - auto& entry = *existing->second; - entry.owner_id.swap(safety_owner); - entry.generation = token.generation; - entry.deadline = std::chrono::steady_clock::time_point::max(); - entry.preemptible = false; - entry.quarantined = false; - entry.quarantined_normal_pending = false; - entry.dispatch_fence_only = false; - entry.safety_holders.swap(safety_holders); - entry.preempted_normal_holders.swap( - preempted_normal_holders); - return {true, std::move(token), {}}; - } catch (...) { - if (existing->second->preemptible) { - quarantine_(*existing->second); - } - throw; - } -} - -bool ControlAuthorityManager::waitForPreemptedRelease( - const ControlLeaseToken& safety_token, - const Duration timeout) -{ - if (!safety_token.valid() || timeout < Duration::zero()) { - return false; - } - - std::unique_lock lock(mutex_); - const auto currentState = [this, &safety_token]() { - const auto found = entries_.find(safety_token.resource_id); - if (found == entries_.end() || - !isActiveSafetyHolder_(*found->second, safety_token)) { - return -1; - } - return !found->second->quarantined_normal_pending && - found->second->preempted_normal_holders.empty() && - found->second->in_flight_dispatches == 0U - ? 1 - : 0; - }; - - if (currentState() < 0) { - return false; - } - release_cv_.wait_for( - lock, - timeout, - [¤tState]() { return currentState() != 0; }); - return currentState() == 1; -} - -bool ControlAuthorityManager::quarantineIfCurrent( - const ControlLeaseToken& expected_token) noexcept -{ - if (!expected_token.valid()) { - return false; - } - try { - std::lock_guard lock(mutex_); - const auto existing = entries_.find(expected_token.resource_id); - if (existing == entries_.end() || - expired_(*existing->second) || - !existing->second->preemptible || - existing->second->owner_id != expected_token.owner_id || - existing->second->generation != expected_token.generation) { - return false; - } - quarantine_(*existing->second); - return true; - } catch (...) { - return false; - } -} - -bool ControlAuthorityManager::retireSafetyHolder( - const ControlLeaseToken& safety_token) noexcept -{ - if (!safety_token.valid()) { - return false; - } - try { - std::lock_guard lock(mutex_); - const auto existing = entries_.find(safety_token.resource_id); - if (existing == entries_.end() || - existing->second->preemptible || - existing->second->dispatch_fence_only) { - return false; - } - const auto holder = existing->second->safety_holders.find( - safety_token.generation); - if (holder == existing->second->safety_holders.end() || - holder->second.owner_id != safety_token.owner_id) { - return false; - } - holder->second.retired = true; - release_cv_.notify_all(); - return true; - } catch (...) { - return false; - } -} - -bool ControlAuthorityManager::recoverRetiredSafetyHolders( - const ControlLeaseToken& recovery_token) noexcept -{ - if (!recovery_token.valid()) { - return false; - } - try { - std::lock_guard lock(mutex_); - const auto existing = entries_.find(recovery_token.resource_id); - if (existing == entries_.end() || - !isActiveSafetyHolder_(*existing->second, recovery_token) || - existing->second->quarantined_normal_pending || - !existing->second->preempted_normal_holders.empty() || - existing->second->in_flight_dispatches != 0U) { - return false; - } - - for (auto holder = existing->second->safety_holders.begin(); - holder != existing->second->safety_holders.end();) { - if (holder->second.retired) { - holder = existing->second->safety_holders.erase(holder); - } else { - ++holder; - } - } - existing->second->quarantined = false; - release_cv_.notify_all(); - return true; - } catch (...) { - return false; - } -} - -bool ControlAuthorityManager::renew( - const ControlLeaseToken& token, - const Duration ttl) -{ - if (!token.valid() || ttl <= Duration::zero()) { - return false; - } - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.resource_id); - if (found == entries_.end()) { - return false; - } - if (expired_(*found->second)) { - if (found->second->in_flight_dispatches != 0U) { - invalidateToDispatchFence_(*found->second); - } else { - entries_.erase(found); - } - return false; - } - if (!found->second->preemptible) { - const auto holder = found->second->safety_holders.find( - token.generation); - return holder != found->second->safety_holders.end() && - holder->second.owner_id == token.owner_id && - !holder->second.retired; - } - if (found->second->owner_id != token.owner_id || - found->second->generation != token.generation) { - return false; - } - found->second->deadline = std::chrono::steady_clock::now() + ttl; - return true; -} - -bool ControlAuthorityManager::validate( - const ControlLeaseToken& token) -{ - if (!token.valid()) { - return false; - } - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.resource_id); - if (found == entries_.end()) { - return false; - } - if (expired_(*found->second)) { - if (found->second->in_flight_dispatches != 0U) { - invalidateToDispatchFence_(*found->second); - } else { - entries_.erase(found); - } - return false; - } - if (!found->second->preemptible) { - const auto holder = found->second->safety_holders.find( - token.generation); - return holder != found->second->safety_holders.end() && - holder->second.owner_id == token.owner_id && - !holder->second.retired; - } - return found->second->owner_id == token.owner_id && - found->second->generation == token.generation; -} - -ControlDispatchGuard ControlAuthorityManager::tryBeginDispatch( - const ControlLeaseToken& token) -{ - if (!token.valid()) { - return {}; - } - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.resource_id); - if (found == entries_.end()) { - return {}; - } - if (expired_(*found->second)) { - if (found->second->in_flight_dispatches != 0U) { - invalidateToDispatchFence_(*found->second); - } else { - entries_.erase(found); - } - return {}; - } - if (!found->second->preemptible || - found->second->owner_id != token.owner_id || - found->second->generation != token.generation) { - return {}; - } - ++found->second->in_flight_dispatches; - return ControlDispatchGuard(this, found->second); -} - -void ControlAuthorityManager::release( - const ControlLeaseToken& token) noexcept -{ - if (!token.valid()) { - return; - } - try { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.resource_id); - if (found == entries_.end()) { - return; - } - auto& entry = *found->second; - if (!entry.preemptible) { - if (entry.dispatch_fence_only) { - return; - } - const auto holder = entry.safety_holders.find(token.generation); - if (holder != entry.safety_holders.end() && - holder->second.owner_id == token.owner_id) { - if (holder->second.retired) { - return; - } - entry.safety_holders.erase(holder); - if (canErase_(entry)) { - entries_.erase(found); - } - release_cv_.notify_all(); - return; - } - - const auto preempted = entry.preempted_normal_holders.find( - token.generation); - if (preempted != entry.preempted_normal_holders.end() && - preempted->second == token.owner_id) { - entry.preempted_normal_holders.erase(preempted); - if (canErase_(entry)) { - entries_.erase(found); - } - release_cv_.notify_all(); - return; - } - - if (entry.quarantined_normal_pending && - entry.owner_id == token.owner_id && - entry.generation == token.generation) { - entry.quarantined_normal_pending = false; - if (canErase_(entry)) { - entries_.erase(found); - } - release_cv_.notify_all(); - } - } else if (entry.owner_id == token.owner_id && - entry.generation == token.generation) { - if (entry.in_flight_dispatches != 0U) { - invalidateToDispatchFence_(entry); - } else { - entries_.erase(found); - } - release_cv_.notify_all(); - } - } catch (...) { - } -} - -bool ControlAuthorityManager::isLeased( - const std::string& resource_id) -{ - if (resource_id.empty()) { - return false; - } - std::lock_guard lock(mutex_); - const auto found = entries_.find(resource_id); - if (found == entries_.end()) { - return false; - } - if (expired_(*found->second)) { - if (found->second->in_flight_dispatches != 0U) { - invalidateToDispatchFence_(*found->second); - return true; - } - entries_.erase(found); - return false; - } - return true; -} - -void ControlAuthorityManager::revoke( - const std::string& resource_id) noexcept -{ - try { - std::lock_guard lock(mutex_); - const auto found = entries_.find(resource_id); - if (found == entries_.end()) { - return; - } - if (found->second->in_flight_dispatches != 0U) { - invalidateToDispatchFence_(*found->second); - } else { - entries_.erase(found); - } - release_cv_.notify_all(); - } catch (...) { - } -} - -void ControlAuthorityManager::clear() noexcept -{ - try { - std::lock_guard lock(mutex_); - for (auto entry = entries_.begin(); entry != entries_.end();) { - if (entry->second->in_flight_dispatches != 0U) { - invalidateToDispatchFence_(*entry->second); - ++entry; - } else { - entry = entries_.erase(entry); - } - } - release_cv_.notify_all(); - } catch (...) { - } -} - -bool ControlAuthorityManager::expired_(const Entry& entry) const noexcept -{ - return !entry.quarantined && - std::chrono::steady_clock::now() >= entry.deadline; -} - -bool ControlAuthorityManager::isSafetyHolder_( - const Entry& entry, - const ControlLeaseToken& token) noexcept -{ - if (entry.preemptible || entry.dispatch_fence_only) { - return false; - } - const auto holder = entry.safety_holders.find(token.generation); - return holder != entry.safety_holders.end() && - holder->second.owner_id == token.owner_id; -} - -bool ControlAuthorityManager::isActiveSafetyHolder_( - const Entry& entry, - const ControlLeaseToken& token) noexcept -{ - if (entry.preemptible || entry.dispatch_fence_only) { - return false; - } - const auto holder = entry.safety_holders.find(token.generation); - return holder != entry.safety_holders.end() && - holder->second.owner_id == token.owner_id && - !holder->second.retired; -} - -bool ControlAuthorityManager::canErase_(const Entry& entry) noexcept -{ - if (entry.in_flight_dispatches != 0U) { - return false; - } - if (entry.dispatch_fence_only) { - return true; - } - return !entry.preemptible && - entry.safety_holders.empty() && - entry.preempted_normal_holders.empty() && - !entry.quarantined_normal_pending && - !entry.quarantined; -} - -void ControlAuthorityManager::invalidateToDispatchFence_( - Entry& entry) noexcept -{ - entry.owner_id.clear(); - entry.generation = 0U; - entry.deadline = std::chrono::steady_clock::time_point::max(); - entry.preemptible = false; - entry.quarantined = false; - entry.quarantined_normal_pending = false; - entry.dispatch_fence_only = true; - entry.safety_holders.clear(); - entry.preempted_normal_holders.clear(); -} - -void ControlAuthorityManager::endDispatch_( - const std::shared_ptr& entry) noexcept -{ - try { - std::lock_guard lock(mutex_); - if (entry->in_flight_dispatches == 0U) { - return; - } - --entry->in_flight_dispatches; - const auto found = entries_.find(entry->resource_id); - if (found != entries_.end() && found->second == entry && - canErase_(*entry)) { - entries_.erase(found); - } - release_cv_.notify_all(); - } catch (...) { - } -} - -void ControlAuthorityManager::quarantine_(Entry& entry) noexcept -{ - entry.quarantined_normal_pending = entry.preemptible; - entry.deadline = std::chrono::steady_clock::time_point::max(); - entry.preemptible = false; - entry.quarantined = true; - entry.dispatch_fence_only = false; -} - -} // namespace cmvr::control diff --git a/cmvr-es/manager/control_authority_manager/tests/control_authority_manager_test.cpp b/cmvr-es/manager/control_authority_manager/tests/control_authority_manager_test.cpp deleted file mode 100644 index f0703b7b..00000000 --- a/cmvr-es/manager/control_authority_manager/tests/control_authority_manager_test.cpp +++ /dev/null @@ -1,923 +0,0 @@ -#include "manager/control_authority_manager/include/control_authority_manager.h" - -#include -#include -#include - -#include - -namespace cmvr::control { -namespace { - -using namespace std::chrono_literals; - -class ControlAuthorityManagerTest : public ::testing::Test { -protected: - void SetUp() override - { - ControlAuthorityManager::instance().clear(); - } - - void TearDown() override - { - ControlAuthorityManager::instance().clear(); - } -}; - -TEST_F(ControlAuthorityManagerTest, LeaseIsExclusiveAndExactReleaseRestoresAccess) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto first = - manager.tryAcquire("right_arm", "session-a", 100ms); - ASSERT_TRUE(first.acquired); - EXPECT_TRUE(manager.validate(first.token)); - EXPECT_TRUE(manager.isLeased("right_arm")); - - const auto conflict = - manager.tryAcquire("right_arm", "session-b", 100ms); - EXPECT_FALSE(conflict.acquired); - - manager.release(first.token); - EXPECT_FALSE(manager.isLeased("right_arm")); - EXPECT_TRUE( - manager.tryAcquire("right_arm", "session-b", 100ms) - .acquired); -} - -TEST_F(ControlAuthorityManagerTest, StaleGenerationCannotReleaseNewLease) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto old = - manager.tryAcquire("right_arm", "session-a", 100ms); - ASSERT_TRUE(old.acquired); - manager.release(old.token); - const auto current = - manager.tryAcquire("right_arm", "session-a", 100ms); - ASSERT_TRUE(current.acquired); - ASSERT_NE( - old.token.generation, - current.token.generation); - - manager.release(old.token); - EXPECT_TRUE(manager.validate(current.token)); -} - -TEST_F(ControlAuthorityManagerTest, - SafetyBarrierAtomicallyPreemptsControlAndRejectsOtherOwners) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 100ms); - ASSERT_TRUE(control.acquired); - - const auto barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 100ms); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - EXPECT_FALSE(manager.validate(control.token)); - EXPECT_TRUE(manager.validate(barrier.token)); - - const auto move_during_stop = - manager.tryAcquire("right_arm", "new-move", 100ms); - EXPECT_FALSE(move_during_stop.acquired); - const auto second_stop = manager.preemptAcquire( - "right_arm", "second-stop", 100ms); - ASSERT_TRUE(second_stop.acquired) << second_stop.detail; - - manager.release(control.token); - EXPECT_TRUE(manager.validate(barrier.token)); - manager.release(barrier.token); - EXPECT_TRUE(manager.validate(second_stop.token)); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "new-move", 100ms) - .acquired); - manager.release(second_stop.token); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, - SafetyPreemptionReturnsWhileDispatchIsInFlightAndWaitsForBoth) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - - auto dispatch = manager.tryBeginDispatch(control.token); - ASSERT_TRUE(dispatch.acquired()); - auto pending_barrier = std::async( - std::launch::async, - [&manager]() { - return manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - }); - - const auto preempt_status = pending_barrier.wait_for(100ms); - if (preempt_status != std::future_status::ready) { - dispatch = {}; - const auto cleanup_barrier = pending_barrier.get(); - if (cleanup_barrier.acquired) { - manager.release(cleanup_barrier.token); - } - FAIL() << "safety preemption waited for an in-flight dispatch"; - return; - } - - const auto acquired_barrier = pending_barrier.get(); - ASSERT_TRUE(acquired_barrier.acquired) - << acquired_barrier.detail; - EXPECT_FALSE(manager.validate(control.token)); - EXPECT_FALSE(manager.tryBeginDispatch(control.token).acquired()); - - EXPECT_FALSE(manager.waitForPreemptedRelease( - acquired_barrier.token, 10ms)); - manager.release(control.token); - EXPECT_FALSE(manager.waitForPreemptedRelease( - acquired_barrier.token, 10ms)); - - dispatch = {}; - EXPECT_TRUE(manager.waitForPreemptedRelease( - acquired_barrier.token, 10ms)); - manager.release(acquired_barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - DispatchOnOneResourceDoesNotDelaySafetyPreemptionOnAnother) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto right_control = - manager.tryAcquire("right_arm", "right-move", 1s); - const auto left_control = - manager.tryAcquire("left_arm", "left-move", 1s); - ASSERT_TRUE(right_control.acquired); - ASSERT_TRUE(left_control.acquired); - - auto right_dispatch = manager.tryBeginDispatch(right_control.token); - ASSERT_TRUE(right_dispatch.acquired()); - auto pending_left_barrier = std::async( - std::launch::async, - [&manager]() { - return manager.preemptAcquire( - "left_arm", "left-stop", 1s); - }); - - const auto preempt_status = pending_left_barrier.wait_for(100ms); - if (preempt_status != std::future_status::ready) { - right_dispatch = {}; - const auto cleanup_barrier = pending_left_barrier.get(); - if (cleanup_barrier.acquired) { - manager.release(cleanup_barrier.token); - } - FAIL() << "one resource's dispatch blocked another resource's stop"; - return; - } - - const auto left_barrier = pending_left_barrier.get(); - ASSERT_TRUE(left_barrier.acquired) << left_barrier.detail; - manager.release(left_control.token); - EXPECT_TRUE(manager.waitForPreemptedRelease( - left_barrier.token, 0ms)); - manager.release(left_barrier.token); - - EXPECT_TRUE(right_dispatch.acquired()); - right_dispatch = {}; - manager.release(right_control.token); -} - -TEST_F(ControlAuthorityManagerTest, - DispatchGuardDestructorReleasesTheInFlightFence) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - - ControlAcquireResult barrier; - { - auto dispatch = manager.tryBeginDispatch(control.token); - ASSERT_TRUE(dispatch.acquired()); - barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - manager.release(control.token); - EXPECT_FALSE(manager.waitForPreemptedRelease( - barrier.token, 0ms)); - } - - EXPECT_TRUE(manager.waitForPreemptedRelease(barrier.token, 0ms)); - manager.release(barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - DispatchGuardMoveAssignmentReleasesOnlyItsPreviousFence) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto right_control = - manager.tryAcquire("right_arm", "right-move", 1s); - const auto left_control = - manager.tryAcquire("left_arm", "left-move", 1s); - ASSERT_TRUE(right_control.acquired); - ASSERT_TRUE(left_control.acquired); - - auto right_dispatch = manager.tryBeginDispatch(right_control.token); - auto left_dispatch = manager.tryBeginDispatch(left_control.token); - ASSERT_TRUE(right_dispatch.acquired()); - ASSERT_TRUE(left_dispatch.acquired()); - const auto right_barrier = manager.preemptAcquire( - "right_arm", "right-stop", 1s); - const auto left_barrier = manager.preemptAcquire( - "left_arm", "left-stop", 1s); - ASSERT_TRUE(right_barrier.acquired) << right_barrier.detail; - ASSERT_TRUE(left_barrier.acquired) << left_barrier.detail; - manager.release(right_control.token); - manager.release(left_control.token); - - right_dispatch = std::move(left_dispatch); - EXPECT_TRUE(right_dispatch.acquired()); - EXPECT_FALSE(left_dispatch.acquired()); - EXPECT_TRUE(manager.waitForPreemptedRelease( - right_barrier.token, 0ms)); - EXPECT_FALSE(manager.waitForPreemptedRelease( - left_barrier.token, 0ms)); - - right_dispatch = {}; - EXPECT_TRUE(manager.waitForPreemptedRelease( - left_barrier.token, 0ms)); - manager.release(right_barrier.token); - manager.release(left_barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - RevokeKeepsAnInFlightDispatchFencedFromNormalSuccessors) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - auto dispatch = manager.tryBeginDispatch(control.token); - ASSERT_TRUE(dispatch.acquired()); - - manager.revoke("right_arm"); - EXPECT_FALSE(manager.validate(control.token)); - EXPECT_TRUE(manager.isLeased("right_arm")); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "successor", 1s).acquired); - manager.release(control.token); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "successor", 1s).acquired); - - dispatch = {}; - const auto successor = - manager.tryAcquire("right_arm", "successor", 1s); - ASSERT_TRUE(successor.acquired) << successor.detail; - manager.release(successor.token); -} - -TEST_F(ControlAuthorityManagerTest, - ClearKeepsInFlightDispatchesWhileErasingIdleEntries) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto active = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(active.acquired); - ASSERT_TRUE(manager.tryAcquire("idle_arm", "idle-session", 1s).acquired); - auto dispatch = manager.tryBeginDispatch(active.token); - ASSERT_TRUE(dispatch.acquired()); - - manager.clear(); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "successor", 1s).acquired); - const auto idle_successor = - manager.tryAcquire("idle_arm", "idle-successor", 1s); - ASSERT_TRUE(idle_successor.acquired) << idle_successor.detail; - manager.release(idle_successor.token); - - dispatch = {}; - const auto active_successor = - manager.tryAcquire("right_arm", "successor", 1s); - ASSERT_TRUE(active_successor.acquired) << active_successor.detail; - manager.release(active_successor.token); -} - -TEST_F(ControlAuthorityManagerTest, - ExpiredLeaseKeepsInFlightDispatchFencedFromNormalSuccessors) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 10ms); - ASSERT_TRUE(control.acquired); - auto dispatch = manager.tryBeginDispatch(control.token); - ASSERT_TRUE(dispatch.acquired()); - std::this_thread::sleep_for(20ms); - - EXPECT_FALSE(manager.validate(control.token)); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "successor", 1s).acquired); - manager.release(control.token); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "successor", 1s).acquired); - - dispatch = {}; - const auto successor = - manager.tryAcquire("right_arm", "successor", 1s); - ASSERT_TRUE(successor.acquired) << successor.detail; - manager.release(successor.token); -} - -TEST_F(ControlAuthorityManagerTest, - DispatchGuardSurvivesAuthorityMapRehash) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - auto dispatch = manager.tryBeginDispatch(control.token); - ASSERT_TRUE(dispatch.acquired()); - - for (int index = 0; index < 512; ++index) { - ASSERT_TRUE(manager.tryAcquire( - "rehash-resource-" + std::to_string(index), - "rehash-owner", - 1s).acquired); - } - - const auto barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - manager.release(control.token); - EXPECT_FALSE(manager.waitForPreemptedRelease(barrier.token, 0ms)); - dispatch = {}; - EXPECT_TRUE(manager.waitForPreemptedRelease(barrier.token, 0ms)); - manager.release(barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - StaleLeaseCannotBeginDispatchAfterSafetyPreemption) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - const auto barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - - EXPECT_FALSE(manager.tryBeginDispatch(control.token).acquired()); - manager.release(barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - ReleasedLastSafetyBarrierKeepsPreemptedHandlerFenced) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - const auto barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - - manager.release(barrier.token); - EXPECT_TRUE(manager.isLeased("right_arm")); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "new-move", 1s).acquired); - - manager.release(control.token); - EXPECT_FALSE(manager.isLeased("right_arm")); - EXPECT_TRUE( - manager.tryAcquire("right_arm", "new-move", 1s).acquired); -} - -TEST_F(ControlAuthorityManagerTest, - SafetyBarrierWaitsForPreemptedNormalLeaseRelease) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - const auto barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - - auto wait_result = std::async( - std::launch::async, - [&manager, token = barrier.token]() { - return manager.waitForPreemptedRelease(token, 1s); - }); - EXPECT_EQ( - wait_result.wait_for(30ms), - std::future_status::timeout); - - manager.release(control.token); - EXPECT_TRUE(wait_result.get()); - EXPECT_TRUE(manager.validate(barrier.token)); - manager.release(barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - ConditionalSafetyBarrierTracksPreemptedNormalLeaseRelease) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - auto dispatch = manager.tryBeginDispatch(control.token); - ASSERT_TRUE(dispatch.acquired()); - const auto barrier = manager.preemptAcquireIfCurrent( - control.token, "timed-out-action", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - - EXPECT_FALSE( - manager.waitForPreemptedRelease(barrier.token, 10ms)); - dispatch = {}; - EXPECT_FALSE( - manager.waitForPreemptedRelease(barrier.token, 10ms)); - manager.release(control.token); - EXPECT_TRUE( - manager.waitForPreemptedRelease(barrier.token, 10ms)); - manager.release(barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - ReleasedSafetyHolderStopsWaitingWithoutAffectingOtherHolder) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - const auto first_barrier = manager.preemptAcquire( - "right_arm", "first-stop", 1s); - ASSERT_TRUE(first_barrier.acquired) << first_barrier.detail; - const auto second_barrier = manager.preemptAcquire( - "right_arm", "second-stop", 1s); - ASSERT_TRUE(second_barrier.acquired) << second_barrier.detail; - - auto first_wait = std::async( - std::launch::async, - [&manager, token = first_barrier.token]() { - return manager.waitForPreemptedRelease(token, 1s); - }); - auto second_wait = std::async( - std::launch::async, - [&manager, token = second_barrier.token]() { - return manager.waitForPreemptedRelease(token, 1s); - }); - EXPECT_EQ(first_wait.wait_for(30ms), std::future_status::timeout); - EXPECT_EQ(second_wait.wait_for(30ms), std::future_status::timeout); - - manager.release(first_barrier.token); - EXPECT_FALSE(first_wait.get()); - EXPECT_EQ(second_wait.wait_for(30ms), std::future_status::timeout); - manager.release(control.token); - EXPECT_TRUE(second_wait.get()); - manager.release(second_barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - PreemptedReleaseRequiresExactNormalLeaseToken) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - const auto barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - - auto forged = control.token; - forged.owner_id = "different-owner"; - manager.release(forged); - EXPECT_FALSE( - manager.waitForPreemptedRelease(barrier.token, 10ms)); - - manager.release(control.token); - EXPECT_TRUE( - manager.waitForPreemptedRelease(barrier.token, 0ms)); - manager.release(barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - ClearWakesPreemptedReleaseWaiterAndInvalidatesSafetyHolder) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - const auto barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - - auto wait_result = std::async( - std::launch::async, - [&manager, token = barrier.token]() { - return manager.waitForPreemptedRelease(token, 1s); - }); - EXPECT_EQ( - wait_result.wait_for(30ms), - std::future_status::timeout); - - manager.clear(); - EXPECT_FALSE(wait_result.get()); - manager.release(control.token); -} - -TEST_F(ControlAuthorityManagerTest, - RevokeWakesPreemptedReleaseWaiterAndInvalidatesSafetyHolder) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - const auto barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - - auto wait_result = std::async( - std::launch::async, - [&manager, token = barrier.token]() { - return manager.waitForPreemptedRelease(token, 1s); - }); - EXPECT_EQ( - wait_result.wait_for(30ms), - std::future_status::timeout); - - manager.revoke("right_arm"); - EXPECT_FALSE(wait_result.get()); - manager.release(control.token); -} - -TEST_F(ControlAuthorityManagerTest, - ExpiredNormalLeaseStillRequiresHandlerReleaseAfterPreemption) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 10ms); - ASSERT_TRUE(control.acquired); - std::this_thread::sleep_for(20ms); - - const auto barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - EXPECT_FALSE( - manager.waitForPreemptedRelease(barrier.token, 10ms)); - - manager.release(control.token); - EXPECT_TRUE( - manager.waitForPreemptedRelease(barrier.token, 0ms)); - manager.release(barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - QuarantinedFallbackTracksOriginalNormalHandlerRelease) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - ASSERT_TRUE(manager.quarantineIfCurrent(control.token)); - - const auto barrier = manager.preemptAcquire( - "right_arm", "stop-operation", 1s); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - EXPECT_FALSE( - manager.waitForPreemptedRelease(barrier.token, 10ms)); - - manager.release(control.token); - EXPECT_TRUE( - manager.waitForPreemptedRelease(barrier.token, 0ms)); - manager.release(barrier.token); - EXPECT_TRUE(manager.isLeased("right_arm")); - - manager.revoke("right_arm"); -} - -TEST_F(ControlAuthorityManagerTest, - WaitRejectsStaleSafetyTokenForSuccessorEntry) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto old_control = - manager.tryAcquire("right_arm", "old-move", 1s); - ASSERT_TRUE(old_control.acquired); - const auto old_barrier = manager.preemptAcquire( - "right_arm", "old-stop", 1s); - ASSERT_TRUE(old_barrier.acquired) << old_barrier.detail; - manager.release(old_barrier.token); - - EXPECT_FALSE( - manager.tryAcquire("right_arm", "new-move", 1s).acquired); - manager.release(old_control.token); - - const auto successor = - manager.tryAcquire("right_arm", "new-move", 1s); - ASSERT_TRUE(successor.acquired); - const auto current_barrier = manager.preemptAcquire( - "right_arm", "new-stop", 1s); - ASSERT_TRUE(current_barrier.acquired) << current_barrier.detail; - - EXPECT_FALSE(manager.waitForPreemptedRelease( - old_barrier.token, 0ms)); - EXPECT_FALSE(manager.waitForPreemptedRelease( - current_barrier.token, 10ms)); - - manager.release(successor.token); - EXPECT_TRUE(manager.waitForPreemptedRelease( - current_barrier.token, 0ms)); - manager.release(current_barrier.token); -} - -TEST_F(ControlAuthorityManagerTest, - ConditionalSafetyBarrierPreemptsMatchingCurrentLease) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 100ms); - ASSERT_TRUE(control.acquired); - - const auto barrier = manager.preemptAcquireIfCurrent( - control.token, "timed-out-action", 100ms); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - EXPECT_FALSE(manager.validate(control.token)); - EXPECT_TRUE(manager.validate(barrier.token)); - manager.release(control.token); - EXPECT_TRUE(manager.validate(barrier.token)); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "new-move", 100ms) - .acquired); - - manager.release(barrier.token); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, - ConditionalSafetyBarrierDoesNotPreemptSuccessorForStaleToken) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto old = - manager.tryAcquire("right_arm", "move-session", 100ms); - ASSERT_TRUE(old.acquired); - const auto direct_stop = manager.preemptAcquire( - "right_arm", "direct-stop", 100ms); - ASSERT_TRUE(direct_stop.acquired) << direct_stop.detail; - manager.release(direct_stop.token); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "move-session", 100ms).acquired); - manager.release(old.token); - const auto successor = - manager.tryAcquire("right_arm", "move-session", 100ms); - ASSERT_TRUE(successor.acquired); - ASSERT_NE(old.token.generation, successor.token.generation); - - const auto barrier = manager.preemptAcquireIfCurrent( - old.token, "delayed-stop", 100ms); - EXPECT_FALSE(barrier.acquired); - EXPECT_FALSE(barrier.token.valid()); - EXPECT_TRUE(manager.validate(successor.token)); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "competing-move", 100ms) - .acquired); - - manager.release(successor.token); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, - ConditionalSafetyBarrierDoesNotJoinExistingSafetyBarrier) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 100ms); - ASSERT_TRUE(control.acquired); - const auto existing_barrier = manager.preemptAcquire( - "right_arm", "direct-stop", 100ms); - ASSERT_TRUE(existing_barrier.acquired) << existing_barrier.detail; - - const auto delayed_barrier = manager.preemptAcquireIfCurrent( - control.token, "delayed-action-stop", 100ms); - EXPECT_FALSE(delayed_barrier.acquired); - EXPECT_FALSE(delayed_barrier.token.valid()); - EXPECT_TRUE(manager.validate(existing_barrier.token)); - - manager.release(existing_barrier.token); - EXPECT_TRUE(manager.isLeased("right_arm")); - manager.release(control.token); - EXPECT_FALSE(manager.isLeased("right_arm")); - EXPECT_TRUE( - manager.tryAcquire("right_arm", "new-move", 100ms) - .acquired); -} - -TEST_F(ControlAuthorityManagerTest, - QuarantineSurvivesNormalAndTemporarySafetyTokenRelease) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 20ms); - ASSERT_TRUE(control.acquired); - - ASSERT_TRUE(manager.quarantineIfCurrent(control.token)); - EXPECT_FALSE(manager.validate(control.token)); - EXPECT_TRUE(manager.isLeased("right_arm")); - - manager.release(control.token); - std::this_thread::sleep_for(30ms); - EXPECT_TRUE(manager.isLeased("right_arm")); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "new-move", 100ms) - .acquired); - - const auto temporary_stop = manager.preemptAcquire( - "right_arm", "temporary-stop", 100ms); - ASSERT_TRUE(temporary_stop.acquired) << temporary_stop.detail; - EXPECT_TRUE(manager.validate(temporary_stop.token)); - manager.release(temporary_stop.token); - - EXPECT_TRUE(manager.isLeased("right_arm")); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "new-move", 100ms) - .acquired); - - manager.revoke("right_arm"); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, - QuarantineWithStaleTokenDoesNotAffectSuccessor) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto old = - manager.tryAcquire("right_arm", "move-session", 100ms); - ASSERT_TRUE(old.acquired); - manager.release(old.token); - const auto successor = - manager.tryAcquire("right_arm", "move-session", 100ms); - ASSERT_TRUE(successor.acquired); - ASSERT_NE(old.token.generation, successor.token.generation); - - EXPECT_FALSE(manager.quarantineIfCurrent(old.token)); - EXPECT_TRUE(manager.validate(successor.token)); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "competing-move", 100ms) - .acquired); - - manager.release(successor.token); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, - RetiredSafetyHolderStaysFailClosedUntilConfirmedRecovery) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - const auto failed_stop = manager.preemptAcquire( - "right_arm", "failed-stop", 1s); - ASSERT_TRUE(failed_stop.acquired) << failed_stop.detail; - - manager.release(control.token); - ASSERT_TRUE(manager.waitForPreemptedRelease( - failed_stop.token, 0ms)); - ASSERT_TRUE(manager.retireSafetyHolder(failed_stop.token)); - EXPECT_FALSE(manager.validate(failed_stop.token)); - - // A delayed destructor release cannot undo the retained fail-closed state. - manager.release(failed_stop.token); - EXPECT_TRUE(manager.isLeased("right_arm")); - EXPECT_FALSE( - manager.tryAcquire("right_arm", "new-move", 1s).acquired); - - const auto recovery = manager.preemptAcquire( - "right_arm", "confirmed-recovery", 1s); - ASSERT_TRUE(recovery.acquired) << recovery.detail; - ASSERT_TRUE(manager.waitForPreemptedRelease(recovery.token, 0ms)); - ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token)); - EXPECT_TRUE(manager.validate(recovery.token)); - - manager.release(recovery.token); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, - RecoveryWaitsForPreemptedNormalHandlerToRelease) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - const auto failed_stop = manager.preemptAcquire( - "right_arm", "failed-stop", 1s); - ASSERT_TRUE(failed_stop.acquired) << failed_stop.detail; - ASSERT_TRUE(manager.retireSafetyHolder(failed_stop.token)); - - const auto recovery = manager.preemptAcquire( - "right_arm", "confirmed-recovery", 1s); - ASSERT_TRUE(recovery.acquired) << recovery.detail; - EXPECT_FALSE(manager.recoverRetiredSafetyHolders(recovery.token)); - EXPECT_FALSE(manager.waitForPreemptedRelease(recovery.token, 10ms)); - - manager.release(control.token); - ASSERT_TRUE(manager.waitForPreemptedRelease(recovery.token, 0ms)); - ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token)); - manager.release(recovery.token); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, - RecoveryPreservesEveryOtherActiveSafetyHolder) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto retired = manager.preemptAcquire( - "right_arm", "failed-stop", 1s); - ASSERT_TRUE(retired.acquired) << retired.detail; - ASSERT_TRUE(manager.retireSafetyHolder(retired.token)); - - const auto independent_stop = manager.preemptAcquire( - "right_arm", "independent-stop", 1s); - const auto recovery = manager.preemptAcquire( - "right_arm", "confirmed-recovery", 1s); - ASSERT_TRUE(independent_stop.acquired) << independent_stop.detail; - ASSERT_TRUE(recovery.acquired) << recovery.detail; - - ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token)); - EXPECT_TRUE(manager.validate(independent_stop.token)); - EXPECT_TRUE(manager.validate(recovery.token)); - EXPECT_FALSE(manager.validate(retired.token)); - - manager.release(recovery.token); - EXPECT_TRUE(manager.isLeased("right_arm")); - EXPECT_TRUE(manager.validate(independent_stop.token)); - manager.release(independent_stop.token); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, - RetiredOrForgedSafetyTokenCannotAuthorizeRecovery) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto retired = manager.preemptAcquire( - "right_arm", "failed-stop", 1s); - ASSERT_TRUE(retired.acquired) << retired.detail; - ASSERT_TRUE(manager.retireSafetyHolder(retired.token)); - EXPECT_FALSE(manager.recoverRetiredSafetyHolders(retired.token)); - - const auto recovery = manager.preemptAcquire( - "right_arm", "confirmed-recovery", 1s); - ASSERT_TRUE(recovery.acquired) << recovery.detail; - auto forged = recovery.token; - forged.owner_id = "different-owner"; - EXPECT_FALSE(manager.recoverRetiredSafetyHolders(forged)); - EXPECT_TRUE(manager.isLeased("right_arm")); - - ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token)); - manager.release(recovery.token); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, - ConfirmedRecoveryCanClearTokenlessQuarantine) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto control = - manager.tryAcquire("right_arm", "move-session", 1s); - ASSERT_TRUE(control.acquired); - ASSERT_TRUE(manager.quarantineIfCurrent(control.token)); - - const auto recovery = manager.preemptAcquire( - "right_arm", "confirmed-recovery", 1s); - ASSERT_TRUE(recovery.acquired) << recovery.detail; - EXPECT_FALSE(manager.recoverRetiredSafetyHolders(recovery.token)); - - manager.release(control.token); - ASSERT_TRUE(manager.waitForPreemptedRelease(recovery.token, 0ms)); - ASSERT_TRUE(manager.recoverRetiredSafetyHolders(recovery.token)); - manager.release(recovery.token); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime) -{ - auto& manager = ControlAuthorityManager::instance(); - const auto lease = - manager.tryAcquire("right_arm", "session-a", 20ms); - ASSERT_TRUE(lease.acquired); - std::this_thread::sleep_for(10ms); - ASSERT_TRUE(manager.renew(lease.token, 30ms)); - std::this_thread::sleep_for(20ms); - EXPECT_TRUE(manager.validate(lease.token)); - std::this_thread::sleep_for(20ms); - EXPECT_FALSE(manager.validate(lease.token)); - EXPECT_FALSE(manager.isLeased("right_arm")); -} - -TEST_F(ControlAuthorityManagerTest, DifferentArmsCanBeLeasedIndependently) -{ - auto& manager = ControlAuthorityManager::instance(); - EXPECT_TRUE( - manager.tryAcquire("right_arm", "session-a", 100ms) - .acquired); - EXPECT_TRUE( - manager.tryAcquire("left_arm", "session-b", 100ms) - .acquired); -} - -} // namespace -} // namespace cmvr::control diff --git a/cmvr-es/manager/device_manager/CMakeLists.txt b/cmvr-es/manager/device_manager/CMakeLists.txt index 5183fced..a9ed557a 100644 --- a/cmvr-es/manager/device_manager/CMakeLists.txt +++ b/cmvr-es/manager/device_manager/CMakeLists.txt @@ -1,6 +1,5 @@ add_library(device_manager STATIC src/device_factory.cpp - src/device_safety_adapters.cpp src/device_manager.cpp ) @@ -8,7 +7,6 @@ target_include_directories(device_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(device_manager PRIVATE cmvr_es::proto - cmvr_es::safety_manager cmvr_es::device::camera cmvr_es::device::agv cmvr_es::device::speaker @@ -44,39 +42,9 @@ if(BUILD_TESTING) "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime") list(JOIN _device_manager_test_library_dirs ":" _device_manager_test_library_path) - set(_device_manager_snapshot_test_environment - "LD_LIBRARY_PATH=${_device_manager_test_library_path}") - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _device_manager_snapshot_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() set_tests_properties(device_manager_snapshot_test PROPERTIES ENVIRONMENT - "${_device_manager_snapshot_test_environment}" + "LD_LIBRARY_PATH=${_device_manager_test_library_path}" ) endif() - - add_executable(device_manager_lifecycle_test - tests/device_manager_lifecycle_test.cpp - ) - target_link_libraries(device_manager_lifecycle_test PRIVATE - cmvr_es::device_manager - gtest - gtest_main - pthread - ) - add_test( - NAME device_manager_lifecycle_test - COMMAND device_manager_lifecycle_test - ) - set(_device_manager_lifecycle_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _device_manager_lifecycle_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(device_manager_lifecycle_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_device_manager_lifecycle_test_environment}" - ) endif() diff --git a/cmvr-es/manager/device_manager/include/device_manager.h b/cmvr-es/manager/device_manager/include/device_manager.h index a974bce0..b6726f20 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -15,16 +15,9 @@ #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; @@ -34,29 +27,16 @@ namespace cmvr::device { static DeviceManager& getInstance(); static void destroyInstance(); - bool start(); - bool restart(); + void start(); + void 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; @@ -74,27 +54,14 @@ 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; - bool pre_scan_robot_arm_dependencies_() const; - bool init_devices_(); + void pre_scan_robot_arm_dependencies_() const; + void init_devices_(); void configure_mujoco_viewer_pip_(); - 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); + void start_devices_(); + void stop_devices_(); }; } // 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 deleted file mode 100644 index 359d7f4e..00000000 --- a/cmvr-es/manager/device_manager/include/device_safety_adapters.h +++ /dev/null @@ -1,18 +0,0 @@ -#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 2ffffbb1..db84586f 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -4,13 +4,9 @@ // #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" @@ -35,113 +31,6 @@ 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) { @@ -233,30 +122,16 @@ std::shared_ptr DeviceManager::instance_ = nullptr; std::mutex DeviceManager::init_mutex_; -DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) - : cfg_(cfg), - dev_factory_(std::make_unique()), - safety_manager_(std::make_unique( - safetyConfigFrom(cfg))) -{ - initialize_device_statuses_(); +DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) { + cfg_ = cfg; + + dev_factory_ = std::make_unique(); logSection("Device Plan"); log_device_plan_(); - const bool dependencies_valid = pre_scan_robot_arm_dependencies_(); + pre_scan_robot_arm_dependencies_(); logSection("Initialize Devices"); - 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"; - } + init_devices_(); + configure_mujoco_viewer_pip_(); } DeviceManager& DeviceManager::getInstance(const config::DeviceManagerConfig& cfg) { @@ -281,159 +156,35 @@ void DeviceManager::destroyInstance() { MotorManager::clearActiveJoints(); } -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) { +void DeviceManager::start(){ + for (auto& [id, record] : devices_) { + if (!record.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); + if (record.device->start()) { 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; } -bool DeviceManager::restart() { +void DeviceManager::restart() { stop(); - return start(); + start(); } void DeviceManager::stop() { - 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) { + for (auto& [id, record] : devices_) { + if (!record.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; } - 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); - } + if (record.device->stop()) { 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}); } } } @@ -441,7 +192,6 @@ void DeviceManager::stop_devices_(const bool update_status) { 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."; @@ -467,27 +217,8 @@ 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); } @@ -513,139 +244,42 @@ 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; - { - 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)); - } + devices_.emplace(record.id, std::move(record)); 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 { - std::vector sources; + struct SnapshotSource { + ManagedDeviceSnapshot status; + std::shared_ptr device; + }; + + std::vector sources; { std::shared_lock lock(devices_mutex_); - sources.reserve(device_statuses_.size()); - for (const auto& [id, stored_status] : device_statuses_) { - (void)id; - sources.push_back(stored_status); + 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)); } } @@ -656,22 +290,23 @@ DeviceManagerSnapshot DeviceManager::snapshot() const result.devices.reserve(sources.size()); for (auto& source : sources) { - 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; + 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.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)); + 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)); } std::sort(result.devices.begin(), result.devices.end(), @@ -682,78 +317,6 @@ 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(); } @@ -791,11 +354,10 @@ void DeviceManager::log_device_plan_() const CMVR_LOG(INFO) << "[DeviceManager]: Device plan end"; } -bool DeviceManager::pre_scan_robot_arm_dependencies_() const +void 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) { @@ -803,22 +365,22 @@ bool DeviceManager::pre_scan_robot_arm_dependencies_() const } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty"; - return false; + return; } if (entry.config_file().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id(); - return false; + return; } 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 false; + return; } 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 false; + return; } motor_roots.emplace(entry.id(), std::move(root_cfg)); } @@ -829,17 +391,17 @@ bool DeviceManager::pre_scan_robot_arm_dependencies_() const } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty"; - return false; + return; } if (entry.config_file().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id(); - return false; + return; } 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 false; + return; } const config::RobotArmConfig* arm_cfg = nullptr; @@ -852,30 +414,29 @@ bool 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 false; + return; } - if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor || - arm_cfg->backend_case() == config::RobotArmConfig::kUme) { + if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) { continue; } if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id(); - return false; + return; } 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 false; + return; } if (motor_config.motor_group_ids_size() == 0) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id(); - return false; + return; } if (motor_config.joint_names_size() == 0) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id(); - return false; + return; } const auto motor_root_it = motor_roots.find(motor_config.motor_system_id()); @@ -883,7 +444,7 @@ bool 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 false; + return; } std::unordered_set allowed_groups; @@ -891,7 +452,7 @@ bool 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 false; + return; } allowed_groups.insert(group_id); } @@ -900,7 +461,7 @@ bool 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 false; + return; } std::string matched_group; @@ -919,7 +480,7 @@ bool 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 false; + return; } group_selection[matched_group].insert(joint_name); } @@ -934,132 +495,46 @@ bool 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; } -bool DeviceManager::init_devices_() { - bool all_initialized = true; +void DeviceManager::init_devices_() { 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; - 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; - } + DeviceRecord record = dev_factory_->create(entry); 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); - 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); + if (devices_.count(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); - 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); + if (!record.device->init()) { 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" @@ -1067,55 +542,8 @@ bool DeviceManager::init_devices_() { << ", type=" << record.type_name << ", kind=" << toString(record.kind) << ", config_file=" << entry.config_file(); - 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)); - } + devices_.emplace(record.id, std::move(record)); } - return all_initialized; } void DeviceManager::configure_mujoco_viewer_pip_() diff --git a/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp b/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp deleted file mode 100644 index eb627d0f..00000000 --- a/cmvr-es/manager/device_manager/src/device_safety_adapters.cpp +++ /dev/null @@ -1,1022 +0,0 @@ -#include "manager/device_manager/include/device_safety_adapters.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "devices/agv/abstract_agv.h" -#include "devices/arm/robot_arm.h" -#include "devices/battery/abstract_battery.h" -#include "devices/biohead/abstract_biohead.h" -#include "devices/camera/abstract_camera.h" -#include "devices/dexhand/abstract_dexhand.h" -#include "devices/gripper/abstract_gripper.h" -#include "devices/microphone/abstract_microphone.h" -#include "devices/motor/manager/include/motor_manager.h" -#include "devices/speaker/abstract_speaker.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" -#include "manager/safety_manager/include/device_safety_endpoint.h" - -namespace cmvr::device { - -namespace { - -using safety::AdmissionPermit; -using safety::BarrierToken; -using safety::BlockerScope; -using safety::CommandIntent; -using safety::DeviceSafetyDescriptor; -using safety::DeviceSafetyEndpoint; -using safety::DeviceSafetySnapshot; -using safety::HardwareCheckResult; -using safety::ParticipantDescriptor; -using safety::ParticipantPhase; -using safety::ParticipantResult; -using safety::RecoveryCheckResult; -using safety::RecoveryContext; -using safety::RecoveryRequirement; -using safety::SafetyBlocker; -using safety::SafetyClock; -using safety::SafetyCondition; -using safety::SafetyOperationContext; -using safety::SafetyParticipant; -using safety::SafetyPolicyFamily; -using safety::SafetyReason; -using safety::SafetySnapshotPublisher; -using safety::TriState; - -using Probe = std::function; -using Operation = std::function; - -TriState tri(const bool value) noexcept -{ - return value ? TriState::True : TriState::False; -} - -bool agvStopResultAccepted(const AgvResult& result) noexcept -{ - return result.ok() || result.code == AgvErrorCode::UnsupportedCommand; -} - -SafetyBlocker blocker( - const SafetyReason reason, - const std::string& source) -{ - return { - reason, - BlockerScope::Device, - RecoveryRequirement::HardwareReleaseRequired, - source, - {}, - 0, - 0}; -} - -DeviceSafetySnapshot baseSnapshot( - const std::string& id, - const std::uint64_t generation, - const std::uint64_t sequence) -{ - DeviceSafetySnapshot snapshot; - snapshot.device_id = id; - snapshot.device_generation = generation; - snapshot.sample_sequence = sequence; - snapshot.observed_at = SafetyClock::now(); - return snapshot; -} - -DeviceSafetyDescriptor defaultDescriptor(const AbstractDevice& device) -{ - DeviceSafetyDescriptor descriptor; - descriptor.device_id = device.id(); - descriptor.kind = device.kind(); - descriptor.maximum_snapshot_age = std::chrono::milliseconds(1000); - switch (device.kind()) { - case DeviceKind::AGV: - case DeviceKind::Arm: - case DeviceKind::BioHead: - case DeviceKind::DexHand: - case DeviceKind::Gripper: - case DeviceKind::Motor: - case DeviceKind::MotorSystem: - case DeviceKind::MujocoWorld: - case DeviceKind::Robot: - descriptor.default_policy = SafetyPolicyFamily::Control; - descriptor.requires_safe_stop = true; - descriptor.maximum_snapshot_age = std::chrono::milliseconds(500); - break; - case DeviceKind::Camera: - case DeviceKind::Microphone: - case DeviceKind::Speaker: - descriptor.default_policy = SafetyPolicyFamily::Sensor; - descriptor.requires_safe_stop = true; - break; - case DeviceKind::Battery: - case DeviceKind::CanBus: - case DeviceKind::MujocoViewer: - case DeviceKind::Unknown: - descriptor.default_policy = SafetyPolicyFamily::Sensor; - descriptor.requires_safe_stop = false; - break; - } - descriptor.supports_active_refresh = true; - return descriptor; -} - -class LegacyDeviceSafetyEndpoint final : public DeviceSafetyEndpoint { -public: - LegacyDeviceSafetyEndpoint(DeviceSafetyDescriptor descriptor, Probe probe) - : descriptor_(std::move(descriptor)), - probe_(std::move(probe)), - poll_interval_(std::clamp( - descriptor_.maximum_snapshot_age / 2, - std::chrono::milliseconds(50), - std::chrono::milliseconds(500))), - worker_([this] { workerLoop_(); }) - { - } - - ~LegacyDeviceSafetyEndpoint() override - { - { - std::lock_guard lock(mutex_); - stopping_ = true; - refresh_requested_ = true; - } - refresh_.notify_all(); - if (worker_.joinable()) { - worker_.join(); - } - } - - DeviceSafetyDescriptor descriptor() const override - { - return descriptor_; - } - - void bindPublisher(SafetySnapshotPublisher publisher) override - { - std::lock_guard lock(mutex_); - publisher_ = std::move(publisher); - } - - void requestSafetyRefresh() noexcept override - { - try { - { - std::lock_guard lock(mutex_); - refresh_requested_ = true; - } - refresh_.notify_one(); - } catch (...) { - } - } - - void onDeviceGenerationChanged( - const std::uint64_t generation) noexcept override - { - generation_.store(generation, std::memory_order_release); - sequence_.store(0, std::memory_order_release); - requestSafetyRefresh(); - } - - HardwareCheckResult validateBeforeDispatch( - const AdmissionPermit& permit) override - { - if (permit.intent == CommandIntent::Observe || - permit.intent == CommandIntent::Stop || - permit.intent == CommandIntent::ResetFault) { - return {true, SafetyReason::None, {}}; - } - DeviceSafetySnapshot snapshot; - try { - snapshot = sample_(); - } catch (const std::exception& error) { - return {false, SafetyReason::InternalError, error.what()}; - } catch (...) { - return { - false, - SafetyReason::InternalError, - "legacy final hardware check threw an unknown exception"}; - } - if (snapshot.device_generation != permit.device_generation) { - return { - false, - SafetyReason::GenerationMismatch, - "device generation changed during final hardware check"}; - } - if (snapshot.condition == SafetyCondition::Unknown) { - return { - false, - SafetyReason::SafetyStateMissing, - "hardware safety state is unknown at dispatch"}; - } - if (snapshot.condition == SafetyCondition::Unsafe) { - return { - false, - SafetyReason::HardwareUnsafe, - "hardware safety state is unsafe at dispatch"}; - } - if (snapshot.connected != TriState::True) { - return { - false, - SafetyReason::DeviceDisconnected, - "device connection is not confirmed at dispatch"}; - } - if (descriptor_.default_policy == SafetyPolicyFamily::Control) { - if (snapshot.emergency_stop_active != TriState::False) { - return { - false, - SafetyReason::EmergencyStopActive, - "emergency stop is active or unknown at dispatch"}; - } - if (snapshot.protective_stop_active != TriState::False) { - return { - false, - SafetyReason::ProtectiveStopActive, - "protective stop is active or unknown at dispatch"}; - } - if (snapshot.fault_active != TriState::False) { - return { - false, - SafetyReason::DeviceFault, - "device fault is active or unknown at dispatch"}; - } - const bool safe_start_from_restricted = - permit.intent == CommandIntent::StartActivity && - snapshot.condition == SafetyCondition::Restricted; - if (snapshot.operational_ready != TriState::True && - !safe_start_from_restricted) { - return { - false, - SafetyReason::DeviceNotReady, - "device is not operationally ready at dispatch"}; - } - } - return {true, SafetyReason::None, {}}; - } - - RecoveryCheckResult reconcileAdmissionState( - const RecoveryContext&) override - { - // The legacy adapter has no operation which powers, enables, or moves - // hardware. A fresh snapshot was already verified by Coordinator. - return {true, SafetyReason::None, {}}; - } - -private: - DeviceSafetySnapshot sample_() - { - const auto generation = generation_.load(std::memory_order_acquire); - const auto sequence = - sequence_.fetch_add(1, std::memory_order_acq_rel) + 1U; - auto snapshot = probe_(generation, sequence); - snapshot.device_id = descriptor_.device_id; - snapshot.device_generation = generation; - snapshot.sample_sequence = sequence; - if (snapshot.observed_at == SafetyClock::time_point{}) { - snapshot.observed_at = SafetyClock::now(); - } - SafetySnapshotPublisher publisher; - { - std::lock_guard lock(mutex_); - publisher = publisher_; - } - if (publisher) { - (void)publisher(snapshot); - } - return snapshot; - } - - void workerLoop_() - { - std::unique_lock lock(mutex_); - while (!stopping_) { - refresh_.wait_for(lock, poll_interval_, [this] { - return stopping_ || refresh_requested_; - }); - if (stopping_) { - return; - } - refresh_requested_ = false; - lock.unlock(); - try { - (void)sample_(); - } catch (...) { - DeviceSafetySnapshot unknown = baseSnapshot( - descriptor_.device_id, - generation_.load(std::memory_order_acquire), - sequence_.fetch_add(1, std::memory_order_acq_rel) + 1U); - unknown.condition = SafetyCondition::Unknown; - unknown.blockers.push_back(blocker( - SafetyReason::SafetyStateMissing, - descriptor_.device_id)); - SafetySnapshotPublisher publisher; - { - std::lock_guard publisher_lock(mutex_); - publisher = publisher_; - } - if (publisher) { - (void)publisher(std::move(unknown)); - } - } - lock.lock(); - } - } - - DeviceSafetyDescriptor descriptor_; - Probe probe_; - const std::chrono::milliseconds poll_interval_; - std::atomic generation_{1}; - std::atomic sequence_{0}; - std::mutex mutex_; - std::condition_variable refresh_; - SafetySnapshotPublisher publisher_; - bool refresh_requested_{true}; - bool stopping_{false}; - std::thread worker_; -}; - -class CallbackSafetyParticipant final : public SafetyParticipant { -public: - CallbackSafetyParticipant( - ParticipantDescriptor descriptor, - bool control_resource, - Operation stop, - Operation verify) - : descriptor_(std::move(descriptor)), - control_resource_(control_resource), - stop_(std::move(stop)), - verify_(std::move(verify)) - { - } - - ParticipantDescriptor descriptor() const override - { - return descriptor_; - } - - BarrierToken beginBarrier( - const SafetyOperationContext& context) override - { - BarrierToken token; - token.participant_id = descriptor_.participant_id; - token.operation_id = context.operation_id; - token.safety_epoch = context.safety_epoch; - token.generation = - generation_.fetch_add(1, std::memory_order_relaxed) + 1U; - - if (!control_resource_) { - return token; - } - const auto acquired = control::ControlAuthorityManager::instance() - .preemptAcquire( - descriptor_.participant_id, - "safety:" + context.operation_id, - std::chrono::hours(24)); - if (!acquired.acquired) { - return {}; - } - try { - std::lock_guard lock(mutex_); - control_barriers_.emplace(token.generation, acquired.token); - } catch (...) { - control::ControlAuthorityManager::instance().release( - acquired.token); - throw; - } - return token; - } - - ParticipantResult requestQuiesce( - const BarrierToken&, - const SafetyOperationContext&) override - { - return stop_ ? stop_() : ParticipantResult{ - false, - SafetyReason::UnsupportedCommand, - "device has no safe-stop operation"}; - } - - ParticipantResult verifyQuiescent( - const BarrierToken& token, - const SafetyOperationContext& context) override - { - bool handler_drained = true; - if (control_resource_) { - control::ControlLeaseToken barrier; - { - std::lock_guard lock(mutex_); - const auto found = control_barriers_.find(token.generation); - if (found == control_barriers_.end()) { - return { - false, - SafetyReason::StopUnconfirmed, - "control barrier is no longer active"}; - } - barrier = found->second; - } - handler_drained = control::ControlAuthorityManager::instance() - .waitForPreemptedRelease( - barrier, - std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>( - std::max( - std::chrono::milliseconds::zero(), - std::chrono::duration_cast< - std::chrono::milliseconds>( - context.deadline - SafetyClock::now())))); - } - - // A preempted handler may have crossed its driver boundary after the - // first stop request. Repeat the typed stop after the dispatch fence - // before trusting the device's final quiescence report. - const auto final_stop = stop_ ? stop_() : ParticipantResult{ - false, - SafetyReason::UnsupportedCommand, - "device has no final safe-stop operation"}; - if (!handler_drained) { - std::string detail = - "preempted control handler did not leave its dispatch fence"; - if (!final_stop.success && !final_stop.detail.empty()) { - detail += "; final stop failed: " + final_stop.detail; - } - return { - false, - SafetyReason::ParticipantTimeout, - std::move(detail)}; - } - if (!final_stop.success) { - return final_stop; - } - return verify_ ? verify_() : ParticipantResult{ - false, - SafetyReason::StopUnconfirmed, - "device has no quiescence verification"}; - } - - RecoveryCheckResult recoverAdmission( - const BarrierToken&, - const RecoveryContext&) override - { - return {true, SafetyReason::None, {}}; - } - - ParticipantResult releaseBarrier( - const BarrierToken& token) noexcept override - { - if (!control_resource_) { - return {true, SafetyReason::None, {}}; - } - try { - control::ControlLeaseToken barrier; - { - std::lock_guard lock(mutex_); - const auto found = control_barriers_.find(token.generation); - if (found == control_barriers_.end()) { - return { - false, - SafetyReason::StopUnconfirmed, - "control barrier is no longer active"}; - } - barrier = found->second; - control_barriers_.erase(found); - } - control::ControlAuthorityManager::instance().release(barrier); - return {true, SafetyReason::None, {}}; - } catch (...) { - return { - false, - SafetyReason::InternalError, - "control barrier release threw an exception"}; - } - } - -private: - ParticipantDescriptor descriptor_; - bool control_resource_{false}; - Operation stop_; - Operation verify_; - std::atomic generation_{0}; - std::mutex mutex_; - std::unordered_map - control_barriers_; -}; - -Probe probeFor(const std::shared_ptr& device) -{ - if (const auto arm = std::dynamic_pointer_cast(device)) { - return [arm](const std::uint64_t generation, - const std::uint64_t sequence) { - auto snapshot = baseSnapshot(arm->id(), generation, sequence); - const auto state = arm->getRobotState(); - snapshot.connected = tri(state.connected); - snapshot.operational_ready = tri( - state.connected && state.powered_on && - !state.fault && !state.emergency_stopped && - !state.protective_stopped); - snapshot.quiescent = tri(!state.moving && !state.program_running); - snapshot.motion_active = tri(state.moving || state.program_running); - snapshot.actuator_enabled = tri(state.powered_on); - snapshot.emergency_stop_active = tri(state.emergency_stopped); - snapshot.protective_stop_active = tri(state.protective_stopped); - snapshot.fault_active = tri(state.fault); - snapshot.condition = !state.connected - ? SafetyCondition::Unknown - : state.fault || state.emergency_stopped || - state.protective_stopped - ? SafetyCondition::Unsafe - : state.powered_on - ? SafetyCondition::Nominal - : SafetyCondition::Restricted; - if (state.emergency_stopped) { - snapshot.blockers.push_back(blocker( - SafetyReason::EmergencyStopActive, arm->id())); - } - if (state.protective_stopped) { - snapshot.blockers.push_back(blocker( - SafetyReason::ProtectiveStopActive, arm->id())); - } - if (state.fault) { - snapshot.blockers.push_back(blocker( - SafetyReason::DeviceFault, arm->id())); - } - return snapshot; - }; - } - - if (const auto agv = std::dynamic_pointer_cast(device)) { - return [agv](const std::uint64_t generation, - const std::uint64_t sequence) { - auto snapshot = baseSnapshot(agv->id(), generation, sequence); - const auto state = agv->runtimeState(); - const auto navigation = agv->navigationStatus(); - const bool active_navigation = - navigation.state == AgvTaskState::Waiting || - navigation.state == AgvTaskState::Running || - navigation.state == AgvTaskState::Paused; - snapshot.connected = tri(state.connected); - snapshot.operational_ready = tri( - state.connected && state.localized && !state.fault && - !state.emergency_stopped); - snapshot.quiescent = tri(!state.moving && !active_navigation); - snapshot.motion_active = tri(state.moving || active_navigation); - snapshot.actuator_enabled = TriState::Unknown; - snapshot.emergency_stop_active = tri(state.emergency_stopped); - snapshot.protective_stop_active = TriState::Unknown; - snapshot.fault_active = tri(state.fault); - snapshot.condition = !state.connected - ? SafetyCondition::Unknown - : state.fault || state.emergency_stopped - ? SafetyCondition::Unsafe - : SafetyCondition::Nominal; - return snapshot; - }; - } - - if (const auto hand = std::dynamic_pointer_cast(device)) { - return [hand](const std::uint64_t generation, - const std::uint64_t sequence) { - auto snapshot = baseSnapshot(hand->id(), generation, sequence); - const auto state = hand->state(); - const bool connected = - state == AbstractDexHand::Status::INITIALIZED || - state == AbstractDexHand::Status::STREAMING || - state == AbstractDexHand::Status::STOPPED; - snapshot.connected = tri(connected); - snapshot.operational_ready = tri(connected); - snapshot.quiescent = state == AbstractDexHand::Status::STOPPED || - state == AbstractDexHand::Status::INITIALIZED - ? TriState::True - : TriState::Unknown; - snapshot.motion_active = TriState::Unknown; - snapshot.actuator_enabled = TriState::Unknown; - snapshot.emergency_stop_active = TriState::Unknown; - snapshot.protective_stop_active = TriState::Unknown; - snapshot.fault_active = tri( - state == AbstractDexHand::Status::FAULT); - snapshot.condition = !connected - ? state == AbstractDexHand::Status::FAULT - ? SafetyCondition::Unsafe - : SafetyCondition::Unknown - : SafetyCondition::Nominal; - return snapshot; - }; - } - - if (const auto motors = std::dynamic_pointer_cast(device)) { - return [motors](const std::uint64_t generation, - const std::uint64_t sequence) { - auto snapshot = baseSnapshot(motors->id(), generation, sequence); - const auto& motor_map = motors->motorsMap(); - if (motor_map.empty()) { - snapshot.condition = SafetyCondition::Unknown; - snapshot.blockers.push_back(blocker( - SafetyReason::SafetyStateMissing, motors->id())); - return snapshot; - } - bool quiescent = true; - for (const auto& [joint, motor] : motor_map) { - (void)joint; - if (!motor) { - snapshot.condition = SafetyCondition::Unknown; - return snapshot; - } - quiescent = quiescent && std::abs(motor->getQd()) < 1e-3; - } - snapshot.connected = TriState::True; - snapshot.operational_ready = TriState::True; - snapshot.quiescent = tri(quiescent); - snapshot.motion_active = tri(!quiescent); - snapshot.condition = SafetyCondition::Nominal; - return snapshot; - }; - } - - if (const auto camera = std::dynamic_pointer_cast(device)) { - return [camera](const std::uint64_t generation, - const std::uint64_t sequence) { - auto snapshot = baseSnapshot(camera->id(), generation, sequence); - CameraState state{}; - camera->getState(state); - snapshot.connected = tri(state.is_initialized && state.is_opened); - snapshot.operational_ready = tri( - state.is_initialized && state.is_opened && !state.is_error); - snapshot.quiescent = tri( - !state.is_streaming && !state.is_recording); - snapshot.motion_active = TriState::Unknown; - snapshot.fault_active = tri(state.is_error); - snapshot.condition = state.is_error - ? SafetyCondition::Unsafe - : state.is_initialized - ? SafetyCondition::Nominal - : SafetyCondition::Unknown; - return snapshot; - }; - } - - if (const auto microphone = - std::dynamic_pointer_cast(device)) { - return [microphone](const std::uint64_t generation, - const std::uint64_t sequence) { - auto snapshot = baseSnapshot( - microphone->id(), generation, sequence); - MicrophoneState state{}; - microphone->getState(state); - snapshot.connected = tri(state.is_initialized); - snapshot.operational_ready = tri( - state.is_initialized && !state.is_error); - snapshot.quiescent = tri(!state.is_recording); - snapshot.fault_active = tri(state.is_error); - snapshot.condition = state.is_error - ? SafetyCondition::Unsafe - : state.is_initialized - ? SafetyCondition::Nominal - : SafetyCondition::Unknown; - return snapshot; - }; - } - - if (const auto speaker = - std::dynamic_pointer_cast(device)) { - return [speaker](const std::uint64_t generation, - const std::uint64_t sequence) { - auto snapshot = baseSnapshot(speaker->id(), generation, sequence); - SpeakerState state{}; - speaker->getState(state); - snapshot.connected = tri(state.is_initialized); - snapshot.operational_ready = tri(state.is_initialized); - snapshot.quiescent = tri( - !state.is_running && !state.is_decoding); - snapshot.condition = state.is_initialized - ? SafetyCondition::Nominal - : SafetyCondition::Unknown; - return snapshot; - }; - } - - if (const auto head = std::dynamic_pointer_cast(device)) { - return [head](const std::uint64_t generation, - const std::uint64_t sequence) { - auto snapshot = baseSnapshot(head->id(), generation, sequence); - RobotState state{}; - head->getState(state); - snapshot.connected = state.error - ? TriState::False - : TriState::True; - snapshot.operational_ready = tri(!state.error); - snapshot.quiescent = TriState::Unknown; - snapshot.motion_active = TriState::Unknown; - snapshot.emergency_stop_active = TriState::Unknown; - snapshot.protective_stop_active = TriState::Unknown; - snapshot.fault_active = tri(state.error); - snapshot.condition = state.error - ? SafetyCondition::Unsafe - : SafetyCondition::Nominal; - return snapshot; - }; - } - - if (const auto battery = - std::dynamic_pointer_cast(device)) { - return [battery](const std::uint64_t generation, - const std::uint64_t sequence) { - auto snapshot = baseSnapshot(battery->id(), generation, sequence); - BatteryState state{}; - battery->getState(state); - const bool fault = state.health == Lifecycle::ERROR || - state.health == Lifecycle::ESTOP; - snapshot.connected = state.health == Lifecycle::INIT - ? TriState::Unknown - : TriState::True; - snapshot.operational_ready = tri(!fault); - snapshot.quiescent = TriState::True; - snapshot.fault_active = tri(fault); - snapshot.condition = fault - ? SafetyCondition::Unsafe - : state.health == Lifecycle::INIT - ? SafetyCondition::Unknown - : SafetyCondition::Nominal; - return snapshot; - }; - } - - return [id = device->id()](const std::uint64_t generation, - const std::uint64_t sequence) { - auto snapshot = baseSnapshot(id, generation, sequence); - snapshot.condition = SafetyCondition::Unknown; - snapshot.blockers.push_back(blocker( - SafetyReason::SafetyStateMissing, id)); - return snapshot; - }; -} - -std::shared_ptr participantFor( - const std::shared_ptr& device, - const DeviceSafetyDescriptor& safety_descriptor, - const std::chrono::milliseconds timeout) -{ - Operation stop; - Operation verify; - - if (const auto arm = std::dynamic_pointer_cast(device)) { - stop = [arm] { - auto result = arm->stopMotion(); - if (!result.ok()) { - result = arm->emergencyStop(); - } - return ParticipantResult{ - result.ok(), - result.ok() ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - result.message}; - }; - verify = [arm] { - const auto state = arm->getRobotState(); - const bool stopped = !state.moving && !state.program_running; - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : SafetyReason::DeviceStillMoving, - stopped ? std::string{} - : "RobotArm still reports active motion"}; - }; - } else if (const auto agv = - std::dynamic_pointer_cast(device)) { - stop = [agv] { - const auto cancel = agv->cancelNavigation(); - const auto velocity = agv->stopVelocityControl(); - const auto mapping = agv->stopMapping(); - const auto confirmed = agv->confirmMotionStopped(); - const bool stopped = - agvStopResultAccepted(cancel) && - agvStopResultAccepted(velocity) && - agvStopResultAccepted(mapping) && confirmed.ok(); - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : confirmed.ok() ? SafetyReason::StopUnconfirmed - : SafetyReason::DeviceStillMoving, - stopped ? std::string{} - : confirmed.message.empty() - ? "AGV operational stop was not confirmed" - : confirmed.message}; - }; - verify = [agv] { - const auto confirmed = agv->confirmMotionStopped(); - return ParticipantResult{ - confirmed.ok(), - confirmed.ok() ? SafetyReason::None - : SafetyReason::DeviceStillMoving, - confirmed.message}; - }; - } else if (const auto hand = - std::dynamic_pointer_cast(device)) { - stop = [hand] { - const bool stopped = hand->stopOperationalActivity(); - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - stopped ? std::string{} - : "DexHand did not confirm operational stop"}; - }; - verify = stop; - } else if (const auto motors = - std::dynamic_pointer_cast(device)) { - stop = [motors] { - bool stopped = true; - for (const auto& [joint, motor] : motors->motorsMap()) { - (void)joint; - stopped = motor && motor->quickStop() && stopped; - } - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - stopped ? std::string{} - : "one or more motors did not acknowledge Quick Stop"}; - }; - verify = [motors] { - bool stopped = !motors->motorsMap().empty(); - for (const auto& [joint, motor] : motors->motorsMap()) { - (void)joint; - stopped = stopped && motor && std::abs(motor->getQd()) < 1e-3; - } - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : SafetyReason::DeviceStillMoving, - stopped ? std::string{} - : "motor velocity has not converged to zero"}; - }; - } else if (const auto camera = - std::dynamic_pointer_cast(device)) { - stop = [camera] { - CameraState state{}; - camera->getState(state); - if (state.is_recording) { - camera->stopRecording(); - } - // Some backends use the default operational hook and report - // false even when no such activity exists. The observable final - // capture state is authoritative for this generic adapter. - (void)camera->stopOperationalActivity(); - camera->getState(state); - const bool stopped = !state.is_streaming && !state.is_recording; - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - stopped ? std::string{} - : "camera activity did not stop"}; - }; - verify = [camera] { - CameraState state{}; - camera->getState(state); - const bool stopped = !state.is_streaming && !state.is_recording; - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - stopped ? std::string{} - : "camera still reports active capture or recording"}; - }; - } else if (const auto microphone = - std::dynamic_pointer_cast(device)) { - stop = [microphone] { - microphone->stopStreaming(); - microphone->stopRecording(); - return ParticipantResult{true, SafetyReason::None, {}}; - }; - verify = [microphone] { - MicrophoneState state{}; - microphone->getState(state); - return ParticipantResult{ - !state.is_recording, - !state.is_recording ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - state.is_recording - ? "microphone still reports active recording" - : std::string{}}; - }; - } else if (const auto speaker = - std::dynamic_pointer_cast(device)) { - stop = [speaker] { - const bool stopped = speaker->stopPlayback(); - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - stopped ? std::string{} - : "speaker playback did not stop"}; - }; - verify = [speaker] { - SpeakerState state{}; - speaker->getState(state); - const bool stopped = !state.is_running && !state.is_decoding; - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - stopped ? std::string{} - : "speaker still reports active playback"}; - }; - } else if (const auto head = - std::dynamic_pointer_cast(device)) { - stop = [head] { - const bool stopped = head->stopOperationalActivity(); - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - stopped ? std::string{} - : "BioHead did not confirm operational stop"}; - }; - verify = stop; - } else if (const auto gripper = - std::dynamic_pointer_cast(device)) { - stop = [gripper] { - const bool stopped = gripper->stopOperationalActivity(); - return ParticipantResult{ - stopped, - stopped ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - stopped ? std::string{} - : "gripper did not confirm operational stop"}; - }; - verify = stop; - } - - if (!stop || !verify) { - return {}; - } - ParticipantDescriptor descriptor; - descriptor.participant_id = device->id(); - descriptor.phase = safety_descriptor.default_policy == - SafetyPolicyFamily::Control - ? ParticipantPhase::Actuator - : ParticipantPhase::PeripheralActivity; - descriptor.required = true; - descriptor.timeout = timeout > std::chrono::milliseconds::zero() - ? timeout - : std::chrono::seconds(5); - return std::make_shared( - std::move(descriptor), - safety_descriptor.default_policy == SafetyPolicyFamily::Control, - std::move(stop), - std::move(verify)); -} - -} // namespace - -safety::DeviceSafetyRegistration makeDeviceSafetyRegistration( - const std::shared_ptr& device, - const std::chrono::milliseconds configured_maximum_age, - const std::chrono::milliseconds configured_stop_timeout) -{ - if (!device) { - return {}; - } - - safety::DeviceSafetyRegistration registration; - if (const auto endpoint_provider = - std::dynamic_pointer_cast( - device)) { - registration.endpoint = endpoint_provider->safetyEndpoint(); - } - if (registration.endpoint) { - registration.descriptor = registration.endpoint->descriptor(); - } else { - registration.descriptor = defaultDescriptor(*device); - registration.endpoint = std::make_shared( - registration.descriptor, probeFor(device)); - } - - if (configured_maximum_age > std::chrono::milliseconds::zero()) { - registration.descriptor.maximum_snapshot_age = std::min( - registration.descriptor.maximum_snapshot_age, - configured_maximum_age); - } - - if (const auto participant_provider = - std::dynamic_pointer_cast( - device)) { - registration.participant = participant_provider->safetyParticipant(); - } - if (!registration.participant) { - registration.participant = participantFor( - device, registration.descriptor, configured_stop_timeout); - } - return registration; -} - -} // namespace cmvr::device diff --git a/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp b/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp deleted file mode 100644 index 4149a835..00000000 --- a/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp +++ /dev/null @@ -1,153 +0,0 @@ -#include "manager/device_manager/include/device_manager.h" - -#include - -#include - -namespace { - -class LifecycleDevice final : public cmvr::device::AbstractDevice { -public: - explicit LifecycleDevice(const std::string& id) - : AbstractDevice(id) - { - } - - cmvr::device::DeviceKind kind() const noexcept override - { - return cmvr::device::DeviceKind::Camera; - } - - std::string typeName() const override { return "LifecycleDevice"; } - - bool start() override - { - ++start_calls; - if (throw_on_start) { - throw std::runtime_error("start failure"); - } - return start_result; - } - - bool stop() override - { - ++stop_calls; - return true; - } - - bool start_result{true}; - bool throw_on_start{false}; - int start_calls{0}; - int stop_calls{0}; -}; - -class DeviceManagerLifecycleTest : public ::testing::Test { -protected: - void SetUp() override - { - cmvr::device::DeviceManager::destroyInstance(); - } - - void TearDown() override - { - cmvr::device::DeviceManager::destroyInstance(); - } -}; - -TEST_F(DeviceManagerLifecycleTest, - EnabledDeviceCreationFailureMarksInitializationFailed) -{ - cmvr::config::DeviceManagerConfig config; - auto* entry = config.add_devices(); - entry->set_id("unsupported"); - entry->set_type( - cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN); - entry->set_enable(true); - - auto& manager = - cmvr::device::DeviceManager::getInstance(config); - EXPECT_FALSE(manager.initialized()); - EXPECT_FALSE(manager.start()); -} - -TEST_F(DeviceManagerLifecycleTest, DisabledInvalidDeviceIsIgnored) -{ - cmvr::config::DeviceManagerConfig config; - auto* entry = config.add_devices(); - entry->set_id("disabled"); - entry->set_type( - cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN); - entry->set_enable(false); - - auto& manager = - cmvr::device::DeviceManager::getInstance(config); - EXPECT_TRUE(manager.initialized()); - EXPECT_TRUE(manager.start()); -} - -TEST_F(DeviceManagerLifecycleTest, - DeviceStartFailureIsReturnedAndTriggersStop) -{ - cmvr::config::DeviceManagerConfig config; - auto& manager = - cmvr::device::DeviceManager::getInstance(config); - ASSERT_TRUE(manager.initialized()); - - auto device = - std::make_shared("start_failure"); - device->start_result = false; - manager.registerDevice(device); - - EXPECT_FALSE(manager.start()); - EXPECT_EQ(device->start_calls, 1); - EXPECT_EQ(device->stop_calls, 1); -} - -TEST_F(DeviceManagerLifecycleTest, - DeviceStartExceptionIsReturnedAndTriggersStop) -{ - cmvr::config::DeviceManagerConfig config; - auto& manager = - cmvr::device::DeviceManager::getInstance(config); - ASSERT_TRUE(manager.initialized()); - - auto device = - std::make_shared("start_exception"); - device->throw_on_start = true; - manager.registerDevice(device); - - EXPECT_FALSE(manager.start()); - EXPECT_EQ(device->start_calls, 1); - EXPECT_EQ(device->stop_calls, 1); -} - -TEST_F(DeviceManagerLifecycleTest, - EnforceSelectedCannotStartWithMissingConfiguredTarget) -{ - cmvr::config::DeviceManagerConfig config; - auto* safety = config.mutable_safety(); - safety->set_mode( - cmvr::config::SafetyManagerConfig::ENFORCE_SELECTED); - safety->add_enforced_device_ids("missing-arm"); - - auto& manager = cmvr::device::DeviceManager::getInstance(config); - ASSERT_TRUE(manager.initialized()); - EXPECT_FALSE(manager.start()); - EXPECT_EQ( - manager.safetyManager().snapshot().system_state, - cmvr::safety::SystemAdmissionState::Starting); -} - -TEST_F(DeviceManagerLifecycleTest, - EnforceSelectedCannotSilentlyCoverNoDevices) -{ - cmvr::config::DeviceManagerConfig config; - config.mutable_safety()->set_mode( - cmvr::config::SafetyManagerConfig::ENFORCE_SELECTED); - - auto& manager = cmvr::device::DeviceManager::getInstance(config); - ASSERT_TRUE(manager.initialized()); - EXPECT_FALSE(manager.start()); -} - -} // namespace 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 25f69f36..05d5bf8c 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,12 +5,8 @@ #include "devices/microphone/abstract_microphone.h" #include -#include -#include #include -#include #include -#include #include #include #include @@ -27,7 +23,6 @@ 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; @@ -128,51 +123,6 @@ 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) { @@ -194,16 +144,6 @@ 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; @@ -313,13 +253,6 @@ 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 = { @@ -331,18 +264,18 @@ bool testConfiguredAndDynamicSnapshots() auto health_throw = std::make_shared("b_health_throw"); health_throw->throw_on_health = true; - dynamic_manager.registerDevice(healthy); - dynamic_manager.registerDevice(degraded); - dynamic_manager.registerDevice(start_fail); - dynamic_manager.registerDevice(stop_fail); - dynamic_manager.registerDevice(health_throw); + manager.registerDevice(healthy); + manager.registerDevice(degraded); + manager.registerDevice(start_fail); + manager.registerDevice(stop_fail); + manager.registerDevice(health_throw); // Duplicate registration must retain the original object and status. - dynamic_manager.registerDevice( + manager.registerDevice( std::make_shared("z_healthy", DeviceKind::Speaker)); - CHECK_TRUE(dynamic_manager.getDeviceBase("z_healthy") == healthy); + CHECK_TRUE(manager.getDeviceBase("z_healthy") == healthy); - const auto registered = dynamic_manager.snapshot(); + const auto registered = manager.snapshot(); CHECK_TRUE(isSorted(registered)); const auto* healthy_registered = findDevice(registered, "z_healthy"); @@ -371,8 +304,8 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(thrown_health->health.error_message.size() <= 512); CHECK_TRUE(thrown_health->error_message.size() <= 512); - CHECK_TRUE(!dynamic_manager.start()); - const auto running = dynamic_manager.snapshot(); + manager.start(); + const auto running = manager.snapshot(); CHECK_TRUE(findDevice(running, "z_healthy")->state == ManagedDeviceState::Running); CHECK_TRUE(findDevice(running, "m_start_fail")->state == @@ -386,16 +319,14 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(healthy_registered->state == ManagedDeviceState::Registered); - dynamic_manager.stop(); - const auto stopped = dynamic_manager.snapshot(); + manager.stop(); + const auto stopped = 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); - // Failed start rolls back every device once; explicit stop performs the - // second best-effort stop. - CHECK_TRUE(healthy->stop_calls.load() == 2); + CHECK_TRUE(healthy->stop_calls.load() == 1); return true; } @@ -434,71 +365,6 @@ 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() @@ -507,8 +373,7 @@ int main() const bool success = testCategoryHealthAdapters() && testConfiguredAndDynamicSnapshots() && - testConcurrentSnapshotAndRegistration() && - testManagerSnapshotsDoNotWaitForDeviceHealth(); + testConcurrentSnapshotAndRegistration(); 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 new file mode 100644 index 00000000..6a3634fd --- /dev/null +++ b/cmvr-es/manager/media_source_hub/CMakeLists.txt @@ -0,0 +1,61 @@ +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(device_media_source_adapter STATIC + src/device_media_source_adapter.cpp + ) + target_compile_features(device_media_source_adapter PUBLIC cxx_std_17) + target_include_directories(device_media_source_adapter + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../.. + ) + target_link_libraries(device_media_source_adapter + PUBLIC + cmvr_es::media_source_hub + cmvr_es::common + cmvr_es::proto + cmvr_es::logging + ) + add_library(cmvr_es::device_media_source_adapter ALIAS device_media_source_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_manager/include/device_media_source_adapter.h b/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h similarity index 66% rename from cmvr-es/manager/media_source_manager/include/device_media_source_adapter.h rename to cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h index 9dcbcef5..728e2d85 100644 --- a/cmvr-es/manager/media_source_manager/include/device_media_source_adapter.h +++ b/cmvr-es/manager/media_source_hub/include/device_media_source_adapter.h @@ -9,35 +9,27 @@ #include "devices/camera/abstract_camera.h" #include "devices/microphone/abstract_microphone.h" -#include "manager/media_source_manager/include/media_source_manager.h" -#include "manager/safety_manager/include/safety_manager.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. -MediaSourceManager& globalMediaSourceManager(); +MediaSourceHub& globalMediaSourceHub(); std::string cameraColorTrackId(const std::string& device_id); std::string microphoneTrackId(const std::string& device_id); -// Acquires the Coordinator's Sensor/StartActivity lane and performs the -// device endpoint's final hardware check. Keep the returned guard alive until -// the operation which can start the physical media producer has returned. -safety::DispatchGuard beginMediaSourceStartDispatch( - safety::SafetyManager& coordinator, - const std::string& device_id); - // 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( - MediaSourceManager& hub, + MediaSourceHub& hub, const std::shared_ptr& camera, size_t ring_capacity = 64); bool ensureMicrophoneMediaSource( - MediaSourceManager& hub, + MediaSourceHub& hub, const std::shared_ptr& microphone, size_t ring_capacity = 256); diff --git a/cmvr-es/manager/media_source_manager/include/media_source_manager.h b/cmvr-es/manager/media_source_hub/include/media_source_hub.h similarity index 62% rename from cmvr-es/manager/media_source_manager/include/media_source_manager.h rename to cmvr-es/manager/media_source_hub/include/media_source_hub.h index 6efa6422..c7dcb553 100644 --- a/cmvr-es/manager/media_source_manager/include/media_source_manager.h +++ b/cmvr-es/manager/media_source_hub/include/media_source_hub.h @@ -1,5 +1,5 @@ -#ifndef CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H -#define CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H +#ifndef CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H +#define CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H #pragma once @@ -15,21 +15,17 @@ #include "common/base/ring_buffer.h" #include "common/media/media_frame.h" -namespace cmvr::service { -class StopAllAdmissionGate; -} - namespace cmvr::media { -// MediaSourceManager owns no protocol-specific state. A device or capture adapter registers +// 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 MediaSourceManager final { +class MediaSourceHub final { public: using FrameRing = BroadcastFrameRing; using FrameReadResult = FrameRing::ReadResult; using StartPosition = FrameRing::StartPosition; using FrameSink = std::function; - // Cancellation checks run while MediaSourceManager protects source lifecycle + // 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; @@ -37,7 +33,7 @@ public: struct SourceCallbacks { // start() may run asynchronously. It must observe cancelled during any // potentially blocking startup work and return false promptly once set. - // MediaSourceManager retains the callback state until a non-cooperative start + // MediaSourceHub retains the callback state until a non-cooperative start // eventually returns, so late completion cannot access destroyed state. std::function stop; - // Optional confirmed variant used by operational StopAll. Returning - // false keeps the source quarantined so a later StopAll can retry it. - // When omitted, a non-throwing stop() call is treated as confirmation. - std::function stop_confirmed; std::function request_key_frame; }; @@ -84,7 +76,7 @@ public: void reset(); private: - friend class MediaSourceManager; + friend class MediaSourceHub; Subscription(std::shared_ptr source, FrameRing::Cursor cursor); std::shared_ptr source_; @@ -92,14 +84,11 @@ public: bool active_{false}; }; - // Pass the process-wide StopAll gate for a hub whose sources are part of - // whole-machine operational stopping. Test/private hubs may remain local. - explicit MediaSourceManager( - service::StopAllAdmissionGate* admission_gate = nullptr); - ~MediaSourceManager(); + MediaSourceHub(); + ~MediaSourceHub(); - MediaSourceManager(const MediaSourceManager&) = delete; - MediaSourceManager& operator=(const MediaSourceManager&) = delete; + MediaSourceHub(const MediaSourceHub&) = delete; + MediaSourceHub& operator=(const MediaSourceHub&) = delete; bool registerSource( TrackDescriptorPtr initial_descriptor, @@ -111,11 +100,6 @@ public: bool hasSource(const std::string& track_id) const; std::vector listTracks() const; - // Returns a stable, sorted snapshot of physical source IDs. The snapshot - // includes sources temporarily removed from the public track map while a - // stop callback is in progress, so StopAll can discover orphaned activity - // without consulting DeviceManager. - std::vector trackedSourceIds() const; size_t subscriberCount(const std::string& track_id) const; // Protocol adapters can request an IDR after a discontinuity without knowing the @@ -127,36 +111,18 @@ public: StartPosition start_position = StartPosition::NEXT_PUBLISHED, CancelPredicate cancelled = {}); - // Stops and unregisters every source whose registered descriptor belongs - // to source_id. Sources for other physical devices remain registered and - // keep running. A failed source is restored for a later retry. - bool stopSourcesForDevice( - const std::string& source_id, - std::vector* failures = nullptr); - - // Stops and unregisters every source that was registered before this call's - // stop phase began. Outstanding subscriptions are invalidated and blocked - // waitRead calls are awakened. Registrations concurrent with the stop wait - // for that phase to finish and are retained, so sources can be ensured and - // subscribed again after this method returns. - bool stopAllSources(std::vector* failures = nullptr); - // 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. Equivalent to stopAllSources(). + // shutdown or risking a use-after-free. void shutdown(); private: - bool stopSources( - const std::optional& source_id, - std::vector* failures); - struct Impl; std::shared_ptr impl_; }; } // namespace cmvr::media -#endif // CMVR_ES_MANAGER_MEDIA_SOURCE_MANAGER_H +#endif // CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H diff --git a/cmvr-es/manager/media_source_manager/src/device_media_source_adapter.cpp b/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp similarity index 88% rename from cmvr-es/manager/media_source_manager/src/device_media_source_adapter.cpp rename to cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp index 279ff554..de6f1338 100644 --- a/cmvr-es/manager/media_source_manager/src/device_media_source_adapter.cpp +++ b/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp @@ -1,6 +1,4 @@ -#include "manager/media_source_manager/include/device_media_source_adapter.h" - -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" +#include "manager/media_source_hub/include/device_media_source_adapter.h" #include #include @@ -20,8 +18,6 @@ namespace cmvr::media { namespace { -std::atomic media_start_sequence{0}; - std::string normalizedCodec(std::string codec) { codec.erase( std::remove_if(codec.begin(), codec.end(), [](const unsigned char c) { @@ -129,12 +125,12 @@ struct PumpState : public std::enable_shared_from_this> { : device(std::move(device_ptr)) {} virtual ~PumpState() { - (void)stop(); + stop(); } bool begin( - const MediaSourceManager::FrameSink& frame_sink, - const MediaSourceManager::CancelPredicate& cancelled) { + const MediaSourceHub::FrameSink& frame_sink, + const MediaSourceHub::CancelPredicate& cancelled) { if (!frame_sink || !device) { return false; } @@ -158,9 +154,7 @@ struct PumpState : public std::enable_shared_from_this> { // A previous worker must always be collected before a new capture lease starts. std::thread stale_worker = std::move(worker); lock.unlock(); - if (!collectThread(std::move(stale_worker))) { - return false; - } + collectThread(std::move(stale_worker)); lock.lock(); } @@ -221,7 +215,7 @@ struct PumpState : public std::enable_shared_from_this> { return true; } - bool stop() noexcept { + void stop() noexcept { std::thread thread; bool stop_streaming = false; { @@ -232,28 +226,24 @@ struct PumpState : public std::enable_shared_from_this> { sink = {}; thread = std::move(worker); } - bool stopped = true; if (stop_streaming) { - stopped = stopDeviceStreaming(); + stopDeviceStreaming(); } if (thread.joinable()) { - stopped = collectThread(std::move(thread)) && stopped; + collectThread(std::move(thread)); } - return stopped; } - static bool collectThread(std::thread thread) noexcept { + static void collectThread(std::thread thread) noexcept { if (!thread.joinable()) { - return true; + return; } try { if (thread.get_id() == std::this_thread::get_id()) { thread.detach(); - return false; } else { thread.join(); } - return true; } catch (const std::exception& error) { CMVR_LOG(ERROR) << "[DeviceMediaSourceAdapter] Failed to collect media pump: " << error.what(); @@ -265,39 +255,36 @@ struct PumpState : public std::enable_shared_from_this> { // platform error occurred; there is no recoverable ownership path. } } - return false; } } virtual void run() = 0; - bool stopDeviceStreaming() noexcept { + void stopDeviceStreaming() noexcept { try { if (device) { device->stopStreaming(); } - return true; } 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"; } - return false; } std::shared_ptr device; std::atomic running{false}; std::mutex mutex; std::thread worker; - MediaSourceManager::FrameSink sink; + 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 { (void)stop(); } + ~CameraPump() override { stop(); } void run() override { size_t cursor = 0; @@ -450,7 +437,7 @@ struct CameraPump final : PumpState { frame.key_frame = source.bKey; frame.discontinuity = pending_discontinuity; - MediaSourceManager::FrameSink current_sink; + MediaSourceHub::FrameSink current_sink; { std::lock_guard lock(mutex); current_sink = sink; @@ -474,7 +461,7 @@ struct CameraPump final : PumpState { struct MicrophonePump final : PumpState { MicrophonePump(std::shared_ptr microphone, std::string id) : PumpState(std::move(microphone)), track_id(std::move(id)) {} - ~MicrophonePump() override { (void)stop(); } + ~MicrophonePump() override { stop(); } void run() override { size_t cursor = 0; @@ -583,7 +570,7 @@ struct MicrophonePump final : PumpState { frame.key_frame = true; frame.discontinuity = pending_discontinuity; - MediaSourceManager::FrameSink current_sink; + MediaSourceHub::FrameSink current_sink; { std::lock_guard lock(mutex); current_sink = sink; @@ -621,8 +608,8 @@ TrackDescriptorPtr initialTrack( } // namespace -MediaSourceManager& globalMediaSourceManager() { - static MediaSourceManager hub(&service::globalStopAllAdmissionGate()); +MediaSourceHub& globalMediaSourceHub() { + static MediaSourceHub hub; return hub; } @@ -634,45 +621,8 @@ std::string microphoneTrackId(const std::string& device_id) { return device_id + "/audio/main"; } -safety::DispatchGuard beginMediaSourceStartDispatch( - safety::SafetyManager& coordinator, - const std::string& device_id) -{ - safety::AdmissionRequest request; - request.command = { - "cmvr.internal.MediaSourceManager/StartSource", - safety::CommandIntent::StartActivity, - safety::SafetyPolicyFamily::Sensor, - true, - false}; - request.actor.principal_id = "internal:media-source-hub"; - request.actor.authenticated = true; - request.command_id = "media-source-start:" + device_id + ':' + - std::to_string( - media_start_sequence.fetch_add(1, std::memory_order_relaxed) + 1U); - request.device_id = device_id; - - auto admission = coordinator.admit(request); - if (!admission.permit.has_value()) { - CMVR_LOG(WARNING) - << "[DeviceMediaSourceAdapter] Media source admission rejected for " - << device_id << ": " << safety::toString(admission.decision.reason) - << " (" << admission.decision.detail << ')'; - return {}; - } - auto dispatch = coordinator.beginDispatch(*admission.permit); - if (!dispatch.acquired()) { - CMVR_LOG(WARNING) - << "[DeviceMediaSourceAdapter] Media source final check rejected for " - << device_id << ": " - << safety::toString(dispatch.hardwareCheck().reason) << " (" - << dispatch.hardwareCheck().detail << ')'; - } - return dispatch; -} - bool ensureCameraMediaSource( - MediaSourceManager& hub, + MediaSourceHub& hub, const std::shared_ptr& camera, const size_t ring_capacity) { if (!camera || camera->id().empty()) { @@ -684,13 +634,13 @@ bool ensureCameraMediaSource( } const auto pump = std::make_shared(camera, track_id); - MediaSourceManager::SourceCallbacks callbacks; + MediaSourceHub::SourceCallbacks callbacks; callbacks.start = [pump]( - const MediaSourceManager::FrameSink& sink, - const MediaSourceManager::CancelPredicate& cancelled) { + const MediaSourceHub::FrameSink& sink, + const MediaSourceHub::CancelPredicate& cancelled) { return pump->begin(sink, cancelled); }; - callbacks.stop_confirmed = [pump] { return pump->stop(); }; + 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), @@ -704,7 +654,7 @@ bool ensureCameraMediaSource( } bool ensureMicrophoneMediaSource( - MediaSourceManager& hub, + MediaSourceHub& hub, const std::shared_ptr& microphone, const size_t ring_capacity) { if (!microphone || microphone->id().empty()) { @@ -716,13 +666,13 @@ bool ensureMicrophoneMediaSource( } const auto pump = std::make_shared(microphone, track_id); - MediaSourceManager::SourceCallbacks callbacks; + MediaSourceHub::SourceCallbacks callbacks; callbacks.start = [pump]( - const MediaSourceManager::FrameSink& sink, - const MediaSourceManager::CancelPredicate& cancelled) { + const MediaSourceHub::FrameSink& sink, + const MediaSourceHub::CancelPredicate& cancelled) { return pump->begin(sink, cancelled); }; - callbacks.stop_confirmed = [pump] { return pump->stop(); }; + callbacks.stop = [pump] { pump->stop(); }; const bool registered = hub.registerSource( initialTrack(track_id, microphone->id(), MediaKind::AUDIO), std::move(callbacks), diff --git a/cmvr-es/manager/media_source_manager/src/media_source_manager.cpp b/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp similarity index 58% rename from cmvr-es/manager/media_source_manager/src/media_source_manager.cpp rename to cmvr-es/manager/media_source_hub/src/media_source_hub.cpp index 1a69522d..cace2727 100644 --- a/cmvr-es/manager/media_source_manager/src/media_source_manager.cpp +++ b/cmvr-es/manager/media_source_hub/src/media_source_hub.cpp @@ -1,4 +1,4 @@ -#include "manager/media_source_manager/include/media_source_manager.h" +#include "manager/media_source_hub/include/media_source_hub.h" #include #include @@ -6,14 +6,11 @@ #include #include #include -#include #include -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - namespace cmvr::media { -struct MediaSourceManager::SourceState final : public std::enable_shared_from_this { +struct MediaSourceHub::SourceState final : public std::enable_shared_from_this { enum class Lifecycle { STOPPED, STARTING, @@ -31,14 +28,11 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th SourceState( TrackDescriptorPtr initial_descriptor, SourceCallbacks source_callbacks, - const size_t ring_capacity, - service::StopAllAdmissionGate* source_admission_gate) + const size_t ring_capacity) : track_id(initial_descriptor->id), - source_id(initial_descriptor->source_id), descriptor(std::move(initial_descriptor)), callbacks(std::move(source_callbacks)), - ring(ring_capacity), - admission_gate(source_admission_gate) {} + ring(ring_capacity) {} FrameSink makeSink() { const std::weak_ptr weak_source = shared_from_this(); @@ -91,18 +85,11 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th } } - bool invokeStop() noexcept { + void invokeStop() noexcept { std::lock_guard callback_lock(callback_mutex); try { - if (callbacks.stop_confirmed) { - return callbacks.stop_confirmed(); - } - if (callbacks.stop) { - callbacks.stop(); - } - return true; + if (callbacks.stop) callbacks.stop(); } catch (...) { - return false; } } @@ -130,10 +117,9 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th } if (stop_abandoned_start) { - const bool stopped = invokeStop(); + invokeStop(); std::lock_guard lock(lifecycle_mutex); - stop_unconfirmed = !stopped; - if (stopped && lifecycle == Lifecycle::STOPPING) { + if (lifecycle == Lifecycle::STOPPING) { lifecycle = Lifecycle::STOPPED; } lifecycle_condition.notify_all(); @@ -144,29 +130,12 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th const StartPosition start_position, FrameRing::Cursor& cursor, const CancelPredicate& cancelled) { - std::optional - admission; - std::unique_lock lock(lifecycle_mutex, std::defer_lock); - for (;;) { - if (admission_gate) { - admission.emplace(admission_gate->lockAdmission()); - } - lock.lock(); - if (!registered || isCancelled(cancelled) || - (admission && !admission->accepting())) { - return false; - } - if (lifecycle != Lifecycle::STOPPING) { - break; - } - - // A device stop may block, so never wait for it while retaining - // the process-wide admission lock. - admission.reset(); - lifecycle_condition.wait_for( - lock, std::chrono::milliseconds(10)); - lock.unlock(); + 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; @@ -209,10 +178,6 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th return false; } - // Startup is now ordered before beginStopAll(). Do not retain the - // process-wide gate while waiting for the device callback to return. - admission.reset(); - while (registered && !attempt->completed) { if (isCancelled(cancelled)) { if (attempt->waiters != 0U) --attempt->waiters; @@ -242,10 +207,9 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th lifecycle = Lifecycle::STOPPING; ring.close(); lock.unlock(); - const bool stopped = invokeStop(); + invokeStop(); lock.lock(); - stop_unconfirmed = !stopped; - if (stopped && lifecycle == Lifecycle::STOPPING) { + if (lifecycle == Lifecycle::STOPPING) { lifecycle = Lifecycle::STOPPED; } lifecycle_condition.notify_all(); @@ -271,12 +235,9 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th lifecycle = Lifecycle::STOPPING; ring.close(); lock.unlock(); - const bool stopped = invokeStop(); + invokeStop(); lock.lock(); - stop_unconfirmed = !stopped; - if (stopped) { - lifecycle = Lifecycle::STOPPED; - } + lifecycle = Lifecycle::STOPPED; lifecycle_condition.notify_all(); } @@ -301,17 +262,16 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th lifecycle = Lifecycle::STOPPING; lock.unlock(); - const bool stopped = invokeStop(); + invokeStop(); lock.lock(); - stop_unconfirmed = !stopped; - if (stopped && lifecycle == Lifecycle::STOPPING) { + if (lifecycle == Lifecycle::STOPPING) { lifecycle = Lifecycle::STOPPED; } lifecycle_condition.notify_all(); - return stopped; + return true; } - bool shutdown() { + void shutdown() { std::unique_lock lock(lifecycle_mutex); registered = false; ring.close(); @@ -320,41 +280,25 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th start_attempt->cancel_requested.store(true, std::memory_order_release); } lifecycle_condition.notify_all(); - return false; + return; } if (lifecycle == Lifecycle::STOPPING) { - if (stop_unconfirmed) { - lock.unlock(); - const bool stopped = invokeStop(); - lock.lock(); - stop_unconfirmed = !stopped; - if (stopped) { - lifecycle = Lifecycle::STOPPED; - } - lifecycle_condition.notify_all(); - return stopped; - } - lifecycle_condition.wait(lock, [this] { - return lifecycle != Lifecycle::STOPPING; - }); lifecycle_condition.notify_all(); - return lifecycle == Lifecycle::STOPPED && !stop_unconfirmed; + return; } if (lifecycle == Lifecycle::STOPPED) { lifecycle_condition.notify_all(); - return !stop_unconfirmed; + return; } lifecycle = Lifecycle::STOPPING; lock.unlock(); - const bool stopped = invokeStop(); + invokeStop(); lock.lock(); - stop_unconfirmed = !stopped; - if (stopped && lifecycle == Lifecycle::STOPPING) { + if (lifecycle == Lifecycle::STOPPING) { lifecycle = Lifecycle::STOPPED; } lifecycle_condition.notify_all(); - return stopped; } bool validForSubscription() const { @@ -390,11 +334,9 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th } const std::string track_id; - const std::string source_id; mutable TrackDescriptorPtr descriptor; const SourceCallbacks callbacks; FrameRing ring; - service::StopAllAdmissionGate* const admission_gate; mutable std::mutex callback_mutex; mutable std::mutex lifecycle_mutex; @@ -402,43 +344,33 @@ struct MediaSourceManager::SourceState final : public std::enable_shared_from_th Lifecycle lifecycle{Lifecycle::STOPPED}; size_t subscriber_count{0}; bool registered{true}; - bool stop_unconfirmed{false}; std::shared_ptr start_attempt; }; -struct MediaSourceManager::Impl final { - explicit Impl(service::StopAllAdmissionGate* source_admission_gate) - : admission_gate(source_admission_gate) {} - +struct MediaSourceHub::Impl final { mutable std::mutex mutex; - std::condition_variable stop_condition; - bool stop_all_in_progress{false}; - std::unordered_set device_stops_in_progress; - std::unordered_set track_stops_in_progress; - std::unordered_map tracked_source_counts; std::unordered_map> sources; - service::StopAllAdmissionGate* const admission_gate; }; -MediaSourceManager::Subscription::Subscription( +MediaSourceHub::Subscription::Subscription( std::shared_ptr source, FrameRing::Cursor cursor) : source_(std::move(source)), cursor_(std::move(cursor)), active_(static_cast(source_)) {} -MediaSourceManager::Subscription::~Subscription() { +MediaSourceHub::Subscription::~Subscription() { reset(); } -MediaSourceManager::Subscription::Subscription(Subscription&& other) noexcept +MediaSourceHub::Subscription::Subscription(Subscription&& other) noexcept : source_(std::move(other.source_)), cursor_(other.cursor_), active_(other.active_) { other.active_ = false; } -MediaSourceManager::Subscription& MediaSourceManager::Subscription::operator=(Subscription&& other) noexcept { +MediaSourceHub::Subscription& MediaSourceHub::Subscription::operator=(Subscription&& other) noexcept { if (this == &other) { return *this; } @@ -450,22 +382,22 @@ MediaSourceManager::Subscription& MediaSourceManager::Subscription::operator=(Su return *this; } -bool MediaSourceManager::Subscription::valid() const { +bool MediaSourceHub::Subscription::valid() const { return active_ && source_ && source_->validForSubscription(); } -TrackDescriptorPtr MediaSourceManager::Subscription::descriptor() const { +TrackDescriptorPtr MediaSourceHub::Subscription::descriptor() const { return source_ ? source_->currentDescriptor() : nullptr; } -std::optional MediaSourceManager::Subscription::tryRead() { +std::optional MediaSourceHub::Subscription::tryRead() { if (!active_ || !source_) { return std::nullopt; } return source_->ring.tryRead(cursor_); } -std::optional MediaSourceManager::Subscription::waitRead( +std::optional MediaSourceHub::Subscription::waitRead( const std::chrono::milliseconds timeout) { if (!active_ || !source_) { return std::nullopt; @@ -473,7 +405,7 @@ std::optional MediaSourceManager::Subscript return source_->ring.waitRead(cursor_, timeout); } -uint64_t MediaSourceManager::Subscription::discardPendingIfExceeds( +uint64_t MediaSourceHub::Subscription::discardPendingIfExceeds( const size_t maximum_pending_frames) { if (!active_ || !source_) { return 0; @@ -481,11 +413,11 @@ uint64_t MediaSourceManager::Subscription::discardPendingIfExceeds( return source_->ring.discardPendingIfExceeds(cursor_, maximum_pending_frames); } -uint64_t MediaSourceManager::Subscription::droppedCount() const noexcept { +uint64_t MediaSourceHub::Subscription::droppedCount() const noexcept { return cursor_.dropped_count; } -void MediaSourceManager::Subscription::reset() { +void MediaSourceHub::Subscription::reset() { if (active_ && source_) { source_->release(); } @@ -493,15 +425,14 @@ void MediaSourceManager::Subscription::reset() { source_.reset(); } -MediaSourceManager::MediaSourceManager( - service::StopAllAdmissionGate* admission_gate) - : impl_(std::make_shared(admission_gate)) {} +MediaSourceHub::MediaSourceHub() + : impl_(std::make_shared()) {} -MediaSourceManager::~MediaSourceManager() { +MediaSourceHub::~MediaSourceHub() { shutdown(); } -bool MediaSourceManager::registerSource( +bool MediaSourceHub::registerSource( TrackDescriptorPtr initial_descriptor, SourceCallbacks callbacks, const size_t ring_capacity) { @@ -513,50 +444,16 @@ bool MediaSourceManager::registerSource( std::shared_ptr source; try { source = std::make_shared( - std::move(initial_descriptor), std::move(callbacks), ring_capacity, - impl_->admission_gate); + std::move(initial_descriptor), std::move(callbacks), ring_capacity); } catch (...) { return false; } - std::optional admission; - std::unique_lock lock(impl_->mutex, std::defer_lock); - for (;;) { - if (impl_->admission_gate) { - admission.emplace(impl_->admission_gate->lockAdmission()); - } - lock.lock(); - if (admission && !admission->accepting()) { - return false; - } - const bool can_register = - !impl_->stop_all_in_progress && - impl_->device_stops_in_progress.count(source->source_id) == 0U && - impl_->track_stops_in_progress.count(source->track_id) == 0U; - if (can_register) { - break; - } - - // Hub-local stops may invoke arbitrary device callbacks. Wait for - // them without delaying process-wide StopAll admission. - admission.reset(); - impl_->stop_condition.wait(lock, [this, &source] { - return !impl_->stop_all_in_progress && - impl_->device_stops_in_progress.count(source->source_id) == 0U && - impl_->track_stops_in_progress.count(source->track_id) == 0U; - }); - lock.unlock(); - } - const std::string source_id = source->source_id; - const bool inserted = - impl_->sources.emplace(source->track_id, std::move(source)).second; - if (inserted) { - ++impl_->tracked_source_counts[source_id]; - } - return inserted; + std::lock_guard lock(impl_->mutex); + return impl_->sources.emplace(source->track_id, std::move(source)).second; } -bool MediaSourceManager::unregisterSource(const std::string& track_id) { +bool MediaSourceHub::unregisterSource(const std::string& track_id) { if (!impl_ || track_id.empty()) { return false; } @@ -578,19 +475,13 @@ bool MediaSourceManager::unregisterSource(const std::string& track_id) { std::lock_guard lock(impl_->mutex); const auto it = impl_->sources.find(track_id); if (it != impl_->sources.end() && it->second == source) { - const std::string source_id = source->source_id; impl_->sources.erase(it); - const auto count_it = impl_->tracked_source_counts.find(source_id); - if (count_it != impl_->tracked_source_counts.end() && - --count_it->second == 0U) { - impl_->tracked_source_counts.erase(count_it); - } return true; } return false; } -bool MediaSourceManager::hasSource(const std::string& track_id) const { +bool MediaSourceHub::hasSource(const std::string& track_id) const { if (!impl_) { return false; } @@ -598,7 +489,7 @@ bool MediaSourceManager::hasSource(const std::string& track_id) const { return impl_->sources.find(track_id) != impl_->sources.end(); } -std::vector MediaSourceManager::listTracks() const { +std::vector MediaSourceHub::listTracks() const { std::vector> sources; if (!impl_) { return {}; @@ -625,26 +516,7 @@ std::vector MediaSourceManager::listTracks() const { return descriptors; } -std::vector MediaSourceManager::trackedSourceIds() const { - if (!impl_) { - return {}; - } - - std::vector source_ids; - { - std::lock_guard lock(impl_->mutex); - source_ids.reserve(impl_->tracked_source_counts.size()); - for (const auto& [source_id, count] : impl_->tracked_source_counts) { - if (count != 0U) { - source_ids.push_back(source_id); - } - } - } - std::sort(source_ids.begin(), source_ids.end()); - return source_ids; -} - -size_t MediaSourceManager::subscriberCount(const std::string& track_id) const { +size_t MediaSourceHub::subscriberCount(const std::string& track_id) const { if (!impl_) { return 0; } @@ -660,7 +532,7 @@ size_t MediaSourceManager::subscriberCount(const std::string& track_id) const { return source->subscriberCount(); } -bool MediaSourceManager::requestKeyFrame(const std::string& track_id) const { +bool MediaSourceHub::requestKeyFrame(const std::string& track_id) const { if (!impl_) { return false; } @@ -676,7 +548,7 @@ bool MediaSourceManager::requestKeyFrame(const std::string& track_id) const { return source->requestKeyFrame(); } -MediaSourceManager::Subscription MediaSourceManager::subscribe( +MediaSourceHub::Subscription MediaSourceHub::subscribe( const std::string& track_id, const StartPosition start_position, CancelPredicate cancelled) { @@ -701,103 +573,24 @@ MediaSourceManager::Subscription MediaSourceManager::subscribe( return Subscription(std::move(source), std::move(cursor)); } -bool MediaSourceManager::stopSourcesForDevice( - const std::string& source_id, - std::vector* failures) { - if (source_id.empty()) { - if (failures) { - failures->clear(); - } - return true; - } - return stopSources(source_id, failures); -} - -bool MediaSourceManager::stopAllSources(std::vector* failures) { - return stopSources(std::nullopt, failures); -} - -bool MediaSourceManager::stopSources( - const std::optional& source_id, - std::vector* failures) { - if (failures) { - failures->clear(); - } +void MediaSourceHub::shutdown() { if (!impl_) { - return true; - } - - std::unordered_map> sources; - { - std::unique_lock lock(impl_->mutex); - if (source_id) { - impl_->stop_condition.wait(lock, [this, &source_id] { - return !impl_->stop_all_in_progress && - impl_->device_stops_in_progress.count(*source_id) == 0U; - }); - impl_->device_stops_in_progress.insert(*source_id); - for (auto source_it = impl_->sources.begin(); - source_it != impl_->sources.end();) { - if (source_it->second->source_id != *source_id) { - ++source_it; - continue; - } - impl_->track_stops_in_progress.insert(source_it->first); - sources.emplace(source_it->first, std::move(source_it->second)); - source_it = impl_->sources.erase(source_it); - } - } else { - impl_->stop_condition.wait(lock, [this] { - return !impl_->stop_all_in_progress && - impl_->device_stops_in_progress.empty(); - }); - impl_->stop_all_in_progress = true; - sources.swap(impl_->sources); - } - } - - std::unordered_map> quarantined; - for (const auto& [track_id, source] : sources) { - if (!source->shutdown()) { - quarantined.emplace(track_id, source); - if (failures) { - failures->push_back(track_id); - } - } + return; } + std::vector> sources; { std::lock_guard lock(impl_->mutex); - for (auto& [track_id, source] : quarantined) { - impl_->sources.emplace(track_id, std::move(source)); - } - for (const auto& [track_id, source] : sources) { - if (quarantined.count(track_id) != 0U) { - continue; - } - const auto count_it = - impl_->tracked_source_counts.find(source->source_id); - if (count_it != impl_->tracked_source_counts.end() && - --count_it->second == 0U) { - impl_->tracked_source_counts.erase(count_it); - } - } - if (source_id) { - for (const auto& [track_id, source] : sources) { - (void)source; - impl_->track_stops_in_progress.erase(track_id); - } - impl_->device_stops_in_progress.erase(*source_id); - } else { - impl_->stop_all_in_progress = false; + 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(); } - impl_->stop_condition.notify_all(); - return quarantined.empty(); -} - -void MediaSourceManager::shutdown() { - (void)stopAllSources(); } } // 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 new file mode 100644 index 00000000..b96cea9b --- /dev/null +++ b/cmvr-es/manager/media_source_hub/tests/media_source_hub_test.cpp @@ -0,0 +1,683 @@ +#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/media_source_manager/CMakeLists.txt b/cmvr-es/manager/media_source_manager/CMakeLists.txt deleted file mode 100644 index b5369f7c..00000000 --- a/cmvr-es/manager/media_source_manager/CMakeLists.txt +++ /dev/null @@ -1,97 +0,0 @@ -if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) - cmake_minimum_required(VERSION 3.22) - project(cmvr_media_source_manager LANGUAGES CXX) - enable_testing() - add_subdirectory( - ${CMAKE_CURRENT_SOURCE_DIR}/../../service/grpc/stop_all - ${CMAKE_CURRENT_BINARY_DIR}/stop_all - ) -endif() - -add_library(media_source_manager STATIC - src/media_source_manager.cpp -) - -target_compile_features(media_source_manager PUBLIC cxx_std_17) -target_include_directories(media_source_manager - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../.. -) -target_link_libraries(media_source_manager - PUBLIC - cmvr_es::stop_all_admission_gate -) - -add_library(cmvr_es::media_source_manager ALIAS media_source_manager) - -if(NOT CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) - add_library(device_media_source_adapter STATIC - src/device_media_source_adapter.cpp - ) - target_compile_features(device_media_source_adapter PUBLIC cxx_std_17) - target_include_directories(device_media_source_adapter - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../.. - ) - target_link_libraries(device_media_source_adapter - PUBLIC - cmvr_es::media_source_manager - cmvr_es::common - cmvr_es::proto - cmvr_es::logging - cmvr_es::safety_manager - ) - add_library(cmvr_es::device_media_source_adapter ALIAS device_media_source_adapter) - - if(BUILD_TESTING) - add_executable(device_media_source_adapter_test - tests/device_media_source_adapter_test.cpp - ) - target_compile_features(device_media_source_adapter_test PRIVATE cxx_std_17) - target_link_libraries(device_media_source_adapter_test - PRIVATE - cmvr_es::device_media_source_adapter - gtest - gtest_main - ) - add_test( - NAME device_media_source_adapter_test - COMMAND device_media_source_adapter_test - ) - set(_device_media_source_adapter_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _device_media_source_adapter_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(device_media_source_adapter_test PROPERTIES - ENVIRONMENT - "${_device_media_source_adapter_test_environment}" - ) - endif() -endif() - -option(CMVR_MEDIA_SOURCE_MANAGER_BUILD_TESTS - "Build the standalone MediaSourceManager self-test" - ${PROJECT_IS_TOP_LEVEL}) - -if(CMVR_MEDIA_SOURCE_MANAGER_BUILD_TESTS) - find_package(Threads REQUIRED) - add_executable(media_source_manager_test - tests/media_source_manager_test.cpp - ) - target_compile_features(media_source_manager_test PRIVATE cxx_std_17) - target_link_libraries(media_source_manager_test - PRIVATE - cmvr_es::media_source_manager - 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_manager_test PROPERTIES - SKIP_BUILD_RPATH TRUE - ) - add_test(NAME media_source_manager_test COMMAND media_source_manager_test) -endif() diff --git a/cmvr-es/manager/media_source_manager/tests/device_media_source_adapter_test.cpp b/cmvr-es/manager/media_source_manager/tests/device_media_source_adapter_test.cpp deleted file mode 100644 index 0c007166..00000000 --- a/cmvr-es/manager/media_source_manager/tests/device_media_source_adapter_test.cpp +++ /dev/null @@ -1,113 +0,0 @@ -#include "manager/media_source_manager/include/device_media_source_adapter.h" - -#include -#include -#include -#include - -#include - -#include "manager/safety_manager/include/device_safety_endpoint.h" - -namespace cmvr::media { -namespace { - -class FakeSensorEndpoint final : public safety::DeviceSafetyEndpoint { -public: - explicit FakeSensorEndpoint(std::string device_id) - { - descriptor_.device_id = std::move(device_id); - descriptor_.kind = device::DeviceKind::Camera; - descriptor_.default_policy = safety::SafetyPolicyFamily::Sensor; - descriptor_.maximum_snapshot_age = std::chrono::seconds(1); - descriptor_.supports_active_refresh = true; - } - - safety::DeviceSafetyDescriptor descriptor() const override - { - return descriptor_; - } - - void bindPublisher(safety::SafetySnapshotPublisher publisher) override - { - publisher_ = std::move(publisher); - } - - void requestSafetyRefresh() noexcept override - { - if (!publisher_) { - return; - } - safety::DeviceSafetySnapshot snapshot; - snapshot.device_id = descriptor_.device_id; - snapshot.condition = safety::SafetyCondition::Nominal; - snapshot.device_generation = 1; - snapshot.sample_sequence = ++sequence_; - snapshot.observed_at = safety::SafetyClock::now(); - snapshot.connected = safety::TriState::True; - snapshot.operational_ready = safety::TriState::True; - snapshot.quiescent = safety::TriState::True; - snapshot.motion_active = safety::TriState::False; - snapshot.actuator_enabled = safety::TriState::False; - snapshot.emergency_stop_active = safety::TriState::False; - snapshot.protective_stop_active = safety::TriState::False; - snapshot.fault_active = safety::TriState::False; - (void)publisher_(std::move(snapshot)); - } - - safety::HardwareCheckResult validateBeforeDispatch( - const safety::AdmissionPermit& permit) override - { - ++hardware_checks; - last_intent = permit.intent; - return {true, safety::SafetyReason::None, {}}; - } - - safety::RecoveryCheckResult reconcileAdmissionState( - const safety::RecoveryContext&) override - { - return {true, safety::SafetyReason::None, {}}; - } - - std::atomic hardware_checks{0}; - safety::CommandIntent last_intent{safety::CommandIntent::Observe}; - -private: - safety::DeviceSafetyDescriptor descriptor_; - safety::SafetySnapshotPublisher publisher_; - std::atomic sequence_{0}; -}; - -TEST(DeviceMediaSourceAdapterTest, - SensorStartUsesFinalCheckAndQuarantineRejectsRestart) -{ - safety::SafetyManagerConfig config; - config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyManager coordinator(config); - auto endpoint = std::make_shared("camera"); - ASSERT_TRUE(coordinator.registerDevice( - {endpoint->descriptor(), endpoint, {}})); - endpoint->requestSafetyRefresh(); - coordinator.updateDeviceRuntimeState( - "camera", - device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - { - auto dispatch = beginMediaSourceStartDispatch(coordinator, "camera"); - ASSERT_TRUE(dispatch.acquired()); - EXPECT_EQ(endpoint->hardware_checks.load(), 1); - EXPECT_EQ(endpoint->last_intent, safety::CommandIntent::StartActivity); - } - - coordinator.quarantineDevice( - "camera", safety::SafetyReason::OutcomeUnknown, - "uncertain-media-start"); - auto rejected = beginMediaSourceStartDispatch(coordinator, "camera"); - EXPECT_FALSE(rejected.acquired()); - EXPECT_EQ(endpoint->hardware_checks.load(), 1); -} - -} // namespace -} // namespace cmvr::media diff --git a/cmvr-es/manager/media_source_manager/tests/media_source_manager_test.cpp b/cmvr-es/manager/media_source_manager/tests/media_source_manager_test.cpp deleted file mode 100644 index a7fd3907..00000000 --- a/cmvr-es/manager/media_source_manager/tests/media_source_manager_test.cpp +++ /dev/null @@ -1,1338 +0,0 @@ -#include "manager/media_source_manager/include/media_source_manager.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.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::MediaSourceManager; -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 = {}, - std::string track_id = "camera.front.video", - std::string source_id = "camera.front") { - TrackDescriptor::Config config; - config.id = std::move(track_id); - config.source_id = std::move(source_id); - 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() { - MediaSourceManager 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; - MediaSourceManager::FrameSink sink; - - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceManager::FrameSink& callback_sink, - const MediaSourceManager::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}); - MediaSourceManager::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() { - MediaSourceManager hub; - const auto descriptor = makeVideoDescriptor(Codec::H264, 1); - std::mutex sink_mutex; - MediaSourceManager::FrameSink sink; - - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceManager::FrameSink& callback_sink, - const MediaSourceManager::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()); - - MediaSourceManager::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() { - MediaSourceManager hub; - const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); - std::atomic start_attempts{0}; - std::atomic retry_stop_count{0}; - MediaSourceManager::SourceCallbacks failed_callbacks; - failed_callbacks.start = [&](const MediaSourceManager::FrameSink&, - const MediaSourceManager::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}; - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = [](const MediaSourceManager::FrameSink&, - const MediaSourceManager::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 testHubStopAllSourcesAllowsReregistration() { - MediaSourceManager hub; - const auto first_descriptor = makeVideoDescriptor(Codec::H264, 1); - std::atomic first_stop_count{0}; - - MediaSourceManager::SourceCallbacks first_callbacks; - first_callbacks.start = [](const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { - return true; - }; - first_callbacks.stop = [&] { ++first_stop_count; }; - CHECK_TRUE(hub.registerSource(first_descriptor, std::move(first_callbacks), 2)); - auto old_subscription = hub.subscribe(first_descriptor->id); - CHECK_TRUE(old_subscription.valid()); - - auto blocked_read = std::async(std::launch::async, [&] { - return old_subscription.waitRead(2s); - }); - CHECK_TRUE(hub.stopAllSources()); - - CHECK_TRUE(first_stop_count.load(std::memory_order_acquire) == 1); - CHECK_TRUE(!hub.hasSource(first_descriptor->id)); - CHECK_TRUE(!old_subscription.valid()); - CHECK_TRUE(blocked_read.wait_for(500ms) == std::future_status::ready); - if (blocked_read.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(!blocked_read.get().has_value()); - } - - const auto second_descriptor = makeVideoDescriptor(Codec::H264, 2); - std::atomic second_start_count{0}; - std::atomic second_stop_count{0}; - MediaSourceManager::SourceCallbacks second_callbacks; - second_callbacks.start = [&](const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { - ++second_start_count; - return true; - }; - second_callbacks.stop = [&] { ++second_stop_count; }; - - CHECK_TRUE(hub.registerSource(second_descriptor, std::move(second_callbacks), 2)); - auto new_subscription = hub.subscribe(second_descriptor->id); - CHECK_TRUE(new_subscription.valid()); - CHECK_TRUE(new_subscription.descriptor()->generation == 2); - CHECK_TRUE(second_start_count.load(std::memory_order_acquire) == 1); - - old_subscription.reset(); - CHECK_TRUE(second_stop_count.load(std::memory_order_acquire) == 0); - new_subscription.reset(); - CHECK_TRUE(second_stop_count.load(std::memory_order_acquire) == 1); -} - -void testStopAllSourcesReportsAndRetriesUnconfirmedStop() { - MediaSourceManager hub; - const auto descriptor = makeVideoDescriptor(Codec::H264, 1); - std::atomic stop_attempts{0}; - - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = [](const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { - return true; - }; - callbacks.stop_confirmed = [&] { - return ++stop_attempts >= 2; - }; - CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 2)); - auto subscription = hub.subscribe(descriptor->id); - CHECK_TRUE(subscription.valid()); - - std::vector stop_failures; - CHECK_TRUE(!hub.stopAllSources(&stop_failures)); - CHECK_TRUE(stop_failures.size() == 1); - CHECK_TRUE(stop_failures.front() == descriptor->id); - CHECK_TRUE(hub.hasSource(descriptor->id)); - CHECK_TRUE(!subscription.valid()); - - stop_failures.clear(); - CHECK_TRUE(hub.stopAllSources(&stop_failures)); - CHECK_TRUE(stop_failures.empty()); - CHECK_TRUE(!hub.hasSource(descriptor->id)); - CHECK_TRUE(stop_attempts.load(std::memory_order_acquire) == 2); -} - -void testStopSourcesForDeviceIsSelectiveAndRetriesFailures() { - MediaSourceManager hub; - const auto front_video = makeVideoDescriptor( - Codec::H264, 1, {}, "front.video", "camera.front"); - const auto front_depth = makeVideoDescriptor( - Codec::H264, 1, {}, "front.depth", "camera.front"); - const auto rear_video = makeVideoDescriptor( - Codec::H264, 1, {}, "rear.video", "camera.rear"); - - std::atomic front_video_stops{0}; - std::atomic front_depth_stops{0}; - std::atomic rear_stops{0}; - auto register_source = [&]( - const TrackDescriptorPtr& descriptor, - std::function stop_confirmed) { - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = []( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { return true; }; - callbacks.stop_confirmed = std::move(stop_confirmed); - return hub.registerSource(descriptor, std::move(callbacks), 2); - }; - - CHECK_TRUE(register_source(front_video, [&] { - return ++front_video_stops >= 2; - })); - CHECK_TRUE(register_source(front_depth, [&] { - ++front_depth_stops; - return true; - })); - CHECK_TRUE(register_source(rear_video, [&] { - ++rear_stops; - return true; - })); - CHECK_TRUE( - hub.trackedSourceIds() == - (std::vector{"camera.front", "camera.rear"})); - - auto front_video_subscription = hub.subscribe(front_video->id); - auto front_depth_subscription = hub.subscribe(front_depth->id); - auto rear_subscription = hub.subscribe(rear_video->id); - CHECK_TRUE(front_video_subscription.valid()); - CHECK_TRUE(front_depth_subscription.valid()); - CHECK_TRUE(rear_subscription.valid()); - - std::vector stop_failures; - CHECK_TRUE(!hub.stopSourcesForDevice("camera.front", &stop_failures)); - CHECK_TRUE(stop_failures.size() == 1U); - CHECK_TRUE(stop_failures.front() == front_video->id); - CHECK_TRUE(hub.hasSource(front_video->id)); - CHECK_TRUE(!hub.hasSource(front_depth->id)); - CHECK_TRUE(hub.hasSource(rear_video->id)); - CHECK_TRUE( - hub.trackedSourceIds() == - (std::vector{"camera.front", "camera.rear"})); - CHECK_TRUE(!front_video_subscription.valid()); - CHECK_TRUE(!front_depth_subscription.valid()); - CHECK_TRUE(rear_subscription.valid()); - CHECK_TRUE(rear_stops.load(std::memory_order_acquire) == 0); - - CHECK_TRUE(hub.stopSourcesForDevice("camera.front", &stop_failures)); - CHECK_TRUE(stop_failures.empty()); - CHECK_TRUE(!hub.hasSource(front_video->id)); - CHECK_TRUE(hub.hasSource(rear_video->id)); - CHECK_TRUE( - hub.trackedSourceIds() == - (std::vector{"camera.rear"})); - CHECK_TRUE(front_video_stops.load(std::memory_order_acquire) == 2); - CHECK_TRUE(front_depth_stops.load(std::memory_order_acquire) == 1); - CHECK_TRUE(rear_stops.load(std::memory_order_acquire) == 0); - - rear_subscription.reset(); - CHECK_TRUE(rear_stops.load(std::memory_order_acquire) == 1); -} - -void testDeviceStopsRunConcurrentlyAndSerializeMatchingRegistration() { - MediaSourceManager hub; - const auto first = makeVideoDescriptor( - Codec::H264, 1, {}, "first.video", "camera.first"); - const auto second = makeVideoDescriptor( - Codec::H264, 1, {}, "second.video", "camera.second"); - - std::atomic first_stop_entered{false}; - std::atomic second_stop_entered{false}; - std::atomic release_stops{false}; - auto register_blocking_source = [&]( - const TrackDescriptorPtr& descriptor, - std::atomic& entered) { - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = []( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { return true; }; - callbacks.stop = [&entered, &release_stops] { - entered.store(true, std::memory_order_release); - while (!release_stops.load(std::memory_order_acquire)) { - std::this_thread::sleep_for(1ms); - } - }; - return hub.registerSource(descriptor, std::move(callbacks), 2); - }; - - CHECK_TRUE(register_blocking_source(first, first_stop_entered)); - CHECK_TRUE(register_blocking_source(second, second_stop_entered)); - auto first_subscription = hub.subscribe(first->id); - auto second_subscription = hub.subscribe(second->id); - CHECK_TRUE(first_subscription.valid()); - CHECK_TRUE(second_subscription.valid()); - - auto first_stop = std::async(std::launch::async, [&] { - return hub.stopSourcesForDevice(first->source_id); - }); - const auto first_deadline = std::chrono::steady_clock::now() + 500ms; - while (!first_stop_entered.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < first_deadline) { - std::this_thread::sleep_for(1ms); - } - CHECK_TRUE(first_stop_entered.load(std::memory_order_acquire)); - CHECK_TRUE( - hub.trackedSourceIds() == - (std::vector{"camera.first", "camera.second"})); - - auto second_stop = std::async(std::launch::async, [&] { - return hub.stopSourcesForDevice(second->source_id); - }); - const auto second_deadline = std::chrono::steady_clock::now() + 500ms; - while (!second_stop_entered.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < second_deadline) { - std::this_thread::sleep_for(1ms); - } - CHECK_TRUE(second_stop_entered.load(std::memory_order_acquire)); - - MediaSourceManager::SourceCallbacks replacement_callbacks; - replacement_callbacks.start = []( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { return true; }; - replacement_callbacks.stop = [] {}; - auto matching_registration = std::async(std::launch::async, [&] { - return hub.registerSource( - makeVideoDescriptor( - Codec::H264, - 2, - {}, - "first.replacement", - "camera.first"), - std::move(replacement_callbacks), - 2); - }); - CHECK_TRUE( - matching_registration.wait_for(20ms) == std::future_status::timeout); - - release_stops.store(true, std::memory_order_release); - CHECK_TRUE(first_stop.wait_for(500ms) == std::future_status::ready); - CHECK_TRUE(second_stop.wait_for(500ms) == std::future_status::ready); - if (first_stop.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(first_stop.get()); - } - if (second_stop.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(second_stop.get()); - } - CHECK_TRUE( - matching_registration.wait_for(500ms) == std::future_status::ready); - if (matching_registration.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(matching_registration.get()); - } - CHECK_TRUE(hub.hasSource("first.replacement")); - CHECK_TRUE( - hub.trackedSourceIds() == - (std::vector{"camera.first"})); -} - -void testStopAllWaitsForDeviceStopAndRetainsItsConcurrentRegistrationRule() { - MediaSourceManager hub; - const auto first = makeVideoDescriptor( - Codec::H264, 1, {}, "first.video", "camera.first"); - const auto other = makeVideoDescriptor( - Codec::H264, 1, {}, "other.video", "camera.other"); - std::atomic first_stop_entered{false}; - std::atomic release_first_stop{false}; - std::atomic other_stops{0}; - - MediaSourceManager::SourceCallbacks first_callbacks; - first_callbacks.start = []( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { return true; }; - first_callbacks.stop = [&] { - first_stop_entered.store(true, std::memory_order_release); - while (!release_first_stop.load(std::memory_order_acquire)) { - std::this_thread::sleep_for(1ms); - } - }; - CHECK_TRUE(hub.registerSource(first, std::move(first_callbacks), 2)); - auto first_subscription = hub.subscribe(first->id); - CHECK_TRUE(first_subscription.valid()); - - auto device_stop = std::async(std::launch::async, [&] { - return hub.stopSourcesForDevice(first->source_id); - }); - const auto stop_deadline = std::chrono::steady_clock::now() + 500ms; - while (!first_stop_entered.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < stop_deadline) { - std::this_thread::sleep_for(1ms); - } - CHECK_TRUE(first_stop_entered.load(std::memory_order_acquire)); - - auto stop_all = std::async(std::launch::async, [&] { - return hub.stopAllSources(); - }); - CHECK_TRUE(stop_all.wait_for(20ms) == std::future_status::timeout); - - MediaSourceManager::SourceCallbacks other_callbacks; - other_callbacks.start = []( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { return true; }; - other_callbacks.stop = [&] { ++other_stops; }; - CHECK_TRUE(hub.registerSource(other, std::move(other_callbacks), 2)); - auto other_subscription = hub.subscribe(other->id); - CHECK_TRUE(other_subscription.valid()); - - release_first_stop.store(true, std::memory_order_release); - CHECK_TRUE(device_stop.wait_for(500ms) == std::future_status::ready); - if (device_stop.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(device_stop.get()); - } - CHECK_TRUE(stop_all.wait_for(500ms) == std::future_status::ready); - if (stop_all.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(stop_all.get()); - } - CHECK_TRUE(!hub.hasSource(first->id)); - CHECK_TRUE(!hub.hasSource(other->id)); - CHECK_TRUE(hub.trackedSourceIds().empty()); - CHECK_TRUE(!other_subscription.valid()); - CHECK_TRUE(other_stops.load(std::memory_order_acquire) == 1); -} - -void testConcurrentRegistrationWaitsForStopAllSources() { - MediaSourceManager hub; - const auto descriptor = makeVideoDescriptor(Codec::H264, 1); - std::atomic stop_entered{false}; - std::atomic release_stop{false}; - std::atomic old_stop_count{0}; - - MediaSourceManager::SourceCallbacks old_callbacks; - old_callbacks.start = [](const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { - return true; - }; - old_callbacks.stop = [&] { - stop_entered.store(true, std::memory_order_release); - while (!release_stop.load(std::memory_order_acquire)) { - std::this_thread::sleep_for(1ms); - } - ++old_stop_count; - }; - CHECK_TRUE(hub.registerSource(descriptor, std::move(old_callbacks), 2)); - auto old_subscription = hub.subscribe(descriptor->id); - CHECK_TRUE(old_subscription.valid()); - - auto stop_all = std::async( - std::launch::async, [&] { return hub.stopAllSources(); }); - const auto stop_deadline = std::chrono::steady_clock::now() + 500ms; - while (!stop_entered.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < stop_deadline) { - std::this_thread::sleep_for(1ms); - } - CHECK_TRUE(stop_entered.load(std::memory_order_acquire)); - - std::atomic new_start_count{0}; - MediaSourceManager::SourceCallbacks new_callbacks; - new_callbacks.start = [&](const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { - ++new_start_count; - return true; - }; - new_callbacks.stop = [] {}; - auto registration = std::async(std::launch::async, [&] { - return hub.registerSource( - makeVideoDescriptor(Codec::H264, 2), std::move(new_callbacks), 2); - }); - - CHECK_TRUE(registration.wait_for(20ms) == std::future_status::timeout); - release_stop.store(true, std::memory_order_release); - CHECK_TRUE(stop_all.wait_for(500ms) == std::future_status::ready); - if (stop_all.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(stop_all.get()); - } - CHECK_TRUE(registration.wait_for(500ms) == std::future_status::ready); - const bool registered = registration.wait_for(0ms) == std::future_status::ready && - registration.get(); - CHECK_TRUE(registered); - CHECK_TRUE(old_stop_count.load(std::memory_order_acquire) == 1); - CHECK_TRUE(hub.hasSource(descriptor->id)); - - auto new_subscription = hub.subscribe(descriptor->id); - CHECK_TRUE(new_subscription.valid()); - CHECK_TRUE(new_subscription.descriptor()->generation == 2); - CHECK_TRUE(new_start_count.load(std::memory_order_acquire) == 1); -} - -void testSystemStopAllAdmissionFencesRegistrationAndStartup() { - cmvr::service::StopAllAdmissionGate admission_gate; - MediaSourceManager hub(&admission_gate); - const auto dormant = makeVideoDescriptor( - Codec::H264, 1, {}, "dormant.video", "camera.dormant"); - const auto new_source = makeVideoDescriptor( - Codec::H264, 1, {}, "new.video", "camera.new"); - std::atomic dormant_starts{0}; - std::atomic new_starts{0}; - - MediaSourceManager::SourceCallbacks dormant_callbacks; - dormant_callbacks.start = [&]( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { - ++dormant_starts; - return true; - }; - dormant_callbacks.stop = [] {}; - CHECK_TRUE(hub.registerSource( - dormant, std::move(dormant_callbacks), 2)); - - const auto stop_ticket = admission_gate.beginStopAll(); - CHECK_TRUE(stop_ticket.valid()); - - MediaSourceManager::SourceCallbacks rejected_callbacks; - rejected_callbacks.start = [&]( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { - ++new_starts; - return true; - }; - rejected_callbacks.stop = [] {}; - CHECK_TRUE(!hub.registerSource( - new_source, std::move(rejected_callbacks), 2)); - auto rejected_subscription = hub.subscribe(dormant->id); - CHECK_TRUE(!rejected_subscription.valid()); - CHECK_TRUE(dormant_starts.load(std::memory_order_acquire) == 0); - CHECK_TRUE(new_starts.load(std::memory_order_acquire) == 0); - - CHECK_TRUE(admission_gate.finishStopAll(stop_ticket, true)); - - MediaSourceManager::SourceCallbacks recovered_callbacks; - recovered_callbacks.start = [&]( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { - ++new_starts; - return true; - }; - recovered_callbacks.stop = [] {}; - CHECK_TRUE(hub.registerSource( - new_source, std::move(recovered_callbacks), 2)); - - auto dormant_subscription = hub.subscribe(dormant->id); - auto new_subscription = hub.subscribe(new_source->id); - CHECK_TRUE(dormant_subscription.valid()); - CHECK_TRUE(new_subscription.valid()); - CHECK_TRUE(dormant_starts.load(std::memory_order_acquire) == 1); - CHECK_TRUE(new_starts.load(std::memory_order_acquire) == 1); -} - -void testSystemStopAllRejectsRegistrationWaitingForLocalStop() { - cmvr::service::StopAllAdmissionGate admission_gate; - MediaSourceManager hub(&admission_gate); - const auto old_source = makeVideoDescriptor( - Codec::H264, 1, {}, "old.video", "camera.shared"); - const auto replacement = makeVideoDescriptor( - Codec::H264, 2, {}, "replacement.video", "camera.shared"); - std::atomic stop_entered{false}; - std::atomic release_stop{false}; - - MediaSourceManager::SourceCallbacks old_callbacks; - old_callbacks.start = []( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { return true; }; - old_callbacks.stop = [&] { - stop_entered.store(true, std::memory_order_release); - while (!release_stop.load(std::memory_order_acquire)) { - std::this_thread::sleep_for(1ms); - } - }; - CHECK_TRUE(hub.registerSource( - old_source, std::move(old_callbacks), 2)); - auto old_subscription = hub.subscribe(old_source->id); - CHECK_TRUE(old_subscription.valid()); - - auto local_stop = std::async(std::launch::async, [&] { - return hub.stopSourcesForDevice(old_source->source_id); - }); - const auto stop_deadline = std::chrono::steady_clock::now() + 500ms; - while (!stop_entered.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < stop_deadline) { - std::this_thread::sleep_for(1ms); - } - CHECK_TRUE(stop_entered.load(std::memory_order_acquire)); - - auto make_replacement_callbacks = [] { - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = []( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { return true; }; - callbacks.stop = [] {}; - return callbacks; - }; - auto waiting_registration = std::async(std::launch::async, [&] { - return hub.registerSource( - replacement, make_replacement_callbacks(), 2); - }); - CHECK_TRUE( - waiting_registration.wait_for(20ms) == - std::future_status::timeout); - - const auto stop_ticket = admission_gate.beginStopAll(); - release_stop.store(true, std::memory_order_release); - CHECK_TRUE(local_stop.wait_for(500ms) == std::future_status::ready); - if (local_stop.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(local_stop.get()); - } - CHECK_TRUE( - waiting_registration.wait_for(500ms) == - std::future_status::ready); - if (waiting_registration.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(!waiting_registration.get()); - } - CHECK_TRUE(!hub.hasSource(replacement->id)); - - CHECK_TRUE(admission_gate.finishStopAll(stop_ticket, true)); - CHECK_TRUE(hub.registerSource( - replacement, make_replacement_callbacks(), 2)); - auto recovered = hub.subscribe(replacement->id); - CHECK_TRUE(recovered.valid()); -} - -void testSystemStopAllRejectsSubscriptionWaitingForLocalStop() { - cmvr::service::StopAllAdmissionGate admission_gate; - MediaSourceManager hub(&admission_gate); - const auto descriptor = makeVideoDescriptor( - Codec::H264, 1, {}, "waiting.video", "camera.waiting"); - std::atomic stop_entered{false}; - std::atomic release_stop{false}; - std::atomic starts{0}; - - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = [&]( - const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { - ++starts; - return true; - }; - callbacks.stop = [&] { - stop_entered.store(true, std::memory_order_release); - while (!release_stop.load(std::memory_order_acquire)) { - std::this_thread::sleep_for(1ms); - } - }; - CHECK_TRUE(hub.registerSource( - descriptor, std::move(callbacks), 2)); - auto active = hub.subscribe(descriptor->id); - CHECK_TRUE(active.valid()); - - auto local_stop = std::async(std::launch::async, [&] { - active.reset(); - }); - const auto stop_deadline = std::chrono::steady_clock::now() + 500ms; - while (!stop_entered.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < stop_deadline) { - std::this_thread::sleep_for(1ms); - } - CHECK_TRUE(stop_entered.load(std::memory_order_acquire)); - - auto waiting_subscription = std::async(std::launch::async, [&] { - return hub.subscribe(descriptor->id); - }); - CHECK_TRUE( - waiting_subscription.wait_for(20ms) == - std::future_status::timeout); - - const auto stop_ticket = admission_gate.beginStopAll(); - release_stop.store(true, std::memory_order_release); - CHECK_TRUE(local_stop.wait_for(500ms) == std::future_status::ready); - if (local_stop.wait_for(0ms) == std::future_status::ready) { - local_stop.get(); - } - CHECK_TRUE( - waiting_subscription.wait_for(500ms) == - std::future_status::ready); - if (waiting_subscription.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(!waiting_subscription.get().valid()); - } - CHECK_TRUE(starts.load(std::memory_order_acquire) == 1); - - CHECK_TRUE(admission_gate.finishStopAll(stop_ticket, true)); - auto recovered = hub.subscribe(descriptor->id); - CHECK_TRUE(recovered.valid()); - CHECK_TRUE(starts.load(std::memory_order_acquire) == 2); -} - -void testStopAllSourcesCancelsStartingSourceBeforeReuse() { - MediaSourceManager hub; - const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); - std::atomic old_start_entered{false}; - std::atomic release_old_start{false}; - std::atomic old_stop_count{0}; - - MediaSourceManager::SourceCallbacks old_callbacks; - old_callbacks.start = [&](const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate& cancelled) { - old_start_entered.store(true, std::memory_order_release); - while (!release_old_start.load(std::memory_order_acquire)) { - if (cancelled()) { - break; - } - std::this_thread::sleep_for(1ms); - } - // Deliberately report a late success to exercise the abandoned-start - // stop path after stopAllSources has unregistered this source. - return true; - }; - old_callbacks.stop = [&] { ++old_stop_count; }; - CHECK_TRUE(hub.registerSource(descriptor, std::move(old_callbacks), 2)); - - auto old_subscription = std::async(std::launch::async, [&] { - return hub.subscribe(descriptor->id); - }); - const auto start_deadline = std::chrono::steady_clock::now() + 500ms; - while (!old_start_entered.load(std::memory_order_acquire) && - std::chrono::steady_clock::now() < start_deadline) { - std::this_thread::sleep_for(1ms); - } - CHECK_TRUE(old_start_entered.load(std::memory_order_acquire)); - - CHECK_TRUE(!hub.stopAllSources()); - CHECK_TRUE(hub.hasSource(descriptor->id)); - CHECK_TRUE(old_subscription.wait_for(500ms) == std::future_status::ready); - if (old_subscription.wait_for(0ms) == std::future_status::ready) { - CHECK_TRUE(!old_subscription.get().valid()); - } - - release_old_start.store(true, std::memory_order_release); - const auto old_stop_deadline = std::chrono::steady_clock::now() + 500ms; - while (old_stop_count.load(std::memory_order_acquire) != 1 && - std::chrono::steady_clock::now() < old_stop_deadline) { - std::this_thread::sleep_for(1ms); - } - CHECK_TRUE(old_stop_count.load(std::memory_order_acquire) == 1); - CHECK_TRUE(hub.stopAllSources()); - - std::atomic new_start_count{0}; - MediaSourceManager::SourceCallbacks new_callbacks; - new_callbacks.start = [&](const MediaSourceManager::FrameSink&, - const MediaSourceManager::CancelPredicate&) { - ++new_start_count; - return true; - }; - new_callbacks.stop = [] {}; - CHECK_TRUE(hub.registerSource( - makeVideoDescriptor(Codec::H264, 2), std::move(new_callbacks), 2)); - auto new_subscription = hub.subscribe(descriptor->id); - CHECK_TRUE(new_subscription.valid()); - CHECK_TRUE(new_start_count.load(std::memory_order_acquire) == 1); - CHECK_TRUE(new_subscription.valid()); -} - -void testKeyFrameRequestIsOrderedBeforeStop() { - MediaSourceManager 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}; - - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = [](const MediaSourceManager::FrameSink&, - const MediaSourceManager::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() { - MediaSourceManager hub; - const auto descriptor = makeVideoDescriptor(Codec::UNKNOWN, 1); - std::atomic start_entered{false}; - std::atomic start_exited{false}; - - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceManager::FrameSink&, - const MediaSourceManager::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() { - MediaSourceManager 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}; - - MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = [&](const MediaSourceManager::FrameSink&, - const MediaSourceManager::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(); - testHubStopAllSourcesAllowsReregistration(); - testStopAllSourcesReportsAndRetriesUnconfirmedStop(); - testStopSourcesForDeviceIsSelectiveAndRetriesFailures(); - testDeviceStopsRunConcurrentlyAndSerializeMatchingRegistration(); - testStopAllWaitsForDeviceStopAndRetainsItsConcurrentRegistrationRule(); - testConcurrentRegistrationWaitsForStopAllSources(); - testSystemStopAllAdmissionFencesRegistrationAndStartup(); - testSystemStopAllRejectsRegistrationWaitingForLocalStop(); - testSystemStopAllRejectsSubscriptionWaitingForLocalStop(); - testStopAllSourcesCancelsStartingSourceBeforeReuse(); - testKeyFrameRequestIsOrderedBeforeStop(); - testHubCancelsBlockedStartWithoutBlockingShutdown(); - testHubQuarantinesNonCooperativeStart(); - - if (failures != 0) { - std::cerr << failures << " media_source_manager checks failed\n"; - return 1; - } - std::cout << "media_source_manager self-test passed\n"; - return 0; -} diff --git a/cmvr-es/manager/safety_manager/CMakeLists.txt b/cmvr-es/manager/safety_manager/CMakeLists.txt deleted file mode 100644 index f647fb1e..00000000 --- a/cmvr-es/manager/safety_manager/CMakeLists.txt +++ /dev/null @@ -1,65 +0,0 @@ -add_library(safety_manager STATIC - src/command_ledger.cpp - src/safety_manager.cpp - src/safety_reason.cpp - src/safety_snapshot_store.cpp -) - -target_include_directories(safety_manager PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} - ${CMAKE_SOURCE_DIR}/cmvr-es -) - -target_link_libraries(safety_manager PUBLIC - cmvr_es::control_authority_manager -) - -add_library(cmvr_es::safety_manager ALIAS safety_manager) -install(TARGETS safety_manager LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - add_executable(safety_snapshot_store_test - tests/safety_snapshot_store_test.cpp - ) - target_link_libraries(safety_snapshot_store_test PRIVATE - cmvr_es::safety_manager - gtest - gtest_main - pthread - ) - add_test( - NAME safety_snapshot_store_test - COMMAND safety_snapshot_store_test - ) - set_tests_properties(safety_snapshot_store_test PROPERTIES TIMEOUT 10) - - add_executable(command_ledger_test - tests/command_ledger_test.cpp - ) - target_link_libraries(command_ledger_test PRIVATE - cmvr_es::safety_manager - gtest - gtest_main - pthread - ) - add_test( - NAME command_ledger_test - COMMAND command_ledger_test - ) - set_tests_properties(command_ledger_test PROPERTIES TIMEOUT 10) - - add_executable(safety_manager_test - tests/safety_manager_test.cpp - ) - target_link_libraries(safety_manager_test PRIVATE - cmvr_es::safety_manager - gtest - gtest_main - pthread - ) - add_test( - NAME safety_manager_test - COMMAND safety_manager_test - ) - set_tests_properties(safety_manager_test PROPERTIES TIMEOUT 15) -endif() diff --git a/cmvr-es/manager/safety_manager/include/command_ledger.h b/cmvr-es/manager/safety_manager/include/command_ledger.h deleted file mode 100644 index f47a7214..00000000 --- a/cmvr-es/manager/safety_manager/include/command_ledger.h +++ /dev/null @@ -1,130 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "manager/safety_manager/include/safety_types.h" - -namespace cmvr::safety { - -struct CommandKey { - std::string effective_principal_id; - std::string command_id; - - bool operator==(const CommandKey& other) const noexcept - { - return effective_principal_id == other.effective_principal_id && - command_id == other.command_id; - } -}; - -struct CommandOutcome { - CommandLifecycle lifecycle{CommandLifecycle::Failed}; - SafetyReason reason{SafetyReason::InternalError}; - std::string detail; - std::string serialized_response; - std::uint64_t safety_epoch{0}; - std::uint64_t device_generation{0}; - bool hardware_submission_possible{false}; -}; - -enum class CommandReservationStatus { - AcceptedNew, - JoinedInFlight, - CachedResult, - CommandIdConflict, - ResultEvicted, - LedgerExhausted, - Invalid, -}; - -class CommandLedger final { -private: - struct State; - -public: - struct Config { - std::size_t result_capacity{4096}; - std::size_t total_id_capacity{256U * 1024U}; - }; - - class Ticket final { - public: - Ticket() = default; - bool valid() const noexcept { return state_ != nullptr; } - - private: - friend class CommandLedger; - explicit Ticket(std::shared_ptr state) - : state_(std::move(state)) - { - } - - std::shared_ptr state_; - }; - - struct Reservation { - CommandReservationStatus status{CommandReservationStatus::Invalid}; - Ticket ticket; - std::optional cached_outcome; - }; - - CommandLedger(); - explicit CommandLedger(Config config); - - Reservation reserve(CommandKey key, std::string payload_hash); - bool setLifecycle(const Ticket& ticket, - CommandLifecycle lifecycle, - std::uint64_t safety_epoch = 0, - std::uint64_t device_generation = 0, - bool hardware_submission_possible = false); - bool complete(const Ticket& ticket, CommandOutcome outcome); - std::optional wait( - const Ticket& ticket, - SafetyClock::time_point deadline = SafetyClock::time_point::max()) const; - std::optional lookup( - const CommandKey& key, - const std::string& payload_hash) const; - - std::size_t acceptedIdCount() const; - std::size_t liveRecordCount() const; - std::size_t retiredIdCount() const; - -private: - struct KeyHash { - std::size_t operator()(const CommandKey& key) const noexcept; - }; - - struct State { - CommandKey key; - std::string payload_hash; - mutable std::mutex mutex; - mutable std::condition_variable condition; - CommandLifecycle lifecycle{CommandLifecycle::Reserved}; - std::optional outcome; - std::uint64_t safety_epoch{0}; - std::uint64_t device_generation{0}; - bool hardware_submission_possible{false}; - bool terminal{false}; - }; - - static bool validKey_(const CommandKey& key) noexcept; - void trimTerminalResultsLocked_(); - - const Config config_; - mutable std::mutex mutex_; - std::unordered_map, KeyHash> records_; - std::unordered_map retired_ids_; - std::vector terminal_order_; - std::size_t terminal_result_count_{0}; -}; - -const char* toString(CommandReservationStatus status) noexcept; - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h b/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h deleted file mode 100644 index 836ca6b8..00000000 --- a/cmvr-es/manager/safety_manager/include/device_safety_endpoint.h +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include -#include - -#include "manager/safety_manager/include/safety_types.h" - -namespace cmvr::safety { - -using SafetySnapshotPublisher = - std::function; - -class DeviceSafetyEndpoint { -public: - virtual ~DeviceSafetyEndpoint() = default; - - virtual DeviceSafetyDescriptor descriptor() const = 0; - virtual void bindPublisher(SafetySnapshotPublisher publisher) = 0; - virtual void requestSafetyRefresh() noexcept = 0; - // Called after a backend/session restart. Implementations must publish - // subsequent samples with this generation or remain fail-closed. - virtual void onDeviceGenerationChanged( - std::uint64_t generation) noexcept - { - (void)generation; - } - virtual HardwareCheckResult validateBeforeDispatch( - const AdmissionPermit& permit) = 0; - virtual RecoveryCheckResult reconcileAdmissionState( - const RecoveryContext& context) = 0; -}; - -class DeviceSafetyEndpointProvider { -public: - virtual ~DeviceSafetyEndpointProvider() = default; - virtual std::shared_ptr safetyEndpoint() = 0; -}; - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/include/safety_manager.h b/cmvr-es/manager/safety_manager/include/safety_manager.h deleted file mode 100644 index 1ea4c982..00000000 --- a/cmvr-es/manager/safety_manager/include/safety_manager.h +++ /dev/null @@ -1,225 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "manager/safety_manager/include/command_ledger.h" -#include "manager/safety_manager/include/device_safety_endpoint.h" -#include "manager/safety_manager/include/safety_participant.h" -#include "manager/safety_manager/include/safety_snapshot_store.h" - -namespace cmvr::safety { - -struct SafetyManagerConfig { - EnforcementMode enforcement_mode{EnforcementMode::Shadow}; - std::unordered_set enforced_device_ids; - std::chrono::milliseconds stop_all_timeout{15000}; - std::chrono::milliseconds recovery_timeout{10000}; - CommandLedger::Config command_ledger; - std::size_t event_history_capacity{2048}; - bool fail_startup_on_missing_control_capability{false}; -}; - -struct AdmissionResult { - AdmissionDecision decision; - std::optional permit; -}; - -struct StartupCoverageIssue { - std::string target_id; - SafetyReason reason{SafetyReason::None}; - std::string detail; -}; - -struct StartupCoverageResult { - bool ready{false}; - std::vector issues; -}; - -struct DeviceSafetyStateView { - DeviceSafetyDescriptor descriptor; - SafetySnapshotView safety; - device::ManagedDeviceState lifecycle{ - device::ManagedDeviceState::Unknown}; - device::DeviceHealthSnapshot health; - DeviceAdmissionState admission_state{DeviceAdmissionState::Observing}; - std::vector blockers; -}; - -struct ParticipantResultView { - bool recorded{false}; - bool success{false}; - SafetyReason reason{SafetyReason::None}; - std::string detail; -}; - -struct ParticipantSafetyStateView { - ParticipantDescriptor descriptor; - bool registered{false}; - bool barrier_active{false}; - bool barrier_retained{false}; - std::string operation_id; - std::uint64_t safety_epoch{0}; - ParticipantResultView last_request; - ParticipantResultView last_verify; - ParticipantResultView last_release; -}; - -struct SafetyManagerSnapshot { - SystemAdmissionState system_state{SystemAdmissionState::Starting}; - std::uint64_t safety_epoch{0}; - std::string service_instance_id; - EnforcementMode enforcement_mode{EnforcementMode::Shadow}; - std::string active_operation_id; - std::string active_operation_phase; - std::vector devices; - std::vector participants; - std::vector recent_events; -}; - -struct SafetyTargetResult { - std::string target_id; - bool success{false}; - SafetyReason reason{SafetyReason::None}; - std::string detail; - DeviceAdmissionState before_state{DeviceAdmissionState::Observing}; - DeviceAdmissionState after_state{DeviceAdmissionState::Observing}; -}; - -struct StopAllResult { - bool success{false}; - std::string operation_id; - std::uint64_t previous_safety_epoch{0}; - std::uint64_t current_safety_epoch{0}; - SystemAdmissionState system_state{SystemAdmissionState::Starting}; - std::vector targets; -}; - -enum class RecoveryResultCode { - Recovered, - VerifiedButStillBlocked, - BlockerRemains, - EpochMismatch, - NothingToRecover, - TimedOut, - Failed, -}; - -struct RecoveryRequest { - std::string recovery_id; - std::vector device_ids; - bool all_devices{false}; - std::uint64_t expected_safety_epoch{0}; - bool verify_only{true}; - std::string reason; - SafetyClock::time_point deadline{SafetyClock::time_point::max()}; - // Called only for a latch-clearing transaction, after hardware facts have - // been verified and before any software barrier is reconciled or released. - // A false result leaves admission latched. - std::function authorize_clear; -}; - -struct RecoveryResult { - RecoveryResultCode result{RecoveryResultCode::Failed}; - std::string recovery_id; - std::uint64_t previous_safety_epoch{0}; - std::uint64_t current_safety_epoch{0}; - SystemAdmissionState system_state{SystemAdmissionState::Starting}; - std::vector targets; -}; - -class SafetyManager; - -class DispatchGuard final { -public: - DispatchGuard() noexcept = default; - ~DispatchGuard() noexcept; - DispatchGuard(DispatchGuard&& other) noexcept; - DispatchGuard& operator=(DispatchGuard&& other) noexcept; - DispatchGuard(const DispatchGuard&) = delete; - DispatchGuard& operator=(const DispatchGuard&) = delete; - - bool acquired() const noexcept { return coordinator_ != nullptr; } - const HardwareCheckResult& hardwareCheck() const noexcept - { - return hardware_check_; - } - -private: - friend class SafetyManager; - DispatchGuard(SafetyManager* coordinator, - std::string device_id, - HardwareCheckResult hardware_check) noexcept; - void reset_() noexcept; - - SafetyManager* coordinator_{nullptr}; - std::string device_id_; - HardwareCheckResult hardware_check_; -}; - -class SafetyManager final { -public: - explicit SafetyManager(SafetyManagerConfig config = {}); - ~SafetyManager(); - SafetyManager(const SafetyManager&) = delete; - SafetyManager& operator=(const SafetyManager&) = delete; - - bool registerDevice(DeviceSafetyRegistration registration); - bool unregisterDevice(const std::string& device_id); - bool registerParticipant(std::shared_ptr participant); - bool unregisterParticipant(const std::string& participant_id); - - void updateDeviceRuntimeState( - const std::string& device_id, - device::ManagedDeviceState lifecycle, - device::DeviceHealthSnapshot health = {}); - bool publishSafetySnapshot(DeviceSafetySnapshot snapshot); - std::optional advanceDeviceGeneration( - const std::string& device_id); - StartupCoverageResult validateStartupCoverage( - SafetyClock::time_point deadline); - void markStartupComplete(); - void beginShutdown() noexcept; - - AdmissionDecision evaluate(const AdmissionRequest& request) const; - AdmissionResult admit(const AdmissionRequest& request); - // Lightweight session check. This validates the coordinator-owned epoch, - // generation, freshness, and admission state without calling the device - // endpoint or entering the hardware dispatch set. - HardwareCheckResult revalidatePermit( - const AdmissionPermit& permit) const; - DispatchGuard beginDispatch(const AdmissionPermit& permit); - void quarantineDevice(const std::string& device_id, - SafetyReason reason, - std::string operation_id = {}); - - StopAllResult stopAll( - std::string operation_id, - SafetyClock::time_point deadline = SafetyClock::time_point::max()); - RecoveryResult recover(const RecoveryRequest& request); - - SafetyManagerSnapshot snapshot() const; - SafetySnapshotStore& snapshotStore() noexcept; - const SafetySnapshotStore& snapshotStore() const noexcept; - CommandLedger& commandLedger() noexcept; - const CommandLedger& commandLedger() const noexcept; - const std::string& serviceInstanceId() const noexcept; - const SafetyManagerConfig& config() const noexcept; - -private: - friend class DispatchGuard; - struct Impl; - void endDispatch_(const std::string& device_id) noexcept; - std::unique_ptr impl_; -}; - -const char* toString(RecoveryResultCode value) noexcept; - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/include/safety_participant.h b/cmvr-es/manager/safety_manager/include/safety_participant.h deleted file mode 100644 index 1a4557ce..00000000 --- a/cmvr-es/manager/safety_manager/include/safety_participant.h +++ /dev/null @@ -1,88 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "manager/safety_manager/include/safety_types.h" - -namespace cmvr::safety { - -class DeviceSafetyEndpoint; - -enum class ParticipantPhase { - Ingress, - Scheduler, - ControlSession, - Actuator, - PeripheralActivity, - Verification, -}; - -struct ParticipantDescriptor { - std::string participant_id; - ParticipantPhase phase{ParticipantPhase::Actuator}; - bool required{true}; - std::chrono::milliseconds timeout{5000}; -}; - -struct SafetyOperationContext { - std::string operation_id; - std::uint64_t safety_epoch{0}; - SafetyClock::time_point deadline{SafetyClock::time_point::max()}; -}; - -struct BarrierToken { - std::string participant_id; - std::string operation_id; - std::uint64_t safety_epoch{0}; - std::uint64_t generation{0}; - - bool valid() const noexcept - { - return !participant_id.empty() && !operation_id.empty() && - safety_epoch != 0 && generation != 0; - } -}; - -struct ParticipantResult { - bool success{false}; - SafetyReason reason{SafetyReason::StopUnconfirmed}; - std::string detail; -}; - -class SafetyParticipant { -public: - virtual ~SafetyParticipant() = default; - virtual ParticipantDescriptor descriptor() const = 0; - virtual BarrierToken beginBarrier( - const SafetyOperationContext& context) = 0; - virtual ParticipantResult requestQuiesce( - const BarrierToken& token, - const SafetyOperationContext& context) = 0; - virtual ParticipantResult verifyQuiescent( - const BarrierToken& token, - const SafetyOperationContext& context) = 0; - virtual RecoveryCheckResult recoverAdmission( - const BarrierToken& token, - const RecoveryContext& context) = 0; - // Commits the participant's admission reopening. A failed commit must - // leave that participant fail-closed and be retryable through recovery. - virtual ParticipantResult releaseBarrier( - const BarrierToken& token) noexcept = 0; -}; - -class SafetyParticipantProvider { -public: - virtual ~SafetyParticipantProvider() = default; - virtual std::shared_ptr safetyParticipant() = 0; -}; - -struct DeviceSafetyRegistration { - DeviceSafetyDescriptor descriptor; - std::shared_ptr endpoint; - std::shared_ptr participant; -}; - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/include/safety_reason.h b/cmvr-es/manager/safety_manager/include/safety_reason.h deleted file mode 100644 index 61b44206..00000000 --- a/cmvr-es/manager/safety_manager/include/safety_reason.h +++ /dev/null @@ -1,48 +0,0 @@ -#pragma once - -#include - -namespace cmvr::safety { - -enum class SafetyReason { - None, - InvalidArgument, - Unauthenticated, - PermissionDenied, - RecoveryRpcDisabled, - DeviceNotFound, - DeviceUnavailable, - UnsupportedCommand, - SystemStarting, - SystemStopping, - SafetyLatched, - SafetyStateMissing, - SafetyStateStale, - HardwareUnsafe, - EmergencyStopActive, - ProtectiveStopActive, - DeviceDisconnected, - DeviceFault, - DeviceNotReady, - DeviceStillMoving, - ControlBusy, - GenerationMismatch, - CommandIdRequired, - CommandIdConflict, - ResultEvicted, - LedgerExhausted, - Backpressure, - DeadlineExceededBeforeDispatch, - OutcomeUnknown, - ParticipantTimeout, - StopUnconfirmed, - RecoveryEpochMismatch, - RecoveryReasonRequired, - RecoveryAuditFailed, - InternalError, -}; - -const char* toString(SafetyReason reason) noexcept; -bool retryWithSameCommandId(SafetyReason reason) noexcept; - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/include/safety_snapshot_store.h b/cmvr-es/manager/safety_manager/include/safety_snapshot_store.h deleted file mode 100644 index b52c1c4a..00000000 --- a/cmvr-es/manager/safety_manager/include/safety_snapshot_store.h +++ /dev/null @@ -1,54 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "manager/safety_manager/include/safety_types.h" - -namespace cmvr::safety { - -class SafetySnapshotStore final { -public: - bool registerDevice(const DeviceSafetyDescriptor& descriptor, - std::uint64_t initial_generation = 1); - bool unregisterDevice(const std::string& device_id); - bool publish(DeviceSafetySnapshot snapshot); - bool markUnknown(const std::string& device_id, - SafetyReason reason, - std::string source_id = {}); - std::optional bumpGeneration( - const std::string& device_id); - - SafetySnapshotView get( - const std::string& device_id, - SafetyClock::time_point now = SafetyClock::now()) const; - std::vector snapshot( - SafetyClock::time_point now = SafetyClock::now()) const; - - bool waitForNewerSample( - const std::string& device_id, - std::uint64_t previous_sequence, - SafetyClock::time_point deadline, - SafetySnapshotView& result) const; - -private: - struct Slot { - DeviceSafetyDescriptor descriptor; - DeviceSafetySnapshot snapshot; - bool has_sample{false}; - }; - - static SafetySnapshotView viewOf_( - const Slot& slot, - SafetyClock::time_point now); - - mutable std::shared_mutex mutex_; - mutable std::condition_variable_any changed_; - std::unordered_map slots_; -}; - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/include/safety_types.h b/cmvr-es/manager/safety_manager/include/safety_types.h deleted file mode 100644 index 743629bd..00000000 --- a/cmvr-es/manager/safety_manager/include/safety_types.h +++ /dev/null @@ -1,235 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include "devices/device_types.h" -#include "manager/safety_manager/include/safety_reason.h" - -namespace cmvr::safety { - -using SafetyClock = std::chrono::steady_clock; - -enum class TriState { - Unknown, - False, - True, -}; - -enum class SafetyCondition { - Nominal, - Restricted, - Unsafe, - Unknown, -}; - -enum class CommandIntent { - Observe, - StartActivity, - Configure, - Actuate, - Stop, - ResetFault, - RecoverAdmission, -}; - -enum class SafetyPolicyFamily { - Sensor, - Control, -}; - -enum class BlockerScope { - Device, - System, -}; - -enum class RecoveryRequirement { - RefreshOnly, - ClearSoftwareLatch, - HardwareReleaseRequired, - ManualInspectionRequired, -}; - -enum class SystemAdmissionState { - Starting, - Open, - Stopping, - Latched, - Recovering, - ShuttingDown, -}; - -enum class DeviceAdmissionState { - Observing, - Open, - Blocked, - Quarantined, - Recovering, - Removed, -}; - -enum class EnforcementMode { - Legacy, - Shadow, - EnforceSelected, - EnforceAll, -}; - -enum class CommandLifecycle { - Received, - Reserved, - RejectedBeforeDispatch, - Admitted, - Dispatching, - AcceptedByHardware, - Completed, - Failed, - CanceledBeforeDispatch, - OutcomeUnknown, -}; - -struct SafetyBlocker { - SafetyReason reason{SafetyReason::None}; - BlockerScope scope{BlockerScope::Device}; - RecoveryRequirement recovery_requirement{ - RecoveryRequirement::RefreshOnly}; - std::string source_id; - std::string operation_id; - std::uint64_t first_observed_at_unix_ms{0}; - std::uint64_t last_observed_at_unix_ms{0}; -}; - -struct DeviceSafetySnapshot { - std::string device_id; - SafetyCondition condition{SafetyCondition::Unknown}; - std::uint64_t device_generation{0}; - std::uint64_t sample_sequence{0}; - SafetyClock::time_point observed_at{}; - std::uint64_t observed_at_unix_ms{0}; - - TriState connected{TriState::Unknown}; - TriState operational_ready{TriState::Unknown}; - TriState quiescent{TriState::Unknown}; - TriState motion_active{TriState::Unknown}; - TriState actuator_enabled{TriState::Unknown}; - TriState emergency_stop_active{TriState::Unknown}; - TriState protective_stop_active{TriState::Unknown}; - TriState fault_active{TriState::Unknown}; - - std::vector blockers; -}; - -struct DeviceSafetyDescriptor { - std::string device_id; - device::DeviceKind kind{device::DeviceKind::Unknown}; - SafetyPolicyFamily default_policy{SafetyPolicyFamily::Sensor}; - std::chrono::milliseconds maximum_snapshot_age{1000}; - bool requires_safe_stop{false}; - bool supports_active_refresh{false}; - bool supports_non_enabling_fault_reset{false}; -}; - -struct SafetySnapshotView { - DeviceSafetyDescriptor descriptor; - DeviceSafetySnapshot snapshot; - bool registered{false}; - bool has_sample{false}; - bool fresh{false}; - std::chrono::milliseconds sample_age{ - std::chrono::milliseconds::max()}; -}; - -struct CommandActor { - std::string principal_id{"anonymous"}; - bool authenticated{false}; - std::vector roles; -}; - -struct CommandDescriptor { - std::string full_method_name; - CommandIntent intent{CommandIntent::Observe}; - SafetyPolicyFamily policy_family{SafetyPolicyFamily::Sensor}; - bool mutating{false}; - bool safety_lane{false}; -}; - -struct AdmissionRequest { - CommandDescriptor command; - CommandActor actor; - std::string command_id; - std::string device_id; - std::optional expected_device_generation; - std::uint64_t authority_generation{0}; - SafetyClock::time_point deadline{SafetyClock::time_point::max()}; -}; - -struct AdmissionDecision { - bool allowed{false}; - bool policy_allowed{false}; - bool enforced{false}; - SafetyReason reason{SafetyReason::None}; - std::string detail; - std::uint64_t safety_epoch{0}; - std::uint64_t device_generation{0}; -}; - -struct AdmissionPermit { - AdmissionPermit() = default; - AdmissionPermit(AdmissionPermit&&) noexcept = default; - AdmissionPermit& operator=(AdmissionPermit&&) noexcept = default; - AdmissionPermit(const AdmissionPermit&) = delete; - AdmissionPermit& operator=(const AdmissionPermit&) = delete; - - std::string command_id; - std::string device_id; - CommandIntent intent{CommandIntent::Observe}; - std::uint64_t safety_epoch{0}; - std::uint64_t device_generation{0}; - std::uint64_t authority_generation{0}; - SafetyClock::time_point deadline{SafetyClock::time_point::max()}; - bool policy_allowed{false}; - bool enforced{false}; -}; - -struct HardwareCheckResult { - bool safe{false}; - SafetyReason reason{SafetyReason::SafetyStateMissing}; - std::string detail; -}; - -struct RecoveryContext { - std::string recovery_id; - std::string reason; - std::uint64_t safety_epoch{0}; - SafetyClock::time_point deadline{SafetyClock::time_point::max()}; - bool verify_only{true}; -}; - -struct RecoveryCheckResult { - bool reconciled{false}; - SafetyReason reason{SafetyReason::None}; - std::string detail; -}; - -struct SafetyEvent { - std::uint64_t sequence{0}; - std::uint64_t safety_epoch{0}; - std::uint64_t occurred_at_unix_ms{0}; - std::string source_id; - std::string operation_id; - SafetyReason reason{SafetyReason::None}; - std::string detail; -}; - -const char* toString(TriState value) noexcept; -const char* toString(SafetyCondition value) noexcept; -const char* toString(CommandIntent value) noexcept; -const char* toString(SafetyPolicyFamily value) noexcept; -const char* toString(SystemAdmissionState value) noexcept; -const char* toString(DeviceAdmissionState value) noexcept; -const char* toString(EnforcementMode value) noexcept; - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/src/command_ledger.cpp b/cmvr-es/manager/safety_manager/src/command_ledger.cpp deleted file mode 100644 index 95a28681..00000000 --- a/cmvr-es/manager/safety_manager/src/command_ledger.cpp +++ /dev/null @@ -1,276 +0,0 @@ -#include "manager/safety_manager/include/command_ledger.h" - -#include -#include -#include -#include -#include - -namespace cmvr::safety { - -namespace { - -constexpr std::size_t kMaxPrincipalIdLength = 256; -constexpr std::size_t kMaxCommandIdLength = 128; - -bool validIdentifier(const std::string& value, const std::size_t maximum) -{ - if (value.empty() || value.size() > maximum) { - return false; - } - return std::all_of( - value.begin(), value.end(), [](const unsigned char character) { - return std::isalnum(character) || character == '-' || - character == '_' || character == '.' || - character == ':' || character == '/'; - }); -} - -} // namespace - -CommandLedger::CommandLedger() - : CommandLedger(Config{}) -{ -} - -CommandLedger::CommandLedger(Config config) - : config_(config) -{ - if (config_.total_id_capacity == 0 || - config_.result_capacity > config_.total_id_capacity) { - throw std::invalid_argument("invalid CommandLedger capacity"); - } -} - -std::size_t CommandLedger::KeyHash::operator()( - const CommandKey& key) const noexcept -{ - const auto first = std::hash{}(key.effective_principal_id); - const auto second = std::hash{}(key.command_id); - return first ^ (second + 0x9e3779b9U + (first << 6U) + (first >> 2U)); -} - -bool CommandLedger::validKey_(const CommandKey& key) noexcept -{ - return validIdentifier( - key.effective_principal_id, kMaxPrincipalIdLength) && - validIdentifier(key.command_id, kMaxCommandIdLength); -} - -CommandLedger::Reservation CommandLedger::reserve( - CommandKey key, - std::string payload_hash) -{ - if (!validKey_(key) || payload_hash.empty()) { - return {}; - } - - std::lock_guard lock(mutex_); - const auto live = records_.find(key); - if (live != records_.end()) { - const auto& state = live->second; - std::lock_guard state_lock(state->mutex); - if (state->payload_hash != payload_hash) { - return {CommandReservationStatus::CommandIdConflict, {}, {}}; - } - if (state->terminal && state->outcome.has_value()) { - return { - CommandReservationStatus::CachedResult, - Ticket(state), - state->outcome}; - } - return { - CommandReservationStatus::JoinedInFlight, - Ticket(state), - {}}; - } - - const auto retired = retired_ids_.find(key); - if (retired != retired_ids_.end()) { - return { - retired->second == payload_hash - ? CommandReservationStatus::ResultEvicted - : CommandReservationStatus::CommandIdConflict, - {}, - {}}; - } - - if (records_.size() + retired_ids_.size() >= - config_.total_id_capacity) { - return {CommandReservationStatus::LedgerExhausted, {}, {}}; - } - - auto state = std::make_shared(); - state->key = std::move(key); - state->payload_hash = std::move(payload_hash); - const auto inserted = records_.emplace(state->key, state); - if (!inserted.second) { - throw std::logic_error("CommandLedger duplicate insertion"); - } - return { - CommandReservationStatus::AcceptedNew, - Ticket(std::move(state)), - {}}; -} - -bool CommandLedger::setLifecycle( - const Ticket& ticket, - const CommandLifecycle lifecycle, - const std::uint64_t safety_epoch, - const std::uint64_t device_generation, - const bool hardware_submission_possible) -{ - if (!ticket.valid()) { - return false; - } - std::lock_guard ledger_lock(mutex_); - const auto found = records_.find(ticket.state_->key); - if (found == records_.end() || found->second != ticket.state_) { - return false; - } - std::lock_guard state_lock(ticket.state_->mutex); - if (ticket.state_->terminal) { - return false; - } - ticket.state_->lifecycle = lifecycle; - ticket.state_->safety_epoch = safety_epoch; - ticket.state_->device_generation = device_generation; - ticket.state_->hardware_submission_possible = - ticket.state_->hardware_submission_possible || - hardware_submission_possible; - return true; -} - -bool CommandLedger::complete(const Ticket& ticket, CommandOutcome outcome) -{ - if (!ticket.valid()) { - return false; - } - - std::lock_guard ledger_lock(mutex_); - const auto found = records_.find(ticket.state_->key); - if (found == records_.end() || found->second != ticket.state_) { - return false; - } - - { - std::lock_guard state_lock(ticket.state_->mutex); - if (ticket.state_->terminal) { - return false; - } - outcome.hardware_submission_possible = - outcome.hardware_submission_possible || - ticket.state_->hardware_submission_possible; - if (outcome.safety_epoch == 0) { - outcome.safety_epoch = ticket.state_->safety_epoch; - } - if (outcome.device_generation == 0) { - outcome.device_generation = ticket.state_->device_generation; - } - ticket.state_->lifecycle = outcome.lifecycle; - ticket.state_->outcome = std::move(outcome); - ticket.state_->terminal = true; - } - ticket.state_->condition.notify_all(); - terminal_order_.push_back(ticket.state_->key); - ++terminal_result_count_; - trimTerminalResultsLocked_(); - return true; -} - -void CommandLedger::trimTerminalResultsLocked_() -{ - std::size_t consumed = 0; - while (terminal_result_count_ > config_.result_capacity && - consumed < terminal_order_.size()) { - const auto key = terminal_order_[consumed++]; - const auto found = records_.find(key); - if (found == records_.end()) { - continue; - } - const auto& state = found->second; - std::lock_guard state_lock(state->mutex); - if (!state->terminal) { - continue; - } - retired_ids_.emplace(state->key, state->payload_hash); - records_.erase(found); - --terminal_result_count_; - } - if (consumed != 0) { - terminal_order_.erase( - terminal_order_.begin(), - terminal_order_.begin() + static_cast(consumed)); - } -} - -std::optional CommandLedger::wait( - const Ticket& ticket, - const SafetyClock::time_point deadline) const -{ - if (!ticket.valid()) { - return std::nullopt; - } - std::unique_lock lock(ticket.state_->mutex); - if (deadline == SafetyClock::time_point::max()) { - ticket.state_->condition.wait( - lock, [&ticket] { return ticket.state_->terminal; }); - } else if (!ticket.state_->condition.wait_until( - lock, deadline, - [&ticket] { return ticket.state_->terminal; })) { - return std::nullopt; - } - return ticket.state_->outcome; -} - -std::optional CommandLedger::lookup( - const CommandKey& key, - const std::string& payload_hash) const -{ - std::lock_guard lock(mutex_); - const auto found = records_.find(key); - if (found == records_.end()) { - return std::nullopt; - } - std::lock_guard state_lock(found->second->mutex); - if (found->second->payload_hash != payload_hash || - !found->second->terminal) { - return std::nullopt; - } - return found->second->outcome; -} - -std::size_t CommandLedger::acceptedIdCount() const -{ - std::lock_guard lock(mutex_); - return records_.size() + retired_ids_.size(); -} - -std::size_t CommandLedger::liveRecordCount() const -{ - std::lock_guard lock(mutex_); - return records_.size(); -} - -std::size_t CommandLedger::retiredIdCount() const -{ - std::lock_guard lock(mutex_); - return retired_ids_.size(); -} - -const char* toString(const CommandReservationStatus status) noexcept -{ - switch (status) { - case CommandReservationStatus::AcceptedNew: return "AcceptedNew"; - case CommandReservationStatus::JoinedInFlight: return "JoinedInFlight"; - case CommandReservationStatus::CachedResult: return "CachedResult"; - case CommandReservationStatus::CommandIdConflict: - return "CommandIdConflict"; - case CommandReservationStatus::ResultEvicted: return "ResultEvicted"; - case CommandReservationStatus::LedgerExhausted: return "LedgerExhausted"; - case CommandReservationStatus::Invalid: return "Invalid"; - } - return "Invalid"; -} - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/src/safety_manager.cpp b/cmvr-es/manager/safety_manager/src/safety_manager.cpp deleted file mode 100644 index bbb31eeb..00000000 --- a/cmvr-es/manager/safety_manager/src/safety_manager.cpp +++ /dev/null @@ -1,2424 +0,0 @@ -#include "manager/safety_manager/include/safety_manager.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::safety { - -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::string makeInstanceId() -{ - static std::atomic sequence{0}; - std::ostringstream output; - output << "safety-" << std::hex - << std::chrono::duration_cast( - SafetyClock::now().time_since_epoch()).count() - << '-' - << sequence.fetch_add(1, std::memory_order_relaxed) + 1U; - return output.str(); -} - -std::string makeOperationId(const char* prefix) -{ - static std::atomic sequence{0}; - return std::string(prefix) + '-' + std::to_string( - sequence.fetch_add(1, std::memory_order_relaxed) + 1U); -} - -bool hasTrue(const TriState value) noexcept -{ - return value == TriState::True; -} - -bool isControlIntent(const CommandIntent intent) noexcept -{ - return intent == CommandIntent::Actuate || - intent == CommandIntent::Configure || - intent == CommandIntent::StartActivity; -} - -std::chrono::milliseconds boundedRemaining( - const SafetyClock::time_point deadline, - const std::chrono::milliseconds maximum) -{ - const auto now = SafetyClock::now(); - if (now >= deadline) { - return std::chrono::milliseconds::zero(); - } - return std::min( - maximum, - std::chrono::duration_cast(deadline - now)); -} - -SafetyBlocker makeBlocker( - const SafetyReason reason, - const BlockerScope scope, - const RecoveryRequirement recovery, - std::string source, - std::string operation = {}) -{ - const auto now = unixTimeMs(); - return { - reason, - scope, - recovery, - std::move(source), - std::move(operation), - now, - now}; -} - -class OperationExecutor final { -public: - explicit OperationExecutor(const std::size_t worker_count) - { - const auto count = std::max(1, worker_count); - workers_.reserve(count); - for (std::size_t index = 0; index < count; ++index) { - workers_.emplace_back([this] { workerLoop_(); }); - } - } - - ~OperationExecutor() - { - { - std::lock_guard lock(mutex_); - stopping_ = true; - } - available_.notify_all(); - for (auto& worker : workers_) { - if (worker.joinable()) { - worker.join(); - } - } - } - - std::future submit( - std::function operation) - { - auto task = std::make_shared>( - [operation = std::move(operation)]() mutable { - try { - return operation(); - } catch (const std::exception& error) { - return ParticipantResult{ - false, SafetyReason::InternalError, error.what()}; - } catch (...) { - return ParticipantResult{ - false, - SafetyReason::InternalError, - "participant threw an unknown exception"}; - } - }); - auto future = task->get_future(); - { - std::lock_guard lock(mutex_); - if (stopping_) { - throw std::runtime_error("safety operation executor stopped"); - } - jobs_.push([task] { (*task)(); }); - } - available_.notify_one(); - return future; - } - -private: - void workerLoop_() - { - while (true) { - std::function job; - { - std::unique_lock lock(mutex_); - available_.wait(lock, [this] { - return stopping_ || !jobs_.empty(); - }); - if (stopping_ && jobs_.empty()) { - return; - } - job = std::move(jobs_.front()); - jobs_.pop(); - } - job(); - } - } - - std::mutex mutex_; - std::condition_variable available_; - std::queue> jobs_; - std::vector workers_; - bool stopping_{false}; -}; - -int phaseRank(const ParticipantPhase phase) noexcept -{ - switch (phase) { - case ParticipantPhase::Ingress: return 0; - case ParticipantPhase::Scheduler: return 1; - case ParticipantPhase::ControlSession: return 2; - case ParticipantPhase::Actuator: return 3; - case ParticipantPhase::PeripheralActivity: return 4; - case ParticipantPhase::Verification: return 5; - } - return 5; -} - -} // namespace - -struct SafetyManager::Impl { - struct PublisherBinding { - std::atomic active{true}; - SafetyManager* coordinator{nullptr}; - }; - - struct DeviceSlot { - DeviceSafetyDescriptor descriptor; - std::shared_ptr endpoint; - std::shared_ptr participant; - std::shared_ptr publisher_binding; - device::ManagedDeviceState lifecycle{ - device::ManagedDeviceState::Unknown}; - device::DeviceHealthSnapshot health; - DeviceAdmissionState admission_state{ - DeviceAdmissionState::Observing}; - std::vector software_blockers; - std::size_t in_flight_dispatches{0}; - bool quarantined{false}; - }; - - struct RetainedBarrier { - std::shared_ptr participant; - BarrierToken token; - }; - - struct ParticipantRuntime { - ParticipantDescriptor descriptor; - bool registered{false}; - bool barrier_active{false}; - bool barrier_retained{false}; - std::string operation_id; - std::uint64_t safety_epoch{0}; - ParticipantResultView last_request; - ParticipantResultView last_verify; - ParticipantResultView last_release; - }; - - struct StopRound { - std::mutex mutex; - std::condition_variable completed; - std::atomic done{false}; - StopAllResult result; - }; - - struct RecoveryRound { - std::mutex mutex; - std::condition_variable completed; - std::string fingerprint; - std::atomic done{false}; - RecoveryResult result; - }; - - explicit Impl(SafetyManagerConfig source) - : config(std::move(source)), - ledger(config.command_ledger), - service_instance_id(makeInstanceId()), - operation_executor(4) - { - if (config.stop_all_timeout <= std::chrono::milliseconds::zero() || - config.recovery_timeout <= std::chrono::milliseconds::zero() || - config.event_history_capacity == 0) { - throw std::invalid_argument("invalid SafetyManagerConfig"); - } - } - - bool isEnforced(const std::string& device_id) const - { - switch (config.enforcement_mode) { - case EnforcementMode::Legacy: - case EnforcementMode::Shadow: - return false; - case EnforcementMode::EnforceSelected: - return config.enforced_device_ids.count(device_id) != 0; - case EnforcementMode::EnforceAll: - return true; - } - return false; - } - - void addEventLocked( - const SafetyReason reason, - std::string source, - std::string operation, - std::string detail) - { - SafetyEvent event; - event.sequence = ++next_event_sequence; - event.safety_epoch = safety_epoch; - event.occurred_at_unix_ms = unixTimeMs(); - event.source_id = std::move(source); - event.operation_id = std::move(operation); - event.reason = reason; - event.detail = std::move(detail); - events.push_back(std::move(event)); - while (events.size() > config.event_history_capacity) { - events.pop_front(); - } - } - - DeviceAdmissionState admissionStateFor( - const DeviceSlot& slot, - const SafetySnapshotView& view) const - { - if (slot.admission_state == DeviceAdmissionState::Removed) { - return DeviceAdmissionState::Removed; - } - if (slot.quarantined) { - return DeviceAdmissionState::Quarantined; - } - if (!view.has_sample) { - return DeviceAdmissionState::Observing; - } - if (!view.fresh || - view.snapshot.condition == SafetyCondition::Unknown || - view.snapshot.condition == SafetyCondition::Unsafe || - view.snapshot.connected == TriState::False || - view.snapshot.fault_active == TriState::True || - slot.lifecycle == device::ManagedDeviceState::Error) { - return DeviceAdmissionState::Blocked; - } - if (slot.descriptor.default_policy == SafetyPolicyFamily::Control && - (view.snapshot.connected != TriState::True || - view.snapshot.operational_ready != TriState::True || - view.snapshot.emergency_stop_active != TriState::False || - view.snapshot.protective_stop_active != TriState::False || - view.snapshot.fault_active != TriState::False)) { - return DeviceAdmissionState::Blocked; - } - return DeviceAdmissionState::Open; - } - - bool hasGlobalLatchLocked() const - { - if (!retained_barriers.empty()) { - return true; - } - for (const auto& [id, slot] : devices) { - (void)id; - if (slot.quarantined) { - return true; - } - const auto view = snapshots.get(slot.descriptor.device_id); - for (const auto& blocker : view.snapshot.blockers) { - if (blocker.scope == BlockerScope::System) { - return true; - } - } - } - return false; - } - - SafetyManagerConfig config; - SafetySnapshotStore snapshots; - CommandLedger ledger; - const std::string service_instance_id; - - mutable std::mutex mutex; - std::condition_variable state_changed; - SystemAdmissionState system_state{SystemAdmissionState::Starting}; - std::uint64_t safety_epoch{1}; - std::unordered_map devices; - std::unordered_map> - participants; - std::unordered_map participant_runtime; - std::unordered_map retained_barriers; - std::deque events; - std::uint64_t next_event_sequence{0}; - std::string active_operation_id; - std::string active_operation_phase; - - std::mutex operation_execution_mutex; - OperationExecutor operation_executor; - std::mutex rounds_mutex; - std::shared_ptr active_stop_round; - std::unordered_map> - recovery_rounds; -}; - -DispatchGuard::DispatchGuard( - SafetyManager* coordinator, - std::string device_id, - HardwareCheckResult hardware_check) noexcept - : coordinator_(coordinator), - device_id_(std::move(device_id)), - hardware_check_(std::move(hardware_check)) -{ -} - -DispatchGuard::~DispatchGuard() noexcept -{ - reset_(); -} - -DispatchGuard::DispatchGuard(DispatchGuard&& other) noexcept - : coordinator_(std::exchange(other.coordinator_, nullptr)), - device_id_(std::move(other.device_id_)), - hardware_check_(std::move(other.hardware_check_)) -{ -} - -DispatchGuard& DispatchGuard::operator=(DispatchGuard&& other) noexcept -{ - if (this != &other) { - reset_(); - coordinator_ = std::exchange(other.coordinator_, nullptr); - device_id_ = std::move(other.device_id_); - hardware_check_ = std::move(other.hardware_check_); - } - return *this; -} - -void DispatchGuard::reset_() noexcept -{ - if (coordinator_ == nullptr) { - return; - } - auto* coordinator = std::exchange(coordinator_, nullptr); - coordinator->endDispatch_(device_id_); -} - -SafetyManager::SafetyManager(SafetyManagerConfig config) - : impl_(std::make_unique(std::move(config))) -{ -} - -SafetyManager::~SafetyManager() -{ - beginShutdown(); - std::vector> endpoints; - { - std::lock_guard lock(impl_->mutex); - endpoints.reserve(impl_->devices.size()); - for (auto& [id, slot] : impl_->devices) { - (void)id; - if (slot.publisher_binding) { - slot.publisher_binding->active.store( - false, std::memory_order_release); - } - if (slot.endpoint) { - endpoints.push_back(slot.endpoint); - } - } - } - for (const auto& endpoint : endpoints) { - endpoint->bindPublisher({}); - } -} - -bool SafetyManager::registerDevice(DeviceSafetyRegistration registration) -{ - if (registration.descriptor.device_id.empty() || - registration.descriptor.maximum_snapshot_age <= - std::chrono::milliseconds::zero()) { - return false; - } - if (registration.endpoint) { - const auto endpoint_descriptor = registration.endpoint->descriptor(); - if (endpoint_descriptor.device_id != - registration.descriptor.device_id || - endpoint_descriptor.kind != registration.descriptor.kind) { - return false; - } - } - if (registration.descriptor.default_policy == - SafetyPolicyFamily::Control && - impl_->config.fail_startup_on_missing_control_capability && - (!registration.endpoint || - (registration.descriptor.requires_safe_stop && - !registration.participant))) { - return false; - } - if (!impl_->snapshots.registerDevice(registration.descriptor)) { - return false; - } - - auto binding = std::make_shared(); - binding->coordinator = this; - { - std::lock_guard lock(impl_->mutex); - if (impl_->devices.count(registration.descriptor.device_id) != 0) { - impl_->snapshots.unregisterDevice( - registration.descriptor.device_id); - return false; - } - Impl::DeviceSlot slot; - slot.descriptor = registration.descriptor; - slot.endpoint = registration.endpoint; - slot.participant = registration.participant; - slot.publisher_binding = binding; - if (!slot.endpoint && - slot.descriptor.default_policy == SafetyPolicyFamily::Control) { - slot.software_blockers.push_back(makeBlocker( - SafetyReason::SafetyStateMissing, - BlockerScope::Device, - RecoveryRequirement::RefreshOnly, - slot.descriptor.device_id)); - } - impl_->devices.emplace(slot.descriptor.device_id, std::move(slot)); - if (registration.participant) { - const auto descriptor = registration.participant->descriptor(); - if (descriptor.participant_id.empty() || - impl_->participants.count(descriptor.participant_id) != 0) { - impl_->devices.erase(registration.descriptor.device_id); - impl_->snapshots.unregisterDevice( - registration.descriptor.device_id); - return false; - } - impl_->participants.emplace( - descriptor.participant_id, registration.participant); - auto& runtime = - impl_->participant_runtime[descriptor.participant_id]; - runtime.descriptor = descriptor; - runtime.registered = true; - } - impl_->addEventLocked( - SafetyReason::SafetyStateMissing, - registration.descriptor.device_id, - {}, - "device safety capability registered; awaiting a fresh sample"); - } - - if (registration.endpoint) { - std::weak_ptr weak_binding = binding; - registration.endpoint->bindPublisher( - [weak_binding](DeviceSafetySnapshot snapshot) { - const auto active = weak_binding.lock(); - if (!active || - !active->active.load(std::memory_order_acquire) || - active->coordinator == nullptr) { - return false; - } - return active->coordinator->publishSafetySnapshot( - std::move(snapshot)); - }); - registration.endpoint->requestSafetyRefresh(); - } else { - impl_->snapshots.markUnknown( - registration.descriptor.device_id, - SafetyReason::SafetyStateMissing); - } - return true; -} - -bool SafetyManager::unregisterDevice(const std::string& device_id) -{ - std::shared_ptr endpoint; - std::string participant_id; - { - std::lock_guard lock(impl_->mutex); - const auto found = impl_->devices.find(device_id); - if (found == impl_->devices.end()) { - return false; - } - if (found->second.in_flight_dispatches != 0) { - return false; - } - found->second.publisher_binding->active.store( - false, std::memory_order_release); - endpoint = found->second.endpoint; - if (found->second.participant) { - participant_id = - found->second.participant->descriptor().participant_id; - impl_->participants.erase(participant_id); - const auto runtime = - impl_->participant_runtime.find(participant_id); - if (runtime != impl_->participant_runtime.end()) { - runtime->second.registered = false; - if (!runtime->second.barrier_active && - impl_->retained_barriers.count(participant_id) == 0U) { - impl_->participant_runtime.erase(runtime); - } - } - } - found->second.admission_state = DeviceAdmissionState::Removed; - impl_->devices.erase(found); - impl_->addEventLocked( - SafetyReason::DeviceUnavailable, - device_id, - {}, - "device safety capability unregistered"); - } - if (endpoint) { - endpoint->bindPublisher({}); - } - impl_->snapshots.unregisterDevice(device_id); - return true; -} - -bool SafetyManager::registerParticipant( - std::shared_ptr participant) -{ - if (!participant) { - return false; - } - const auto descriptor = participant->descriptor(); - if (descriptor.participant_id.empty()) { - return false; - } - std::lock_guard lock(impl_->mutex); - if (!impl_->participants.emplace( - descriptor.participant_id, std::move(participant)).second) { - return false; - } - auto& runtime = impl_->participant_runtime[descriptor.participant_id]; - runtime.descriptor = descriptor; - runtime.registered = true; - return true; -} - -bool SafetyManager::unregisterParticipant( - const std::string& participant_id) -{ - std::lock_guard lock(impl_->mutex); - if (impl_->participants.erase(participant_id) == 0U) { - return false; - } - const auto runtime = impl_->participant_runtime.find(participant_id); - if (runtime != impl_->participant_runtime.end()) { - runtime->second.registered = false; - if (!runtime->second.barrier_active && - impl_->retained_barriers.count(participant_id) == 0U) { - impl_->participant_runtime.erase(runtime); - } - } - return true; -} - -void SafetyManager::updateDeviceRuntimeState( - const std::string& device_id, - const device::ManagedDeviceState lifecycle, - device::DeviceHealthSnapshot health) -{ - const auto view = impl_->snapshots.get(device_id); - std::lock_guard lock(impl_->mutex); - const auto found = impl_->devices.find(device_id); - if (found == impl_->devices.end()) { - return; - } - found->second.lifecycle = lifecycle; - found->second.health = std::move(health); - found->second.admission_state = - impl_->admissionStateFor(found->second, view); - impl_->state_changed.notify_all(); -} - -bool SafetyManager::publishSafetySnapshot(DeviceSafetySnapshot snapshot) -{ - const std::string device_id = snapshot.device_id; - if (!impl_->snapshots.publish(std::move(snapshot))) { - return false; - } - const auto view = impl_->snapshots.get(device_id); - std::lock_guard lock(impl_->mutex); - const auto found = impl_->devices.find(device_id); - if (found == impl_->devices.end()) { - return false; - } - const auto previous = found->second.admission_state; - found->second.admission_state = - impl_->admissionStateFor(found->second, view); - bool system_blocker = false; - for (const auto& blocker : view.snapshot.blockers) { - system_blocker = system_blocker || - blocker.scope == BlockerScope::System; - } - if (system_blocker && - impl_->system_state != SystemAdmissionState::Stopping && - impl_->system_state != SystemAdmissionState::Recovering && - impl_->system_state != SystemAdmissionState::ShuttingDown && - impl_->system_state != SystemAdmissionState::Latched) { - impl_->system_state = SystemAdmissionState::Latched; - ++impl_->safety_epoch; - impl_->addEventLocked( - SafetyReason::HardwareUnsafe, - device_id, - {}, - "system-scope hardware blocker latched admission"); - } else if (previous != found->second.admission_state) { - impl_->addEventLocked( - found->second.admission_state == DeviceAdmissionState::Open - ? SafetyReason::None - : SafetyReason::HardwareUnsafe, - device_id, - {}, - std::string("device admission state changed to ") + - toString(found->second.admission_state)); - } - impl_->state_changed.notify_all(); - return true; -} - -std::optional SafetyManager::advanceDeviceGeneration( - const std::string& device_id) -{ - const auto generation = impl_->snapshots.bumpGeneration(device_id); - if (!generation.has_value()) { - return std::nullopt; - } - std::shared_ptr endpoint; - { - std::lock_guard lock(impl_->mutex); - const auto found = impl_->devices.find(device_id); - if (found == impl_->devices.end()) { - return std::nullopt; - } - found->second.admission_state = DeviceAdmissionState::Observing; - endpoint = found->second.endpoint; - impl_->addEventLocked( - SafetyReason::GenerationMismatch, - device_id, - {}, - "device backend generation advanced; awaiting a fresh sample"); - impl_->state_changed.notify_all(); - } - if (endpoint) { - endpoint->onDeviceGenerationChanged(*generation); - endpoint->requestSafetyRefresh(); - } - return generation; -} - -StartupCoverageResult SafetyManager::validateStartupCoverage( - const SafetyClock::time_point deadline) -{ - struct Target { - DeviceSafetyDescriptor descriptor; - std::shared_ptr endpoint; - bool has_participant{false}; - }; - - StartupCoverageResult result; - std::vector targets; - std::unordered_set target_ids; - bool require_fresh_snapshot = false; - { - std::lock_guard lock(impl_->mutex); - const auto mode = impl_->config.enforcement_mode; - require_fresh_snapshot = - mode == EnforcementMode::EnforceSelected || - mode == EnforcementMode::EnforceAll; - - const auto add_target = [&](const Impl::DeviceSlot& slot) { - if (target_ids.insert(slot.descriptor.device_id).second) { - targets.push_back({ - slot.descriptor, - slot.endpoint, - static_cast(slot.participant)}); - } - }; - - if (mode == EnforcementMode::EnforceSelected) { - if (impl_->config.enforced_device_ids.empty()) { - result.issues.push_back({ - "system", - SafetyReason::InvalidArgument, - "ENFORCE_SELECTED requires at least one enforced_device_id"}); - } - for (const auto& id : impl_->config.enforced_device_ids) { - const auto found = impl_->devices.find(id); - if (found == impl_->devices.end()) { - result.issues.push_back({ - id, - SafetyReason::DeviceNotFound, - "enforced device is not registered"}); - continue; - } - add_target(found->second); - } - } - - if (mode == EnforcementMode::EnforceAll || - impl_->config.fail_startup_on_missing_control_capability) { - for (const auto& [id, slot] : impl_->devices) { - (void)id; - if (slot.descriptor.default_policy == - SafetyPolicyFamily::Control) { - add_target(slot); - } - } - } - } - - std::sort(targets.begin(), targets.end(), [](const auto& lhs, - const auto& rhs) { - return lhs.descriptor.device_id < rhs.descriptor.device_id; - }); - for (const auto& target : targets) { - if (!target.endpoint) { - result.issues.push_back({ - target.descriptor.device_id, - SafetyReason::SafetyStateMissing, - "enforced device has no final hardware safety endpoint"}); - } - if (target.descriptor.requires_safe_stop && - !target.has_participant) { - result.issues.push_back({ - target.descriptor.device_id, - SafetyReason::StopUnconfirmed, - "enforced device requires safe stop but has no participant"}); - } - if (require_fresh_snapshot && target.endpoint) { - target.endpoint->requestSafetyRefresh(); - } - } - - if (require_fresh_snapshot) { - for (;;) { - bool all_fresh = true; - for (const auto& target : targets) { - if (!target.endpoint) { - continue; - } - const auto view = impl_->snapshots.get( - target.descriptor.device_id); - if (!view.registered || !view.has_sample || !view.fresh) { - all_fresh = false; - break; - } - } - if (all_fresh || SafetyClock::now() >= deadline) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - } - - for (const auto& target : targets) { - if (!target.endpoint) { - continue; - } - const auto view = impl_->snapshots.get( - target.descriptor.device_id); - if (!view.registered || !view.has_sample) { - result.issues.push_back({ - target.descriptor.device_id, - SafetyReason::SafetyStateMissing, - "enforced device did not publish a startup safety snapshot"}); - } else if (!view.fresh) { - result.issues.push_back({ - target.descriptor.device_id, - SafetyReason::SafetyStateStale, - "enforced device startup safety snapshot is stale"}); - } - } - } - - std::sort(result.issues.begin(), result.issues.end(), - [](const auto& lhs, const auto& rhs) { - return lhs.target_id != rhs.target_id - ? lhs.target_id < rhs.target_id - : static_cast(lhs.reason) < - static_cast(rhs.reason); - }); - result.ready = result.issues.empty(); - return result; -} - -void SafetyManager::markStartupComplete() -{ - std::lock_guard lock(impl_->mutex); - if (impl_->system_state != SystemAdmissionState::Starting) { - return; - } - impl_->system_state = impl_->hasGlobalLatchLocked() - ? SystemAdmissionState::Latched - : SystemAdmissionState::Open; - impl_->addEventLocked( - impl_->system_state == SystemAdmissionState::Open - ? SafetyReason::None - : SafetyReason::SafetyLatched, - "system", - {}, - "startup safety reconciliation completed"); - impl_->state_changed.notify_all(); -} - -void SafetyManager::beginShutdown() noexcept -{ - if (!impl_) { - return; - } - try { - std::lock_guard lock(impl_->mutex); - if (impl_->system_state == SystemAdmissionState::ShuttingDown) { - return; - } - impl_->system_state = SystemAdmissionState::ShuttingDown; - ++impl_->safety_epoch; - impl_->addEventLocked( - SafetyReason::SystemStopping, - "system", - {}, - "safety coordinator is shutting down"); - impl_->state_changed.notify_all(); - } catch (...) { - } -} - -AdmissionDecision SafetyManager::evaluate( - const AdmissionRequest& request) const -{ - AdmissionDecision decision; - SystemAdmissionState system_state; - DeviceAdmissionState device_state{DeviceAdmissionState::Observing}; - std::uint64_t epoch = 0; - bool enforced = false; - bool device_exists = false; - { - std::lock_guard lock(impl_->mutex); - system_state = impl_->system_state; - epoch = impl_->safety_epoch; - enforced = impl_->isEnforced(request.device_id); - const auto found = impl_->devices.find(request.device_id); - if (found != impl_->devices.end()) { - device_exists = true; - device_state = found->second.admission_state; - } - } - decision.safety_epoch = epoch; - decision.enforced = enforced; - - const auto reject = [&](const SafetyReason reason, std::string detail) { - decision.policy_allowed = false; - decision.allowed = !decision.enforced; - decision.reason = reason; - decision.detail = std::move(detail); - return decision; - }; - const auto allow = [&]() { - decision.policy_allowed = true; - decision.allowed = true; - decision.reason = SafetyReason::None; - return decision; - }; - - if (request.command.intent == CommandIntent::Observe) { - return allow(); - } - if (request.command.intent == CommandIntent::Stop || - request.command.intent == CommandIntent::ResetFault || - request.command.intent == CommandIntent::RecoverAdmission || - request.command.safety_lane) { - return allow(); - } - if (request.deadline <= SafetyClock::now()) { - return reject( - SafetyReason::DeadlineExceededBeforeDispatch, - "command deadline expired before admission"); - } - if (system_state != SystemAdmissionState::Open) { - return reject( - system_state == SystemAdmissionState::Starting - ? SafetyReason::SystemStarting - : system_state == SystemAdmissionState::Stopping || - system_state == SystemAdmissionState::ShuttingDown - ? SafetyReason::SystemStopping - : SafetyReason::SafetyLatched, - std::string("system admission is ") + toString(system_state)); - } - if (request.device_id.empty() || !device_exists) { - return reject( - SafetyReason::DeviceNotFound, - "device is not registered with the safety coordinator"); - } - if (device_state == DeviceAdmissionState::Quarantined) { - return reject( - SafetyReason::SafetyLatched, - "device is quarantined after an uncertain result"); - } - - const auto view = impl_->snapshots.get(request.device_id); - if (!view.has_sample) { - return reject( - SafetyReason::SafetyStateMissing, - "device has not published a safety sample"); - } - decision.device_generation = view.snapshot.device_generation; - if (!view.fresh) { - return reject( - SafetyReason::SafetyStateStale, - "device safety sample is stale"); - } - if (request.expected_device_generation.has_value() && - *request.expected_device_generation != - view.snapshot.device_generation) { - return reject( - SafetyReason::GenerationMismatch, - "expected device generation does not match the active backend"); - } - if (view.snapshot.condition == SafetyCondition::Unknown) { - return reject( - SafetyReason::SafetyStateMissing, - "hardware safety condition is unknown"); - } - if (view.snapshot.connected != TriState::True) { - return reject( - SafetyReason::DeviceDisconnected, - "device connection is not confirmed"); - } - if (hasTrue(view.snapshot.emergency_stop_active)) { - return reject( - SafetyReason::EmergencyStopActive, - "hardware emergency stop is active"); - } - if (hasTrue(view.snapshot.protective_stop_active)) { - return reject( - SafetyReason::ProtectiveStopActive, - "hardware protective stop is active"); - } - if (hasTrue(view.snapshot.fault_active)) { - return reject(SafetyReason::DeviceFault, "device fault is active"); - } - if (view.snapshot.condition == SafetyCondition::Unsafe) { - return reject(SafetyReason::HardwareUnsafe, "hardware is unsafe"); - } - if (request.command.policy_family == SafetyPolicyFamily::Sensor) { - return allow(); - } - if (view.snapshot.emergency_stop_active != TriState::False) { - return reject( - SafetyReason::EmergencyStopActive, - "control device emergency stop state is active or unknown"); - } - if (view.snapshot.protective_stop_active != TriState::False) { - return reject( - SafetyReason::ProtectiveStopActive, - "control device protective stop state is active or unknown"); - } - if (view.snapshot.fault_active != TriState::False) { - return reject( - SafetyReason::DeviceFault, - "control device fault state is active or unknown"); - } - if (request.command.intent == CommandIntent::Actuate && - view.snapshot.condition != SafetyCondition::Nominal) { - return reject( - SafetyReason::HardwareUnsafe, - "control actuation requires nominal hardware safety state"); - } - const bool safe_start_from_restricted = - request.command.intent == CommandIntent::StartActivity && - view.snapshot.condition == SafetyCondition::Restricted; - if (isControlIntent(request.command.intent) && - view.snapshot.operational_ready != TriState::True && - !safe_start_from_restricted) { - return reject( - SafetyReason::DeviceNotReady, - "control device is not confirmed operationally ready"); - } - return allow(); -} - -AdmissionResult SafetyManager::admit(const AdmissionRequest& request) -{ - AdmissionResult result; - result.decision = evaluate(request); - if (!result.decision.allowed) { - return result; - } - AdmissionPermit permit; - permit.command_id = request.command_id; - permit.device_id = request.device_id; - permit.intent = request.command.intent; - permit.safety_epoch = result.decision.safety_epoch; - permit.device_generation = result.decision.device_generation; - permit.authority_generation = request.authority_generation; - permit.deadline = request.deadline; - permit.policy_allowed = result.decision.policy_allowed; - permit.enforced = result.decision.enforced; - result.permit.emplace(std::move(permit)); - return result; -} - -HardwareCheckResult SafetyManager::revalidatePermit( - const AdmissionPermit& permit) const -{ - const auto rejected = [](const SafetyReason reason, std::string detail) { - return HardwareCheckResult{false, reason, std::move(detail)}; - }; - const bool safety_lane = - permit.intent == CommandIntent::Stop || - permit.intent == CommandIntent::ResetFault || - permit.intent == CommandIntent::RecoverAdmission; - - std::lock_guard lock(impl_->mutex); - const auto found = impl_->devices.find(permit.device_id); - if (found == impl_->devices.end()) { - return rejected( - SafetyReason::DeviceNotFound, - "device was removed after session admission"); - } - if (permit.deadline <= SafetyClock::now()) { - return rejected( - SafetyReason::DeadlineExceededBeforeDispatch, - "session deadline expired before dispatch"); - } - - const auto view = impl_->snapshots.get(permit.device_id); - const bool safe_start_from_restricted = - permit.intent == CommandIntent::StartActivity && view.fresh && - view.snapshot.condition == SafetyCondition::Restricted; - if (permit.enforced && !safety_lane && - (impl_->system_state != SystemAdmissionState::Open || - permit.safety_epoch != impl_->safety_epoch || - (found->second.admission_state != DeviceAdmissionState::Open && - !safe_start_from_restricted))) { - return rejected( - impl_->system_state == SystemAdmissionState::Stopping || - impl_->system_state == SystemAdmissionState::ShuttingDown - ? SafetyReason::SystemStopping - : SafetyReason::SafetyLatched, - "safety admission changed after session open"); - } - if (permit.enforced && !safety_lane && !view.fresh) { - return rejected( - SafetyReason::SafetyStateStale, - "device safety sample became stale during the session"); - } - if (permit.enforced && !safety_lane && - permit.device_generation != view.snapshot.device_generation) { - return rejected( - SafetyReason::GenerationMismatch, - "device generation changed during the session"); - } - return {true, SafetyReason::None, {}}; -} - -DispatchGuard SafetyManager::beginDispatch( - const AdmissionPermit& permit) -{ - const auto rejected = [&permit]( - const SafetyReason reason, - std::string detail) { - return DispatchGuard( - nullptr, - permit.device_id, - HardwareCheckResult{false, reason, std::move(detail)}); - }; - const bool safety_lane = - permit.intent == CommandIntent::Stop || - permit.intent == CommandIntent::ResetFault || - permit.intent == CommandIntent::RecoverAdmission; - std::shared_ptr endpoint; - { - std::lock_guard lock(impl_->mutex); - const auto found = impl_->devices.find(permit.device_id); - if (found == impl_->devices.end()) { - return rejected( - SafetyReason::DeviceNotFound, - "device was removed before dispatch"); - } - if (permit.deadline <= SafetyClock::now()) { - return rejected( - SafetyReason::DeadlineExceededBeforeDispatch, - "command deadline expired before dispatch"); - } - const auto view = impl_->snapshots.get(permit.device_id); - const bool safe_start_from_restricted = - permit.intent == CommandIntent::StartActivity && view.fresh && - view.snapshot.condition == SafetyCondition::Restricted; - if (permit.enforced && !safety_lane && - (impl_->system_state != SystemAdmissionState::Open || - permit.safety_epoch != impl_->safety_epoch || - (found->second.admission_state != DeviceAdmissionState::Open && - !safe_start_from_restricted))) { - return rejected( - impl_->system_state == SystemAdmissionState::Stopping || - impl_->system_state == - SystemAdmissionState::ShuttingDown - ? SafetyReason::SystemStopping - : SafetyReason::SafetyLatched, - "safety admission changed before dispatch"); - } - if (permit.enforced && !safety_lane && !view.fresh) { - return rejected( - SafetyReason::SafetyStateStale, - "device safety sample became stale before dispatch"); - } - if (permit.enforced && !safety_lane && - permit.device_generation != view.snapshot.device_generation) { - return rejected( - SafetyReason::GenerationMismatch, - "device generation changed before dispatch"); - } - ++found->second.in_flight_dispatches; - endpoint = found->second.endpoint; - } - - HardwareCheckResult hardware_check{ - true, SafetyReason::None, {}}; - if (permit.enforced) { - if (!endpoint) { - hardware_check = { - false, - SafetyReason::SafetyStateMissing, - "device has no final hardware safety endpoint"}; - } else { - try { - hardware_check = endpoint->validateBeforeDispatch(permit); - } catch (const std::exception& error) { - hardware_check = { - false, SafetyReason::InternalError, error.what()}; - } catch (...) { - hardware_check = { - false, - SafetyReason::InternalError, - "final hardware check threw an unknown exception"}; - } - } - } - if (!hardware_check.safe) { - endDispatch_(permit.device_id); - return DispatchGuard( - nullptr, permit.device_id, std::move(hardware_check)); - } - return DispatchGuard(this, permit.device_id, std::move(hardware_check)); -} - -void SafetyManager::endDispatch_(const std::string& device_id) noexcept -{ - try { - std::lock_guard lock(impl_->mutex); - const auto found = impl_->devices.find(device_id); - if (found != impl_->devices.end() && - found->second.in_flight_dispatches != 0) { - --found->second.in_flight_dispatches; - impl_->state_changed.notify_all(); - } - } catch (...) { - } -} - -void SafetyManager::quarantineDevice( - const std::string& device_id, - const SafetyReason reason, - std::string operation_id) -{ - std::lock_guard lock(impl_->mutex); - const auto found = impl_->devices.find(device_id); - if (found == impl_->devices.end()) { - return; - } - found->second.quarantined = true; - found->second.admission_state = DeviceAdmissionState::Quarantined; - found->second.software_blockers.push_back(makeBlocker( - reason, - BlockerScope::Device, - RecoveryRequirement::ClearSoftwareLatch, - device_id, - operation_id)); - impl_->addEventLocked( - reason, - device_id, - std::move(operation_id), - "device quarantined"); - impl_->state_changed.notify_all(); -} - -StopAllResult SafetyManager::stopAll( - std::string operation_id, - SafetyClock::time_point deadline) -{ - if (operation_id.empty()) { - operation_id = makeOperationId("stop"); - } - if (deadline == SafetyClock::time_point::max()) { - deadline = SafetyClock::now() + impl_->config.stop_all_timeout; - } - - std::shared_ptr round; - bool runner = false; - { - std::lock_guard lock(impl_->rounds_mutex); - if (impl_->active_stop_round && - !impl_->active_stop_round->done.load(std::memory_order_acquire)) { - round = impl_->active_stop_round; - } else { - round = std::make_shared(); - round->result.operation_id = std::move(operation_id); - impl_->active_stop_round = round; - runner = true; - } - } - - if (!runner) { - std::unique_lock lock(round->mutex); - if (!round->completed.wait_until( - lock, deadline, [&round] { - return round->done.load(std::memory_order_acquire); - })) { - StopAllResult timeout; - timeout.operation_id = round->result.operation_id; - timeout.success = false; - timeout.system_state = SystemAdmissionState::Stopping; - timeout.targets.push_back({ - "system", - false, - SafetyReason::ParticipantTimeout, - "caller timed out while joining the active StopAll round"}); - return timeout; - } - return round->result; - } - - StopAllResult result; - result.operation_id = round->result.operation_id; - std::unique_lock operation_lock(impl_->operation_execution_mutex); - - struct Work { - ParticipantDescriptor descriptor; - std::shared_ptr participant; - BarrierToken token; - std::future request_future; - std::future verify_future; - SafetyClock::time_point participant_deadline{ - SafetyClock::time_point::max()}; - bool request_completed{false}; - bool result_recorded{false}; - bool reused_retained_barrier{false}; - DeviceAdmissionState before{DeviceAdmissionState::Observing}; - }; - std::vector work; - { - std::lock_guard lock(impl_->mutex); - result.previous_safety_epoch = impl_->safety_epoch; - impl_->system_state = SystemAdmissionState::Stopping; - ++impl_->safety_epoch; - result.current_safety_epoch = impl_->safety_epoch; - impl_->active_operation_id = result.operation_id; - impl_->active_operation_phase = "barrier"; - impl_->addEventLocked( - SafetyReason::SystemStopping, - "system", - result.operation_id, - "StopAll entered the stopping state"); - work.reserve( - impl_->participants.size() + impl_->retained_barriers.size()); - for (const auto& [id, participant] : impl_->participants) { - Work item; - const auto retained = impl_->retained_barriers.find(id); - if (retained != impl_->retained_barriers.end()) { - item.participant = retained->second.participant; - item.token = retained->second.token; - item.reused_retained_barrier = true; - } else { - item.participant = participant; - } - item.descriptor = item.participant->descriptor(); - const auto device = impl_->devices.find(id); - if (device != impl_->devices.end()) { - item.before = device->second.admission_state; - } - work.push_back(std::move(item)); - } - for (const auto& [id, retained] : impl_->retained_barriers) { - if (impl_->participants.count(id) != 0) { - continue; - } - Work item; - item.participant = retained.participant; - item.descriptor = item.participant->descriptor(); - item.token = retained.token; - item.reused_retained_barrier = true; - const auto device = impl_->devices.find(id); - if (device != impl_->devices.end()) { - item.before = device->second.admission_state; - } - work.push_back(std::move(item)); - } - } - std::sort(work.begin(), work.end(), [](const auto& lhs, const auto& rhs) { - const auto left_phase = phaseRank(lhs.descriptor.phase); - const auto right_phase = phaseRank(rhs.descriptor.phase); - return left_phase != right_phase - ? left_phase < right_phase - : lhs.descriptor.participant_id < rhs.descriptor.participant_id; - }); - - SafetyOperationContext context{ - result.operation_id, - result.current_safety_epoch, - deadline}; - bool required_success = true; - for (auto& item : work) { - if (item.reused_retained_barrier) { - continue; - } - try { - item.token = item.participant->beginBarrier(context); - } catch (const std::exception& error) { - result.targets.push_back({ - item.descriptor.participant_id, - false, - SafetyReason::InternalError, - error.what(), - item.before, - item.before}); - item.result_recorded = true; - } catch (...) { - result.targets.push_back({ - item.descriptor.participant_id, - false, - SafetyReason::InternalError, - "beginBarrier threw an unknown exception", - item.before, - item.before}); - item.result_recorded = true; - } - if (!item.token.valid()) { - if (!item.result_recorded) { - result.targets.push_back({ - item.descriptor.participant_id, - false, - SafetyReason::StopUnconfirmed, - "participant did not establish its safety barrier", - item.before, - item.before}); - item.result_recorded = true; - } - if (item.descriptor.required) { - required_success = false; - } - continue; - } - { - std::lock_guard lock(impl_->mutex); - auto& runtime = impl_->participant_runtime[ - item.descriptor.participant_id]; - runtime.descriptor = item.descriptor; - runtime.registered = - impl_->participants.count( - item.descriptor.participant_id) != 0U; - runtime.barrier_active = true; - runtime.barrier_retained = item.reused_retained_barrier; - runtime.operation_id = item.token.operation_id; - runtime.safety_epoch = item.token.safety_epoch; - runtime.last_request = {}; - runtime.last_verify = {}; - runtime.last_release = {}; - } - } - - // Dispatch every physical stop request before waiting for a slow task, - // session, or driver. Phase ordering is applied to verification below. - for (int phase = 0; phase <= 5; ++phase) { - { - std::lock_guard lock(impl_->mutex); - impl_->active_operation_phase = - "request-phase-" + std::to_string(phase); - } - for (auto& item : work) { - if (phaseRank(item.descriptor.phase) != phase || - !item.token.valid()) { - continue; - } - const auto participant = item.participant; - const auto token = item.token; - item.participant_deadline = std::min( - deadline, SafetyClock::now() + item.descriptor.timeout); - item.request_future = impl_->operation_executor.submit( - [participant, token, context] { - return participant->requestQuiesce(token, context); - }); - } - } - - const auto record_result = [&](Work& item, - ParticipantResult participant_result) { - DeviceAdmissionState after = item.before; - { - std::lock_guard lock(impl_->mutex); - const auto device = impl_->devices.find( - item.descriptor.participant_id); - if (device != impl_->devices.end()) { - if (!participant_result.success && - item.descriptor.required) { - device->second.quarantined = true; - device->second.admission_state = - DeviceAdmissionState::Quarantined; - device->second.software_blockers.push_back(makeBlocker( - participant_result.reason, - BlockerScope::Device, - RecoveryRequirement::ClearSoftwareLatch, - item.descriptor.participant_id, - result.operation_id)); - } - after = device->second.admission_state; - } - } - result.targets.push_back({ - item.descriptor.participant_id, - participant_result.success, - participant_result.reason, - std::move(participant_result.detail), - item.before, - after}); - item.result_recorded = true; - if (!participant_result.success && item.descriptor.required) { - required_success = false; - } - }; - - const auto record_participant_result = [this]( - const std::string& participant_id, - const char* stage, - const ParticipantResult& participant_result) { - std::lock_guard lock(impl_->mutex); - const auto found = impl_->participant_runtime.find(participant_id); - if (found == impl_->participant_runtime.end()) { - return; - } - ParticipantResultView value{ - true, - participant_result.success, - participant_result.reason, - participant_result.detail}; - if (std::strcmp(stage, "request") == 0) { - found->second.last_request = std::move(value); - } else if (std::strcmp(stage, "verify") == 0) { - found->second.last_verify = std::move(value); - } else { - found->second.last_release = std::move(value); - } - }; - - for (int phase = 0; phase <= 5; ++phase) { - for (auto& item : work) { - if (phaseRank(item.descriptor.phase) != phase || - !item.token.valid() || !item.request_future.valid()) { - continue; - } - if (item.request_future.wait_until(item.participant_deadline) != - std::future_status::ready) { - const ParticipantResult timed_out{ - false, - SafetyReason::ParticipantTimeout, - "participant did not dispatch its stop request before its deadline"}; - record_participant_result( - item.descriptor.participant_id, "request", timed_out); - record_result(item, timed_out); - continue; - } - const auto requested = item.request_future.get(); - record_participant_result( - item.descriptor.participant_id, "request", requested); - item.request_completed = true; - if (!requested.success) { - // A completed initial failure still receives a final stop in - // verification. This preserves the retry-after-fence safety - // contract while retaining fail-closed timeout behavior. - item.request_completed = true; - } - } - } - - // Verification is phase ordered so scheduler/session fences are drained - // before actuator and peripheral participants perform their final stop. - for (int phase = 0; phase <= 5; ++phase) { - { - std::lock_guard lock(impl_->mutex); - impl_->active_operation_phase = - "verify-phase-" + std::to_string(phase); - } - for (auto& item : work) { - if (phaseRank(item.descriptor.phase) != phase || - !item.token.valid() || !item.request_completed || - item.result_recorded) { - continue; - } - const auto participant = item.participant; - const auto token = item.token; - item.verify_future = impl_->operation_executor.submit( - [participant, token, context] { - return participant->verifyQuiescent(token, context); - }); - } - for (auto& item : work) { - if (phaseRank(item.descriptor.phase) != phase || - !item.verify_future.valid() || item.result_recorded) { - continue; - } - if (item.verify_future.wait_until(item.participant_deadline) != - std::future_status::ready) { - const ParticipantResult timed_out{ - false, - SafetyReason::ParticipantTimeout, - "participant did not confirm quiescence before its deadline"}; - record_participant_result( - item.descriptor.participant_id, "verify", timed_out); - record_result(item, timed_out); - } else { - const auto verified = item.verify_future.get(); - record_participant_result( - item.descriptor.participant_id, "verify", verified); - record_result(item, verified); - } - } - } - - { - std::unique_lock lock(impl_->mutex); - impl_->active_operation_phase = "dispatch-fence"; - const auto no_in_flight = [&] { - return std::all_of( - impl_->devices.begin(), impl_->devices.end(), - [](const auto& item) { - return item.second.in_flight_dispatches == 0; - }); - }; - if (!impl_->state_changed.wait_until(lock, deadline, no_in_flight)) { - required_success = false; - result.targets.push_back({ - "dispatch-fence", - false, - SafetyReason::ParticipantTimeout, - "an admitted command was still inside its device submission boundary"}); - } - } - - const auto retain_barrier = [&](const Work& item) { - std::lock_guard lock(impl_->mutex); - impl_->retained_barriers[item.descriptor.participant_id] = { - item.participant, item.token}; - auto& runtime = impl_->participant_runtime[ - item.descriptor.participant_id]; - runtime.descriptor = item.descriptor; - runtime.barrier_active = true; - runtime.barrier_retained = true; - runtime.operation_id = item.token.operation_id; - runtime.safety_epoch = item.token.safety_epoch; - }; - const auto forget_retained_barrier = [&](const Work& item) { - std::lock_guard lock(impl_->mutex); - const auto found = impl_->retained_barriers.find( - item.descriptor.participant_id); - if (found != impl_->retained_barriers.end() && - found->second.token.operation_id == item.token.operation_id && - found->second.token.safety_epoch == item.token.safety_epoch && - found->second.token.generation == item.token.generation) { - impl_->retained_barriers.erase(found); - } - const auto runtime = impl_->participant_runtime.find( - item.descriptor.participant_id); - if (runtime != impl_->participant_runtime.end() && - runtime->second.operation_id == item.token.operation_id && - runtime->second.safety_epoch == item.token.safety_epoch) { - runtime->second.barrier_active = false; - runtime->second.barrier_retained = false; - if (!runtime->second.registered) { - impl_->participant_runtime.erase(runtime); - } - } - }; - const auto mark_release_failure = [&](Work& item, - ParticipantResult released) { - if (released.reason == SafetyReason::None) { - released.reason = SafetyReason::StopUnconfirmed; - } - const auto target = std::find_if( - result.targets.begin(), result.targets.end(), - [&item](const auto& candidate) { - return candidate.target_id == - item.descriptor.participant_id; - }); - if (target != result.targets.end()) { - target->success = false; - target->reason = released.reason; - target->detail = std::move(released.detail); - } else { - result.targets.push_back({ - item.descriptor.participant_id, - false, - released.reason, - std::move(released.detail), - item.before, - item.before}); - } - if (item.descriptor.required) { - required_success = false; - std::lock_guard lock(impl_->mutex); - const auto device = impl_->devices.find( - item.descriptor.participant_id); - if (device != impl_->devices.end()) { - device->second.quarantined = true; - device->second.admission_state = - DeviceAdmissionState::Quarantined; - device->second.software_blockers.push_back(makeBlocker( - released.reason, - BlockerScope::Device, - RecoveryRequirement::ClearSoftwareLatch, - item.descriptor.participant_id, - result.operation_id)); - } - } - }; - - // Commit admission reopening in reverse phase order. Ingress is therefore - // the final required barrier released; a failed subsystem commit keeps it - // closed and prevents a transient normal-command admission window. - for (auto current = work.rbegin(); current != work.rend(); ++current) { - auto& item = *current; - if (!item.token.valid()) { - continue; - } - const auto target = std::find_if( - result.targets.begin(), result.targets.end(), - [&item](const auto& candidate) { - return candidate.target_id == - item.descriptor.participant_id; - }); - const bool participant_verified = - target != result.targets.end() && target->success; - if (item.descriptor.required && - (!required_success || !participant_verified)) { - retain_barrier(item); - continue; - } - - const auto released = item.participant->releaseBarrier(item.token); - record_participant_result( - item.descriptor.participant_id, "release", released); - if (!released.success) { - mark_release_failure(item, released); - if (item.descriptor.required) { - retain_barrier(item); - } - } else { - forget_retained_barrier(item); - } - } - - { - std::lock_guard lock(impl_->mutex); - result.success = required_success; - impl_->system_state = required_success - ? SystemAdmissionState::Open - : SystemAdmissionState::Latched; - result.system_state = impl_->system_state; - impl_->active_operation_id.clear(); - impl_->active_operation_phase.clear(); - impl_->addEventLocked( - required_success - ? SafetyReason::None - : SafetyReason::StopUnconfirmed, - "system", - result.operation_id, - required_success - ? "StopAll confirmed all required participants quiescent" - : "StopAll left admission latched because a required participant was unconfirmed"); - impl_->state_changed.notify_all(); - } - - { - std::lock_guard lock(round->mutex); - round->result = result; - round->done.store(true, std::memory_order_release); - } - round->completed.notify_all(); - { - std::lock_guard lock(impl_->rounds_mutex); - if (impl_->active_stop_round == round) { - impl_->active_stop_round.reset(); - } - } - return result; -} - -RecoveryResult SafetyManager::recover(const RecoveryRequest& request) -{ - RecoveryResult invalid; - invalid.recovery_id = request.recovery_id; - if (request.recovery_id.empty() || request.reason.empty() || - (!request.all_devices && request.device_ids.empty())) { - invalid.result = RecoveryResultCode::Failed; - invalid.targets.push_back({ - "system", - false, - request.reason.empty() - ? SafetyReason::RecoveryReasonRequired - : SafetyReason::InvalidArgument, - "recovery_id, reason, and an explicit non-empty scope are required"}); - return invalid; - } - std::unordered_set unique_ids; - for (const auto& id : request.device_ids) { - if (id.empty() || !unique_ids.insert(id).second) { - invalid.result = RecoveryResultCode::Failed; - invalid.targets.push_back({ - id, - false, - SafetyReason::InvalidArgument, - "recovery device IDs must be non-empty and unique"}); - return invalid; - } - } - - std::vector fingerprint_ids = request.device_ids; - std::sort(fingerprint_ids.begin(), fingerprint_ids.end()); - std::ostringstream fingerprint; - fingerprint << request.expected_safety_epoch << ':' - << request.verify_only << ':' << request.all_devices << ':' - << request.reason; - for (const auto& id : fingerprint_ids) { - fingerprint << ':' << id; - } - - std::shared_ptr round; - bool runner = false; - { - std::lock_guard lock(impl_->rounds_mutex); - const auto found = impl_->recovery_rounds.find(request.recovery_id); - if (found != impl_->recovery_rounds.end()) { - round = found->second; - if (round->fingerprint != fingerprint.str()) { - invalid.result = RecoveryResultCode::Failed; - invalid.targets.push_back({ - "system", - false, - SafetyReason::CommandIdConflict, - "recovery_id is already associated with a different request"}); - return invalid; - } - } else { - round = std::make_shared(); - round->fingerprint = fingerprint.str(); - impl_->recovery_rounds.emplace(request.recovery_id, round); - runner = true; - } - } - - auto deadline = request.deadline; - if (deadline == SafetyClock::time_point::max()) { - deadline = SafetyClock::now() + impl_->config.recovery_timeout; - } - if (!runner) { - std::unique_lock lock(round->mutex); - if (!round->completed.wait_until( - lock, deadline, [&round] { - return round->done.load(std::memory_order_acquire); - })) { - invalid.result = RecoveryResultCode::TimedOut; - invalid.targets.push_back({ - "system", - false, - SafetyReason::ParticipantTimeout, - "caller timed out while joining the recovery transaction"}); - return invalid; - } - return round->result; - } - - RecoveryResult result; - result.recovery_id = request.recovery_id; - std::unique_lock operation_lock(impl_->operation_execution_mutex); - std::vector target_ids; - std::unordered_map - target_before_states; - SystemAdmissionState state_before_recovery{ - SystemAdmissionState::Starting}; - { - std::lock_guard lock(impl_->mutex); - result.previous_safety_epoch = impl_->safety_epoch; - state_before_recovery = impl_->system_state; - if (request.expected_safety_epoch != impl_->safety_epoch) { - result.result = RecoveryResultCode::EpochMismatch; - result.current_safety_epoch = impl_->safety_epoch; - result.system_state = impl_->system_state; - result.targets.push_back({ - "system", - false, - SafetyReason::RecoveryEpochMismatch, - "expected safety epoch does not match the active state"}); - } else if (impl_->system_state == - SystemAdmissionState::ShuttingDown) { - result.result = RecoveryResultCode::Failed; - result.current_safety_epoch = impl_->safety_epoch; - result.system_state = impl_->system_state; - result.targets.push_back({ - "system", - false, - SafetyReason::SystemStopping, - "recovery is unavailable during shutdown"}); - } else { - impl_->system_state = SystemAdmissionState::Recovering; - ++impl_->safety_epoch; - result.current_safety_epoch = impl_->safety_epoch; - impl_->active_operation_id = request.recovery_id; - impl_->active_operation_phase = "refresh"; - if (request.all_devices) { - target_ids.reserve(impl_->devices.size()); - for (auto& [id, slot] : impl_->devices) { - target_ids.push_back(id); - target_before_states.emplace(id, slot.admission_state); - if (slot.admission_state == - DeviceAdmissionState::Quarantined || - slot.admission_state == - DeviceAdmissionState::Blocked) { - slot.admission_state = - DeviceAdmissionState::Recovering; - } - } - } else { - target_ids = request.device_ids; - for (const auto& id : target_ids) { - const auto found = impl_->devices.find(id); - if (found != impl_->devices.end()) { - target_before_states.emplace( - id, found->second.admission_state); - } - if (found != impl_->devices.end() && - (found->second.admission_state == - DeviceAdmissionState::Quarantined || - found->second.admission_state == - DeviceAdmissionState::Blocked)) { - found->second.admission_state = - DeviceAdmissionState::Recovering; - } - } - } - impl_->addEventLocked( - SafetyReason::None, - "system", - request.recovery_id, - request.verify_only - ? "recovery verification started" - : "software latch recovery started"); - } - } - - if (!result.targets.empty()) { - std::lock_guard lock(round->mutex); - round->result = result; - round->done.store(true, std::memory_order_release); - round->completed.notify_all(); - return result; - } - - struct Target { - std::string id; - std::shared_ptr endpoint; - DeviceAdmissionState before{DeviceAdmissionState::Observing}; - std::uint64_t previous_sequence{0}; - SafetyPolicyFamily policy{SafetyPolicyFamily::Sensor}; - }; - std::vector targets; - { - std::lock_guard lock(impl_->mutex); - for (const auto& id : target_ids) { - const auto found = impl_->devices.find(id); - if (found == impl_->devices.end()) { - result.targets.push_back({ - id, - false, - SafetyReason::DeviceNotFound, - "device is not registered", - DeviceAdmissionState::Removed, - DeviceAdmissionState::Removed}); - continue; - } - const auto view = impl_->snapshots.get(id); - const auto before = target_before_states.find(id); - targets.push_back({ - id, - found->second.endpoint, - before == target_before_states.end() - ? found->second.admission_state - : before->second, - view.snapshot.sample_sequence, - found->second.descriptor.default_policy}); - } - } - - for (const auto& target : targets) { - if (target.endpoint) { - target.endpoint->requestSafetyRefresh(); - } - } - - bool all_verified = result.targets.empty(); - RecoveryContext recovery_context{ - request.recovery_id, - request.reason, - result.current_safety_epoch, - deadline, - request.verify_only}; - for (const auto& target : targets) { - SafetyTargetResult target_result; - target_result.target_id = target.id; - target_result.before_state = target.before; - target_result.after_state = target.before; - - SafetySnapshotView view; - if (!target.endpoint) { - target_result.reason = SafetyReason::SafetyStateMissing; - target_result.detail = "device has no safety endpoint"; - } else if (!impl_->snapshots.waitForNewerSample( - target.id, - target.previous_sequence, - deadline, - view)) { - target_result.reason = SafetyReason::SafetyStateStale; - target_result.detail = - "active safety refresh did not publish a newer sample"; - } else if (!view.fresh) { - target_result.reason = SafetyReason::SafetyStateStale; - target_result.detail = "refreshed safety sample is stale"; - } else if (view.snapshot.condition == SafetyCondition::Unknown) { - target_result.reason = SafetyReason::SafetyStateMissing; - target_result.detail = "hardware safety condition remains unknown"; - } else if (view.snapshot.condition == SafetyCondition::Unsafe) { - target_result.reason = SafetyReason::HardwareUnsafe; - target_result.detail = "hardware safety condition remains unsafe"; - } else if (view.snapshot.connected != TriState::True) { - target_result.reason = SafetyReason::DeviceDisconnected; - target_result.detail = "device connection is not confirmed"; - } else if (view.snapshot.emergency_stop_active != TriState::False) { - target_result.reason = SafetyReason::EmergencyStopActive; - target_result.detail = - "emergency stop must be released and observed as false"; - } else if (view.snapshot.protective_stop_active != TriState::False) { - target_result.reason = SafetyReason::ProtectiveStopActive; - target_result.detail = - "protective stop must be resolved by the typed hardware flow"; - } else if (view.snapshot.fault_active != TriState::False) { - target_result.reason = SafetyReason::DeviceFault; - target_result.detail = "device fault remains active or unknown"; - } else if (target.policy == SafetyPolicyFamily::Control && - view.snapshot.quiescent != TriState::True) { - target_result.reason = SafetyReason::DeviceStillMoving; - target_result.detail = - "control device has not confirmed a quiescent state"; - } else { - target_result.success = true; - target_result.reason = SafetyReason::None; - } - - if (!target_result.success) { - all_verified = false; - } - result.targets.push_back(std::move(target_result)); - } - - std::vector retained_candidate_ids; - if (!request.verify_only) { - const bool has_device_clear_candidate = std::any_of( - result.targets.begin(), result.targets.end(), - [](const auto& target) { return target.success; }); - { - std::lock_guard lock(impl_->mutex); - retained_candidate_ids.reserve( - impl_->retained_barriers.size()); - for (const auto& [id, retained] : impl_->retained_barriers) { - (void)retained; - const auto device = impl_->devices.find(id); - if (device == impl_->devices.end()) { - retained_candidate_ids.push_back(id); - continue; - } - const auto target = std::find_if( - result.targets.begin(), result.targets.end(), - [&id](const auto& candidate) { - return candidate.target_id == id && - candidate.success; - }); - if (target != result.targets.end()) { - retained_candidate_ids.push_back(id); - } - } - } - const bool has_clear_candidate = - has_device_clear_candidate || - !retained_candidate_ids.empty(); - bool clear_authorized = true; - if (has_clear_candidate && request.authorize_clear) { - try { - clear_authorized = request.authorize_clear(); - } catch (...) { - clear_authorized = false; - } - } - if (!clear_authorized) { - for (auto& target : result.targets) { - if (!target.success) { - continue; - } - target.success = false; - target.reason = SafetyReason::RecoveryAuditFailed; - target.detail = - "persistent recovery audit did not authorize latch clear"; - } - for (const auto& id : retained_candidate_ids) { - const auto existing = std::find_if( - result.targets.begin(), result.targets.end(), - [&id](const auto& target) { - return target.target_id == id; - }); - if (existing == result.targets.end()) { - result.targets.push_back({ - id, - false, - SafetyReason::RecoveryAuditFailed, - "persistent recovery audit did not authorize subsystem latch clear", - DeviceAdmissionState::Quarantined, - DeviceAdmissionState::Quarantined}); - } - } - all_verified = false; - } else { - for (auto& target_result : result.targets) { - if (!target_result.success) { - continue; - } - const auto target = std::find_if( - targets.begin(), targets.end(), - [&target_result](const auto& candidate) { - return candidate.id == target_result.target_id; - }); - if (target == targets.end() || !target->endpoint) { - target_result.success = false; - target_result.reason = SafetyReason::SafetyStateMissing; - target_result.detail = - "device recovery endpoint disappeared"; - all_verified = false; - continue; - } - - RecoveryCheckResult reconciled{ - true, SafetyReason::None, {}}; - try { - reconciled = target->endpoint->reconcileAdmissionState( - recovery_context); - } catch (const std::exception& error) { - reconciled = { - false, SafetyReason::InternalError, error.what()}; - } catch (...) { - reconciled = { - false, - SafetyReason::InternalError, - "device recovery reconciliation threw an unknown exception"}; - } - target_result.success = reconciled.reconciled; - target_result.reason = reconciled.reason; - target_result.detail = reconciled.detail; - if (!target_result.success) { - all_verified = false; - } - } - } - } - - struct BarrierRecovery { - std::size_t target_index{0}; - std::shared_ptr participant; - BarrierToken token; - }; - std::vector barrier_recoveries; - if (!request.verify_only) { - std::lock_guard lock(impl_->mutex); - for (const auto& id : retained_candidate_ids) { - const auto barrier = impl_->retained_barriers.find(id); - if (barrier == impl_->retained_barriers.end()) { - continue; - } - auto target = std::find_if( - result.targets.begin(), result.targets.end(), - [&id](const auto& candidate) { - return candidate.target_id == id; - }); - if (target == result.targets.end()) { - result.targets.push_back({ - id, - true, - SafetyReason::None, - {}, - DeviceAdmissionState::Quarantined, - DeviceAdmissionState::Quarantined}); - target = std::prev(result.targets.end()); - } - if (!target->success) { - continue; - } - barrier_recoveries.push_back({ - static_cast( - std::distance(result.targets.begin(), target)), - barrier->second.participant, - barrier->second.token}); - } - } - for (const auto& pending : barrier_recoveries) { - RecoveryCheckResult recovered; - try { - recovered = pending.participant->recoverAdmission( - pending.token, recovery_context); - } catch (const std::exception& error) { - recovered = {false, SafetyReason::InternalError, error.what()}; - } catch (...) { - recovered = { - false, - SafetyReason::InternalError, - "participant recovery threw an unknown exception"}; - } - if (!recovered.reconciled) { - auto& target = result.targets[pending.target_index]; - target.success = false; - target.reason = recovered.reason; - target.detail = recovered.detail; - all_verified = false; - continue; - } - - bool release = false; - { - std::lock_guard lock(impl_->mutex); - const auto found = impl_->retained_barriers.find( - pending.token.participant_id); - if (found != impl_->retained_barriers.end() && - found->second.token.operation_id == pending.token.operation_id && - found->second.token.safety_epoch == pending.token.safety_epoch && - found->second.token.generation == pending.token.generation) { - release = true; - } - } - if (release) { - const auto released = - pending.participant->releaseBarrier(pending.token); - { - std::lock_guard lock(impl_->mutex); - const auto runtime = impl_->participant_runtime.find( - pending.token.participant_id); - if (runtime != impl_->participant_runtime.end()) { - runtime->second.last_release = { - true, - released.success, - released.reason, - released.detail}; - } - } - if (!released.success) { - auto& target = result.targets[pending.target_index]; - target.success = false; - target.reason = released.reason == SafetyReason::None - ? SafetyReason::StopUnconfirmed - : released.reason; - target.detail = released.detail; - all_verified = false; - continue; - } - { - std::lock_guard lock(impl_->mutex); - const auto found = impl_->retained_barriers.find( - pending.token.participant_id); - if (found != impl_->retained_barriers.end() && - found->second.token.operation_id == - pending.token.operation_id && - found->second.token.safety_epoch == - pending.token.safety_epoch && - found->second.token.generation == - pending.token.generation) { - impl_->retained_barriers.erase(found); - const auto runtime = impl_->participant_runtime.find( - pending.token.participant_id); - if (runtime != impl_->participant_runtime.end()) { - runtime->second.barrier_active = false; - runtime->second.barrier_retained = false; - if (!runtime->second.registered) { - impl_->participant_runtime.erase(runtime); - } - } - } - } - result.targets[pending.target_index].after_state = - DeviceAdmissionState::Open; - } - } - - { - std::lock_guard lock(impl_->mutex); - impl_->active_operation_phase = "reconcile"; - for (auto& target : result.targets) { - const auto device = impl_->devices.find(target.target_id); - if (device == impl_->devices.end()) { - continue; - } - if (target.success && !request.verify_only) { - device->second.quarantined = false; - device->second.software_blockers.clear(); - } - const auto view = impl_->snapshots.get(target.target_id); - device->second.admission_state = - impl_->admissionStateFor(device->second, view); - target.after_state = device->second.admission_state; - } - - const bool latch_remains = impl_->hasGlobalLatchLocked(); - if (request.verify_only) { - impl_->system_state = state_before_recovery == - SystemAdmissionState::Open && - !latch_remains - ? SystemAdmissionState::Open - : SystemAdmissionState::Latched; - result.result = all_verified - ? RecoveryResultCode::VerifiedButStillBlocked - : RecoveryResultCode::BlockerRemains; - } else if (all_verified && !latch_remains) { - impl_->system_state = SystemAdmissionState::Open; - result.result = RecoveryResultCode::Recovered; - } else { - impl_->system_state = SystemAdmissionState::Latched; - result.result = RecoveryResultCode::BlockerRemains; - } - result.system_state = impl_->system_state; - impl_->active_operation_id.clear(); - impl_->active_operation_phase.clear(); - impl_->addEventLocked( - result.result == RecoveryResultCode::Recovered - ? SafetyReason::None - : SafetyReason::SafetyLatched, - "system", - request.recovery_id, - std::string("recovery completed with result ") + - toString(result.result)); - impl_->state_changed.notify_all(); - } - - if (targets.empty() && result.targets.empty()) { - result.result = RecoveryResultCode::NothingToRecover; - } - { - std::lock_guard lock(round->mutex); - round->result = result; - round->done.store(true, std::memory_order_release); - } - round->completed.notify_all(); - return result; -} - -SafetyManagerSnapshot SafetyManager::snapshot() const -{ - SafetyManagerSnapshot result; - std::lock_guard lock(impl_->mutex); - result.system_state = impl_->system_state; - result.safety_epoch = impl_->safety_epoch; - result.service_instance_id = impl_->service_instance_id; - result.enforcement_mode = impl_->config.enforcement_mode; - result.active_operation_id = impl_->active_operation_id; - result.active_operation_phase = impl_->active_operation_phase; - result.devices.reserve(impl_->devices.size()); - for (const auto& [id, slot] : impl_->devices) { - DeviceSafetyStateView view; - view.descriptor = slot.descriptor; - view.safety = impl_->snapshots.get(id); - view.lifecycle = slot.lifecycle; - view.health = slot.health; - view.admission_state = slot.admission_state; - view.blockers = view.safety.snapshot.blockers; - view.blockers.insert( - view.blockers.end(), - slot.software_blockers.begin(), - slot.software_blockers.end()); - result.devices.push_back(std::move(view)); - } - std::sort(result.devices.begin(), result.devices.end(), - [](const auto& lhs, const auto& rhs) { - return lhs.descriptor.device_id < rhs.descriptor.device_id; - }); - result.participants.reserve(impl_->participant_runtime.size()); - for (const auto& [id, runtime] : impl_->participant_runtime) { - (void)id; - ParticipantSafetyStateView view; - view.descriptor = runtime.descriptor; - view.registered = runtime.registered; - view.barrier_active = runtime.barrier_active; - view.barrier_retained = runtime.barrier_retained; - view.operation_id = runtime.operation_id; - view.safety_epoch = runtime.safety_epoch; - view.last_request = runtime.last_request; - view.last_verify = runtime.last_verify; - view.last_release = runtime.last_release; - result.participants.push_back(std::move(view)); - } - std::sort(result.participants.begin(), result.participants.end(), - [](const auto& lhs, const auto& rhs) { - return lhs.descriptor.participant_id < - rhs.descriptor.participant_id; - }); - result.recent_events.assign(impl_->events.begin(), impl_->events.end()); - return result; -} - -SafetySnapshotStore& SafetyManager::snapshotStore() noexcept -{ - return impl_->snapshots; -} - -const SafetySnapshotStore& SafetyManager::snapshotStore() const noexcept -{ - return impl_->snapshots; -} - -CommandLedger& SafetyManager::commandLedger() noexcept -{ - return impl_->ledger; -} - -const CommandLedger& SafetyManager::commandLedger() const noexcept -{ - return impl_->ledger; -} - -const std::string& SafetyManager::serviceInstanceId() const noexcept -{ - return impl_->service_instance_id; -} - -const SafetyManagerConfig& SafetyManager::config() const noexcept -{ - return impl_->config; -} - -const char* toString(const RecoveryResultCode value) noexcept -{ - switch (value) { - case RecoveryResultCode::Recovered: return "Recovered"; - case RecoveryResultCode::VerifiedButStillBlocked: - return "VerifiedButStillBlocked"; - case RecoveryResultCode::BlockerRemains: return "BlockerRemains"; - case RecoveryResultCode::EpochMismatch: return "EpochMismatch"; - case RecoveryResultCode::NothingToRecover: return "NothingToRecover"; - case RecoveryResultCode::TimedOut: return "TimedOut"; - case RecoveryResultCode::Failed: return "Failed"; - } - return "Failed"; -} - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/src/safety_reason.cpp b/cmvr-es/manager/safety_manager/src/safety_reason.cpp deleted file mode 100644 index 87e22e39..00000000 --- a/cmvr-es/manager/safety_manager/src/safety_reason.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include "manager/safety_manager/include/safety_reason.h" - -namespace cmvr::safety { - -const char* toString(const SafetyReason reason) noexcept -{ - switch (reason) { - case SafetyReason::None: return "NONE"; - case SafetyReason::InvalidArgument: return "INVALID_ARGUMENT"; - case SafetyReason::Unauthenticated: return "UNAUTHENTICATED"; - case SafetyReason::PermissionDenied: return "PERMISSION_DENIED"; - case SafetyReason::RecoveryRpcDisabled: return "RECOVERY_RPC_DISABLED"; - case SafetyReason::DeviceNotFound: return "DEVICE_NOT_FOUND"; - case SafetyReason::DeviceUnavailable: return "DEVICE_UNAVAILABLE"; - case SafetyReason::UnsupportedCommand: return "UNSUPPORTED_COMMAND"; - case SafetyReason::SystemStarting: return "SYSTEM_STARTING"; - case SafetyReason::SystemStopping: return "SYSTEM_STOPPING"; - case SafetyReason::SafetyLatched: return "SAFETY_LATCHED"; - case SafetyReason::SafetyStateMissing: return "SAFETY_STATE_MISSING"; - case SafetyReason::SafetyStateStale: return "SAFETY_STATE_STALE"; - case SafetyReason::HardwareUnsafe: return "HARDWARE_UNSAFE"; - case SafetyReason::EmergencyStopActive: return "EMERGENCY_STOP_ACTIVE"; - case SafetyReason::ProtectiveStopActive: return "PROTECTIVE_STOP_ACTIVE"; - case SafetyReason::DeviceDisconnected: return "DEVICE_DISCONNECTED"; - case SafetyReason::DeviceFault: return "DEVICE_FAULT"; - case SafetyReason::DeviceNotReady: return "DEVICE_NOT_READY"; - case SafetyReason::DeviceStillMoving: return "DEVICE_STILL_MOVING"; - case SafetyReason::ControlBusy: return "CONTROL_BUSY"; - case SafetyReason::GenerationMismatch: return "GENERATION_MISMATCH"; - case SafetyReason::CommandIdRequired: return "COMMAND_ID_REQUIRED"; - case SafetyReason::CommandIdConflict: return "COMMAND_ID_CONFLICT"; - case SafetyReason::ResultEvicted: return "RESULT_EVICTED"; - case SafetyReason::LedgerExhausted: return "LEDGER_EXHAUSTED"; - case SafetyReason::Backpressure: return "BACKPRESSURE"; - case SafetyReason::DeadlineExceededBeforeDispatch: - return "DEADLINE_EXCEEDED_BEFORE_DISPATCH"; - case SafetyReason::OutcomeUnknown: return "OUTCOME_UNKNOWN"; - case SafetyReason::ParticipantTimeout: return "PARTICIPANT_TIMEOUT"; - case SafetyReason::StopUnconfirmed: return "STOP_UNCONFIRMED"; - case SafetyReason::RecoveryEpochMismatch: return "RECOVERY_EPOCH_MISMATCH"; - case SafetyReason::RecoveryReasonRequired: return "RECOVERY_REASON_REQUIRED"; - case SafetyReason::RecoveryAuditFailed: return "RECOVERY_AUDIT_FAILED"; - case SafetyReason::InternalError: return "INTERNAL_ERROR"; - } - return "INTERNAL_ERROR"; -} - -bool retryWithSameCommandId(const SafetyReason reason) noexcept -{ - switch (reason) { - case SafetyReason::RecoveryRpcDisabled: - case SafetyReason::SystemStopping: - return true; - case SafetyReason::None: - case SafetyReason::InvalidArgument: - case SafetyReason::Unauthenticated: - case SafetyReason::PermissionDenied: - case SafetyReason::DeviceNotFound: - case SafetyReason::DeviceUnavailable: - case SafetyReason::UnsupportedCommand: - case SafetyReason::SystemStarting: - case SafetyReason::SafetyLatched: - case SafetyReason::SafetyStateMissing: - case SafetyReason::SafetyStateStale: - case SafetyReason::HardwareUnsafe: - case SafetyReason::EmergencyStopActive: - case SafetyReason::ProtectiveStopActive: - case SafetyReason::DeviceDisconnected: - case SafetyReason::DeviceFault: - case SafetyReason::DeviceNotReady: - case SafetyReason::DeviceStillMoving: - case SafetyReason::ControlBusy: - case SafetyReason::GenerationMismatch: - case SafetyReason::CommandIdRequired: - case SafetyReason::CommandIdConflict: - case SafetyReason::ResultEvicted: - case SafetyReason::LedgerExhausted: - case SafetyReason::Backpressure: - case SafetyReason::DeadlineExceededBeforeDispatch: - case SafetyReason::OutcomeUnknown: - case SafetyReason::ParticipantTimeout: - case SafetyReason::StopUnconfirmed: - case SafetyReason::RecoveryEpochMismatch: - case SafetyReason::RecoveryReasonRequired: - case SafetyReason::RecoveryAuditFailed: - case SafetyReason::InternalError: - return false; - } - return false; -} - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/src/safety_snapshot_store.cpp b/cmvr-es/manager/safety_manager/src/safety_snapshot_store.cpp deleted file mode 100644 index 5a80a38b..00000000 --- a/cmvr-es/manager/safety_manager/src/safety_snapshot_store.cpp +++ /dev/null @@ -1,305 +0,0 @@ -#include "manager/safety_manager/include/safety_snapshot_store.h" - -#include -#include -#include -#include - -namespace cmvr::safety { - -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; -} - -} // namespace - -bool SafetySnapshotStore::registerDevice( - const DeviceSafetyDescriptor& descriptor, - const std::uint64_t initial_generation) -{ - if (descriptor.device_id.empty() || - descriptor.maximum_snapshot_age <= std::chrono::milliseconds::zero() || - initial_generation == 0) { - return false; - } - - Slot slot; - slot.descriptor = descriptor; - slot.snapshot.device_id = descriptor.device_id; - slot.snapshot.device_generation = initial_generation; - slot.snapshot.condition = SafetyCondition::Unknown; - - std::unique_lock lock(mutex_); - const auto inserted = slots_.emplace(descriptor.device_id, std::move(slot)); - if (inserted.second) { - changed_.notify_all(); - } - return inserted.second; -} - -bool SafetySnapshotStore::unregisterDevice(const std::string& device_id) -{ - std::unique_lock lock(mutex_); - const bool removed = slots_.erase(device_id) != 0; - if (removed) { - changed_.notify_all(); - } - return removed; -} - -bool SafetySnapshotStore::publish(DeviceSafetySnapshot snapshot) -{ - if (snapshot.device_id.empty() || snapshot.device_generation == 0 || - snapshot.sample_sequence == 0 || - snapshot.observed_at == SafetyClock::time_point{}) { - return false; - } - - std::unique_lock lock(mutex_); - const auto found = slots_.find(snapshot.device_id); - if (found == slots_.end()) { - return false; - } - - auto& slot = found->second; - const auto current_generation = slot.snapshot.device_generation; - if (snapshot.device_generation < current_generation) { - return false; - } - if (snapshot.device_generation == current_generation && - slot.has_sample && - snapshot.sample_sequence <= slot.snapshot.sample_sequence) { - return false; - } - if (snapshot.observed_at_unix_ms == 0) { - snapshot.observed_at_unix_ms = unixTimeMs(); - } - slot.snapshot = std::move(snapshot); - slot.has_sample = true; - changed_.notify_all(); - return true; -} - -bool SafetySnapshotStore::markUnknown( - const std::string& device_id, - const SafetyReason reason, - std::string source_id) -{ - std::unique_lock lock(mutex_); - const auto found = slots_.find(device_id); - if (found == slots_.end()) { - return false; - } - - auto& slot = found->second; - DeviceSafetySnapshot snapshot; - snapshot.device_id = device_id; - snapshot.device_generation = slot.snapshot.device_generation; - snapshot.sample_sequence = slot.snapshot.sample_sequence + 1U; - if (snapshot.sample_sequence == 0) { - snapshot.sample_sequence = 1U; - } - snapshot.observed_at = SafetyClock::now(); - snapshot.observed_at_unix_ms = unixTimeMs(); - snapshot.condition = SafetyCondition::Unknown; - snapshot.blockers.push_back(SafetyBlocker{ - reason, - BlockerScope::Device, - RecoveryRequirement::RefreshOnly, - source_id.empty() ? device_id : std::move(source_id), - {}, - snapshot.observed_at_unix_ms, - snapshot.observed_at_unix_ms}); - slot.snapshot = std::move(snapshot); - slot.has_sample = true; - changed_.notify_all(); - return true; -} - -std::optional SafetySnapshotStore::bumpGeneration( - const std::string& device_id) -{ - std::unique_lock lock(mutex_); - const auto found = slots_.find(device_id); - if (found == slots_.end()) { - return std::nullopt; - } - auto& slot = found->second; - if (slot.snapshot.device_generation == - std::numeric_limits::max()) { - return std::nullopt; - } - ++slot.snapshot.device_generation; - slot.snapshot.sample_sequence = 0; - slot.snapshot.observed_at = {}; - slot.snapshot.observed_at_unix_ms = 0; - slot.snapshot.condition = SafetyCondition::Unknown; - slot.snapshot.blockers.clear(); - slot.has_sample = false; - changed_.notify_all(); - return slot.snapshot.device_generation; -} - -SafetySnapshotView SafetySnapshotStore::viewOf_( - const Slot& slot, - const SafetyClock::time_point now) -{ - SafetySnapshotView view; - view.descriptor = slot.descriptor; - view.snapshot = slot.snapshot; - view.registered = true; - view.has_sample = slot.has_sample; - if (!slot.has_sample || - slot.snapshot.observed_at == SafetyClock::time_point{}) { - return view; - } - - const auto elapsed = now <= slot.snapshot.observed_at - ? SafetyClock::duration::zero() - : now - slot.snapshot.observed_at; - view.sample_age = std::chrono::duration_cast( - elapsed); - view.fresh = view.sample_age <= slot.descriptor.maximum_snapshot_age; - return view; -} - -SafetySnapshotView SafetySnapshotStore::get( - const std::string& device_id, - const SafetyClock::time_point now) const -{ - std::shared_lock lock(mutex_); - const auto found = slots_.find(device_id); - if (found == slots_.end()) { - return {}; - } - return viewOf_(found->second, now); -} - -std::vector SafetySnapshotStore::snapshot( - const SafetyClock::time_point now) const -{ - std::vector result; - std::shared_lock lock(mutex_); - result.reserve(slots_.size()); - for (const auto& [id, slot] : slots_) { - (void)id; - result.push_back(viewOf_(slot, now)); - } - std::sort(result.begin(), result.end(), [](const auto& lhs, const auto& rhs) { - return lhs.descriptor.device_id < rhs.descriptor.device_id; - }); - return result; -} - -bool SafetySnapshotStore::waitForNewerSample( - const std::string& device_id, - const std::uint64_t previous_sequence, - const SafetyClock::time_point deadline, - SafetySnapshotView& result) const -{ - std::unique_lock lock(mutex_); - const auto ready = [&]() { - const auto found = slots_.find(device_id); - return found == slots_.end() || - (found->second.has_sample && - found->second.snapshot.sample_sequence > previous_sequence); - }; - if (!changed_.wait_until(lock, deadline, ready)) { - return false; - } - const auto found = slots_.find(device_id); - if (found == slots_.end()) { - return false; - } - result = viewOf_(found->second, SafetyClock::now()); - return result.has_sample && - result.snapshot.sample_sequence > previous_sequence; -} - -const char* toString(const TriState value) noexcept -{ - switch (value) { - case TriState::Unknown: return "Unknown"; - case TriState::False: return "False"; - case TriState::True: return "True"; - } - return "Unknown"; -} - -const char* toString(const SafetyCondition value) noexcept -{ - switch (value) { - case SafetyCondition::Nominal: return "Nominal"; - case SafetyCondition::Restricted: return "Restricted"; - case SafetyCondition::Unsafe: return "Unsafe"; - case SafetyCondition::Unknown: return "Unknown"; - } - return "Unknown"; -} - -const char* toString(const CommandIntent value) noexcept -{ - switch (value) { - case CommandIntent::Observe: return "Observe"; - case CommandIntent::StartActivity: return "StartActivity"; - case CommandIntent::Configure: return "Configure"; - case CommandIntent::Actuate: return "Actuate"; - case CommandIntent::Stop: return "Stop"; - case CommandIntent::ResetFault: return "ResetFault"; - case CommandIntent::RecoverAdmission: return "RecoverAdmission"; - } - return "Observe"; -} - -const char* toString(const SafetyPolicyFamily value) noexcept -{ - switch (value) { - case SafetyPolicyFamily::Sensor: return "Sensor"; - case SafetyPolicyFamily::Control: return "Control"; - } - return "Sensor"; -} - -const char* toString(const SystemAdmissionState value) noexcept -{ - switch (value) { - case SystemAdmissionState::Starting: return "Starting"; - case SystemAdmissionState::Open: return "Open"; - case SystemAdmissionState::Stopping: return "Stopping"; - case SystemAdmissionState::Latched: return "Latched"; - case SystemAdmissionState::Recovering: return "Recovering"; - case SystemAdmissionState::ShuttingDown: return "ShuttingDown"; - } - return "Starting"; -} - -const char* toString(const DeviceAdmissionState value) noexcept -{ - switch (value) { - case DeviceAdmissionState::Observing: return "Observing"; - case DeviceAdmissionState::Open: return "Open"; - case DeviceAdmissionState::Blocked: return "Blocked"; - case DeviceAdmissionState::Quarantined: return "Quarantined"; - case DeviceAdmissionState::Recovering: return "Recovering"; - case DeviceAdmissionState::Removed: return "Removed"; - } - return "Observing"; -} - -const char* toString(const EnforcementMode value) noexcept -{ - switch (value) { - case EnforcementMode::Legacy: return "Legacy"; - case EnforcementMode::Shadow: return "Shadow"; - case EnforcementMode::EnforceSelected: return "EnforceSelected"; - case EnforcementMode::EnforceAll: return "EnforceAll"; - } - return "Legacy"; -} - -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/tests/command_ledger_test.cpp b/cmvr-es/manager/safety_manager/tests/command_ledger_test.cpp deleted file mode 100644 index bad8e503..00000000 --- a/cmvr-es/manager/safety_manager/tests/command_ledger_test.cpp +++ /dev/null @@ -1,122 +0,0 @@ -#include "manager/safety_manager/include/command_ledger.h" - -#include -#include -#include -#include - -#include - -namespace cmvr::safety { -namespace { - -CommandKey key(const std::string& id) -{ - return {"anonymous", id}; -} - -CommandOutcome completedOutcome() -{ - CommandOutcome outcome; - outcome.lifecycle = CommandLifecycle::Completed; - outcome.reason = SafetyReason::None; - outcome.serialized_response = "done"; - return outcome; -} - -TEST(CommandLedgerTest, SameIdJoinsAndDifferentPayloadConflicts) -{ - CommandLedger ledger; - const auto first = ledger.reserve(key("command-1"), "payload-a"); - ASSERT_EQ(first.status, CommandReservationStatus::AcceptedNew); - EXPECT_EQ( - ledger.reserve(key("command-1"), "payload-a").status, - CommandReservationStatus::JoinedInFlight); - EXPECT_EQ( - ledger.reserve(key("command-1"), "payload-b").status, - CommandReservationStatus::CommandIdConflict); - - ASSERT_TRUE(ledger.complete(first.ticket, completedOutcome())); - const auto cached = ledger.reserve(key("command-1"), "payload-a"); - ASSERT_EQ(cached.status, CommandReservationStatus::CachedResult); - ASSERT_TRUE(cached.cached_outcome.has_value()); - EXPECT_EQ(cached.cached_outcome->serialized_response, "done"); -} - -TEST(CommandLedgerTest, ConcurrentCallersNeverCreateASecondReservation) -{ - CommandLedger ledger; - std::atomic accepted{0}; - std::vector workers; - for (int index = 0; index < 16; ++index) { - workers.emplace_back([&] { - const auto result = ledger.reserve(key("shared"), "payload"); - if (result.status == CommandReservationStatus::AcceptedNew) { - accepted.fetch_add(1); - } - }); - } - for (auto& worker : workers) { - worker.join(); - } - EXPECT_EQ(accepted.load(), 1); - EXPECT_EQ(ledger.acceptedIdCount(), 1U); -} - -TEST(CommandLedgerTest, OutcomeUnknownIsCachedAndNeverReservedAgain) -{ - CommandLedger ledger; - const auto first = ledger.reserve(key("uncertain"), "payload"); - ASSERT_EQ(first.status, CommandReservationStatus::AcceptedNew); - - CommandOutcome outcome; - outcome.lifecycle = CommandLifecycle::OutcomeUnknown; - outcome.reason = SafetyReason::OutcomeUnknown; - outcome.hardware_submission_possible = true; - ASSERT_TRUE(ledger.complete(first.ticket, outcome)); - - const auto retry = ledger.reserve(key("uncertain"), "payload"); - ASSERT_EQ(retry.status, CommandReservationStatus::CachedResult); - ASSERT_TRUE(retry.cached_outcome.has_value()); - EXPECT_EQ( - retry.cached_outcome->lifecycle, CommandLifecycle::OutcomeUnknown); - EXPECT_TRUE(retry.cached_outcome->hardware_submission_possible); -} - -TEST(CommandLedgerTest, EvictedResultLeavesAnExactTombstone) -{ - CommandLedger ledger({1, 3}); - auto first = ledger.reserve(key("first"), "payload-1"); - ASSERT_TRUE(ledger.complete(first.ticket, completedOutcome())); - auto second = ledger.reserve(key("second"), "payload-2"); - ASSERT_TRUE(ledger.complete(second.ticket, completedOutcome())); - - EXPECT_EQ( - ledger.reserve(key("first"), "payload-1").status, - CommandReservationStatus::ResultEvicted); - EXPECT_EQ( - ledger.reserve(key("first"), "different").status, - CommandReservationStatus::CommandIdConflict); - - auto third = ledger.reserve(key("third"), "payload-3"); - ASSERT_EQ(third.status, CommandReservationStatus::AcceptedNew); - EXPECT_EQ( - ledger.reserve(key("fourth"), "payload-4").status, - CommandReservationStatus::LedgerExhausted); -} - -TEST(CommandLedgerTest, WaitTimesOutWithoutChangingExecution) -{ - CommandLedger ledger; - const auto reservation = ledger.reserve(key("slow"), "payload"); - ASSERT_EQ(reservation.status, CommandReservationStatus::AcceptedNew); - EXPECT_FALSE(ledger.wait( - reservation.ticket, - SafetyClock::now() + std::chrono::milliseconds(5)).has_value()); - EXPECT_EQ( - ledger.reserve(key("slow"), "payload").status, - CommandReservationStatus::JoinedInFlight); -} - -} // namespace -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp b/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp deleted file mode 100644 index 1667e251..00000000 --- a/cmvr-es/manager/safety_manager/tests/safety_manager_test.cpp +++ /dev/null @@ -1,507 +0,0 @@ -#include "manager/safety_manager/include/safety_manager.h" - -#include -#include -#include -#include - -#include - -namespace cmvr::safety { -namespace { - -DeviceSafetyDescriptor controlDescriptor() -{ - DeviceSafetyDescriptor descriptor; - descriptor.device_id = "arm"; - descriptor.kind = device::DeviceKind::Arm; - descriptor.default_policy = SafetyPolicyFamily::Control; - descriptor.maximum_snapshot_age = std::chrono::seconds(1); - descriptor.requires_safe_stop = true; - descriptor.supports_active_refresh = true; - return descriptor; -} - -class FakeEndpoint final : public DeviceSafetyEndpoint { -public: - explicit FakeEndpoint(DeviceSafetyDescriptor descriptor) - : descriptor_(std::move(descriptor)) - { - } - - DeviceSafetyDescriptor descriptor() const override - { - return descriptor_; - } - - void bindPublisher(SafetySnapshotPublisher publisher) override - { - publisher_ = std::move(publisher); - } - - void requestSafetyRefresh() noexcept override - { - if (!publisher_ || !publish_on_refresh) { - return; - } - DeviceSafetySnapshot snapshot; - snapshot.device_id = descriptor_.device_id; - snapshot.condition = condition; - snapshot.device_generation = generation; - snapshot.sample_sequence = ++sequence; - snapshot.observed_at = SafetyClock::now(); - snapshot.connected = connected; - snapshot.operational_ready = ready; - snapshot.quiescent = quiescent; - snapshot.motion_active = - quiescent == TriState::True ? TriState::False : TriState::Unknown; - snapshot.emergency_stop_active = emergency_stop; - snapshot.protective_stop_active = protective_stop; - snapshot.fault_active = fault; - (void)publisher_(std::move(snapshot)); - } - - HardwareCheckResult validateBeforeDispatch( - const AdmissionPermit&) override - { - ++hardware_checks; - return final_check; - } - - RecoveryCheckResult reconcileAdmissionState( - const RecoveryContext&) override - { - ++recoveries; - return recovery_check; - } - - DeviceSafetyDescriptor descriptor_; - SafetySnapshotPublisher publisher_; - SafetyCondition condition{SafetyCondition::Nominal}; - TriState connected{TriState::True}; - TriState ready{TriState::True}; - TriState quiescent{TriState::True}; - TriState emergency_stop{TriState::False}; - TriState protective_stop{TriState::False}; - TriState fault{TriState::False}; - HardwareCheckResult final_check{true, SafetyReason::None, {}}; - RecoveryCheckResult recovery_check{true, SafetyReason::None, {}}; - std::uint64_t generation{1}; - std::uint64_t sequence{0}; - bool publish_on_refresh{true}; - std::atomic hardware_checks{0}; - std::atomic recoveries{0}; -}; - -class FakeParticipant final : public SafetyParticipant { -public: - ParticipantDescriptor descriptor() const override - { - return {"arm", ParticipantPhase::Actuator, true, - std::chrono::milliseconds(100)}; - } - - BarrierToken beginBarrier( - const SafetyOperationContext& context) override - { - ++barriers; - return {"arm", context.operation_id, context.safety_epoch, - static_cast(barriers.load())}; - } - - ParticipantResult requestQuiesce( - const BarrierToken&, - const SafetyOperationContext&) override - { - ++stop_requests; - return stop_result; - } - - ParticipantResult verifyQuiescent( - const BarrierToken&, - const SafetyOperationContext&) override - { - ++verifications; - return verify_result; - } - - RecoveryCheckResult recoverAdmission( - const BarrierToken&, - const RecoveryContext&) override - { - ++recoveries; - return recovery_result; - } - - ParticipantResult releaseBarrier( - const BarrierToken&) noexcept override - { - ++releases; - return release_result; - } - - ParticipantResult stop_result{true, SafetyReason::None, {}}; - ParticipantResult verify_result{true, SafetyReason::None, {}}; - RecoveryCheckResult recovery_result{true, SafetyReason::None, {}}; - ParticipantResult release_result{true, SafetyReason::None, {}}; - std::atomic barriers{0}; - std::atomic stop_requests{0}; - std::atomic verifications{0}; - std::atomic recoveries{0}; - std::atomic releases{0}; -}; - -AdmissionRequest actuateRequest() -{ - AdmissionRequest request; - request.command = { - "/cmvr.api.ArmService/moveJ", - CommandIntent::Actuate, - SafetyPolicyFamily::Control, - true, - false}; - request.device_id = "arm"; - request.command_id = "command-1"; - request.deadline = SafetyClock::now() + std::chrono::seconds(1); - return request; -} - -TEST(SafetyManagerTest, ShadowReportsDenyWithoutChangingLegacyBehavior) -{ - SafetyManager coordinator; - ASSERT_TRUE(coordinator.registerDevice({controlDescriptor(), {}, {}})); - coordinator.markStartupComplete(); - - const auto result = coordinator.admit(actuateRequest()); - EXPECT_TRUE(result.decision.allowed); - EXPECT_FALSE(result.decision.policy_allowed); - EXPECT_FALSE(result.decision.enforced); - EXPECT_EQ(result.decision.reason, SafetyReason::SafetyStateMissing); - EXPECT_TRUE(result.permit.has_value()); -} - -TEST(SafetyManagerTest, - EnforceSelectedStartupRejectsEmptyOrUnknownCoverage) -{ - SafetyManagerConfig empty_config; - empty_config.enforcement_mode = EnforcementMode::EnforceSelected; - SafetyManager empty(empty_config); - const auto empty_result = empty.validateStartupCoverage( - SafetyClock::now() + std::chrono::milliseconds(10)); - ASSERT_FALSE(empty_result.ready); - ASSERT_EQ(empty_result.issues.size(), 1U); - EXPECT_EQ(empty_result.issues.front().reason, SafetyReason::InvalidArgument); - - SafetyManagerConfig missing_config; - missing_config.enforcement_mode = EnforcementMode::EnforceSelected; - missing_config.enforced_device_ids.insert("missing-arm"); - SafetyManager missing(missing_config); - const auto missing_result = missing.validateStartupCoverage( - SafetyClock::now() + std::chrono::milliseconds(10)); - ASSERT_FALSE(missing_result.ready); - ASSERT_EQ(missing_result.issues.size(), 1U); - EXPECT_EQ(missing_result.issues.front().target_id, "missing-arm"); - EXPECT_EQ(missing_result.issues.front().reason, SafetyReason::DeviceNotFound); -} - -TEST(SafetyManagerTest, - EnforceAllStartupRequiresEndpointParticipantAndFreshSnapshot) -{ - SafetyManagerConfig config; - config.enforcement_mode = EnforcementMode::EnforceAll; - - SafetyManager missing_capability(config); - ASSERT_TRUE(missing_capability.registerDevice( - {controlDescriptor(), {}, {}})); - const auto structural = missing_capability.validateStartupCoverage( - SafetyClock::now() + std::chrono::milliseconds(10)); - EXPECT_FALSE(structural.ready); - EXPECT_EQ(structural.issues.size(), 2U); - - SafetyManager missing_sample(config); - auto silent_endpoint = - std::make_shared(controlDescriptor()); - silent_endpoint->publish_on_refresh = false; - ASSERT_TRUE(missing_sample.registerDevice({ - controlDescriptor(), - silent_endpoint, - std::make_shared()})); - const auto stale = missing_sample.validateStartupCoverage( - SafetyClock::now() + std::chrono::milliseconds(10)); - ASSERT_FALSE(stale.ready); - ASSERT_EQ(stale.issues.size(), 1U); - EXPECT_EQ(stale.issues.front().reason, SafetyReason::SafetyStateMissing); -} - -TEST(SafetyManagerTest, - HardwareUnsafeSnapshotBlocksAdmissionButNotStructuralStartup) -{ - SafetyManagerConfig config; - config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyManager coordinator(config); - auto endpoint = std::make_shared(controlDescriptor()); - endpoint->condition = SafetyCondition::Unsafe; - endpoint->emergency_stop = TriState::True; - ASSERT_TRUE(coordinator.registerDevice({ - controlDescriptor(), endpoint, std::make_shared()})); - coordinator.updateDeviceRuntimeState( - "arm", device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - - const auto coverage = coordinator.validateStartupCoverage( - SafetyClock::now() + std::chrono::milliseconds(50)); - EXPECT_TRUE(coverage.ready); - coordinator.markStartupComplete(); - const auto admission = coordinator.admit(actuateRequest()); - EXPECT_FALSE(admission.decision.allowed); - EXPECT_EQ( - admission.decision.reason, SafetyReason::EmergencyStopActive); -} - -TEST(SafetyManagerTest, EnforceAllFailsClosedOnUnknownControlState) -{ - SafetyManagerConfig config; - config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyManager coordinator(config); - ASSERT_TRUE(coordinator.registerDevice({controlDescriptor(), {}, {}})); - coordinator.markStartupComplete(); - - const auto result = coordinator.admit(actuateRequest()); - EXPECT_FALSE(result.decision.allowed); - EXPECT_FALSE(result.decision.policy_allowed); - EXPECT_TRUE(result.decision.enforced); - EXPECT_FALSE(result.permit.has_value()); -} - -TEST(SafetyManagerTest, ControlSafetyBitsMustBeExplicitlyFalse) -{ - SafetyManagerConfig config; - config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyManager coordinator(config); - auto endpoint = std::make_shared(controlDescriptor()); - endpoint->protective_stop = TriState::Unknown; - ASSERT_TRUE(coordinator.registerDevice( - {controlDescriptor(), endpoint, {}})); - coordinator.updateDeviceRuntimeState( - "arm", device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - const auto result = coordinator.admit(actuateRequest()); - EXPECT_FALSE(result.decision.allowed); - EXPECT_EQ(result.decision.reason, SafetyReason::ProtectiveStopActive); - ASSERT_EQ(coordinator.snapshot().devices.size(), 1U); - EXPECT_EQ( - coordinator.snapshot().devices.front().admission_state, - DeviceAdmissionState::Blocked); -} - -TEST(SafetyManagerTest, EnforcedDispatchRunsFinalHardwareCheck) -{ - SafetyManagerConfig config; - config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyManager coordinator(config); - auto endpoint = std::make_shared(controlDescriptor()); - ASSERT_TRUE(coordinator.registerDevice( - {controlDescriptor(), endpoint, {}})); - coordinator.updateDeviceRuntimeState( - "arm", device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - auto admission = coordinator.admit(actuateRequest()); - ASSERT_TRUE(admission.decision.allowed) << admission.decision.detail; - ASSERT_TRUE(admission.permit.has_value()); - auto dispatch = coordinator.beginDispatch(*admission.permit); - EXPECT_TRUE(dispatch.acquired()); - EXPECT_EQ(endpoint->hardware_checks.load(), 1); -} - -TEST(SafetyManagerTest, - StartActivityMayEnterFromRestrictedButActuationMayNot) -{ - SafetyManagerConfig config; - config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyManager coordinator(config); - auto endpoint = std::make_shared(controlDescriptor()); - endpoint->condition = SafetyCondition::Restricted; - endpoint->ready = TriState::False; - ASSERT_TRUE(coordinator.registerDevice( - {controlDescriptor(), endpoint, {}})); - coordinator.updateDeviceRuntimeState( - "arm", device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - auto start = actuateRequest(); - start.command.intent = CommandIntent::StartActivity; - const auto start_result = coordinator.admit(start); - ASSERT_TRUE(start_result.decision.allowed) - << start_result.decision.detail; - ASSERT_TRUE(start_result.permit.has_value()); - EXPECT_TRUE(coordinator.beginDispatch(*start_result.permit).acquired()); - - const auto actuation = coordinator.admit(actuateRequest()); - EXPECT_FALSE(actuation.decision.allowed); - EXPECT_EQ(actuation.decision.reason, SafetyReason::HardwareUnsafe); -} - -TEST(SafetyManagerTest, SuccessfulStopInvalidatesOldPermitAndReopens) -{ - SafetyManagerConfig config; - config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyManager coordinator(config); - auto endpoint = std::make_shared(controlDescriptor()); - auto participant = std::make_shared(); - ASSERT_TRUE(coordinator.registerDevice( - {controlDescriptor(), endpoint, participant})); - coordinator.updateDeviceRuntimeState( - "arm", device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - auto admission = coordinator.admit(actuateRequest()); - ASSERT_TRUE(admission.permit.has_value()); - const auto old_epoch = admission.permit->safety_epoch; - const auto stopped = coordinator.stopAll( - "stop-1", SafetyClock::now() + std::chrono::seconds(1)); - ASSERT_TRUE(stopped.success); - EXPECT_EQ(stopped.system_state, SystemAdmissionState::Open); - EXPECT_GT(stopped.current_safety_epoch, old_epoch); - const auto revalidated = - coordinator.revalidatePermit(*admission.permit); - EXPECT_FALSE(revalidated.safe); - EXPECT_EQ(revalidated.reason, SafetyReason::SafetyLatched); - EXPECT_FALSE(coordinator.beginDispatch(*admission.permit).acquired()); - EXPECT_EQ(participant->stop_requests.load(), 1); - EXPECT_EQ(participant->verifications.load(), 1); - const auto snapshot = coordinator.snapshot(); - ASSERT_EQ(snapshot.participants.size(), 1U); - EXPECT_EQ(snapshot.participants.front().descriptor.participant_id, "arm"); - EXPECT_FALSE(snapshot.participants.front().barrier_active); - EXPECT_FALSE(snapshot.participants.front().barrier_retained); - EXPECT_TRUE(snapshot.participants.front().last_request.recorded); - EXPECT_TRUE(snapshot.participants.front().last_request.success); - EXPECT_TRUE(snapshot.participants.front().last_verify.recorded); - EXPECT_TRUE(snapshot.participants.front().last_verify.success); - EXPECT_TRUE(snapshot.participants.front().last_release.recorded); - EXPECT_TRUE(snapshot.participants.front().last_release.success); -} - -TEST(SafetyManagerTest, FailedStopRequiresVerifiedRecovery) -{ - SafetyManagerConfig config; - config.enforcement_mode = EnforcementMode::EnforceAll; - SafetyManager coordinator(config); - auto endpoint = std::make_shared(controlDescriptor()); - auto participant = std::make_shared(); - participant->verify_result = { - false, SafetyReason::StopUnconfirmed, "motion not confirmed"}; - ASSERT_TRUE(coordinator.registerDevice( - {controlDescriptor(), endpoint, participant})); - coordinator.updateDeviceRuntimeState( - "arm", device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - const auto stopped = coordinator.stopAll( - "stop-failed", SafetyClock::now() + std::chrono::seconds(1)); - ASSERT_FALSE(stopped.success); - ASSERT_EQ(stopped.system_state, SystemAdmissionState::Latched); - const auto latched = coordinator.snapshot(); - ASSERT_EQ(latched.participants.size(), 1U); - EXPECT_TRUE(latched.participants.front().barrier_active); - EXPECT_TRUE(latched.participants.front().barrier_retained); - EXPECT_TRUE(latched.participants.front().last_verify.recorded); - EXPECT_FALSE(latched.participants.front().last_verify.success); - EXPECT_FALSE(latched.participants.front().last_release.recorded); - - participant->verify_result = {true, SafetyReason::None, {}}; - RecoveryRequest request; - request.recovery_id = "recovery-1"; - request.all_devices = true; - request.expected_safety_epoch = stopped.current_safety_epoch; - request.verify_only = false; - request.reason = "operator confirmed work cell is clear"; - request.deadline = SafetyClock::now() + std::chrono::seconds(1); - const auto recovered = coordinator.recover(request); - EXPECT_EQ(recovered.result, RecoveryResultCode::Recovered); - EXPECT_EQ(recovered.system_state, SystemAdmissionState::Open); - EXPECT_EQ(endpoint->recoveries.load(), 1); - EXPECT_EQ(participant->recoveries.load(), 1); - const auto reopened = coordinator.snapshot(); - ASSERT_EQ(reopened.participants.size(), 1U); - EXPECT_FALSE(reopened.participants.front().barrier_active); - EXPECT_FALSE(reopened.participants.front().barrier_retained); - EXPECT_TRUE(reopened.participants.front().last_release.recorded); - EXPECT_TRUE(reopened.participants.front().last_release.success); -} - -TEST(SafetyManagerTest, RecoveryCannotIgnoreEmergencyStop) -{ - SafetyManager coordinator; - auto endpoint = std::make_shared(controlDescriptor()); - auto participant = std::make_shared(); - ASSERT_TRUE(coordinator.registerDevice( - {controlDescriptor(), endpoint, participant})); - coordinator.markStartupComplete(); - coordinator.quarantineDevice("arm", SafetyReason::OutcomeUnknown); - const auto before = coordinator.snapshot(); - - endpoint->emergency_stop = TriState::True; - RecoveryRequest request; - request.recovery_id = "recovery-estop"; - request.all_devices = true; - request.expected_safety_epoch = before.safety_epoch; - request.verify_only = false; - request.reason = "test"; - request.deadline = SafetyClock::now() + std::chrono::seconds(1); - const auto recovered = coordinator.recover(request); - EXPECT_EQ(recovered.result, RecoveryResultCode::BlockerRemains); - EXPECT_EQ(recovered.system_state, SystemAdmissionState::Latched); - ASSERT_FALSE(recovered.targets.empty()); - EXPECT_EQ( - recovered.targets.front().reason, - SafetyReason::EmergencyStopActive); - EXPECT_EQ(endpoint->recoveries.load(), 0); -} - -TEST(SafetyManagerTest, RecoveryAuditFailureCannotReleaseLatch) -{ - SafetyManager coordinator; - auto endpoint = std::make_shared(controlDescriptor()); - auto participant = std::make_shared(); - participant->verify_result = { - false, SafetyReason::StopUnconfirmed, "motion not confirmed"}; - ASSERT_TRUE(coordinator.registerDevice( - {controlDescriptor(), endpoint, participant})); - coordinator.markStartupComplete(); - - const auto stopped = coordinator.stopAll( - "stop-audit", SafetyClock::now() + std::chrono::seconds(1)); - ASSERT_FALSE(stopped.success); - participant->verify_result = {true, SafetyReason::None, {}}; - - RecoveryRequest request; - request.recovery_id = "recovery-audit"; - request.all_devices = true; - request.expected_safety_epoch = stopped.current_safety_epoch; - request.verify_only = false; - request.reason = "operator inspected the work cell"; - request.deadline = SafetyClock::now() + std::chrono::seconds(1); - request.authorize_clear = [] { return false; }; - const auto recovered = coordinator.recover(request); - - EXPECT_EQ(recovered.result, RecoveryResultCode::BlockerRemains); - EXPECT_EQ(recovered.system_state, SystemAdmissionState::Latched); - ASSERT_FALSE(recovered.targets.empty()); - EXPECT_EQ( - recovered.targets.front().reason, - SafetyReason::RecoveryAuditFailed); - EXPECT_EQ(endpoint->recoveries.load(), 0); - EXPECT_EQ(participant->recoveries.load(), 0); - EXPECT_EQ(participant->releases.load(), 0); -} - -} // namespace -} // namespace cmvr::safety diff --git a/cmvr-es/manager/safety_manager/tests/safety_snapshot_store_test.cpp b/cmvr-es/manager/safety_manager/tests/safety_snapshot_store_test.cpp deleted file mode 100644 index 7d2c93d6..00000000 --- a/cmvr-es/manager/safety_manager/tests/safety_snapshot_store_test.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include "manager/safety_manager/include/safety_snapshot_store.h" - -#include -#include -#include - -#include - -namespace cmvr::safety { -namespace { - -DeviceSafetyDescriptor descriptor() -{ - DeviceSafetyDescriptor result; - result.device_id = "arm"; - result.kind = device::DeviceKind::Arm; - result.default_policy = SafetyPolicyFamily::Control; - result.maximum_snapshot_age = std::chrono::milliseconds(50); - result.requires_safe_stop = true; - return result; -} - -DeviceSafetySnapshot sample( - const std::uint64_t generation, - const std::uint64_t sequence, - const SafetyClock::time_point observed_at = SafetyClock::now()) -{ - DeviceSafetySnapshot result; - result.device_id = "arm"; - result.condition = SafetyCondition::Nominal; - result.device_generation = generation; - result.sample_sequence = sequence; - result.observed_at = observed_at; - result.connected = TriState::True; - result.operational_ready = TriState::True; - result.quiescent = TriState::True; - result.motion_active = TriState::False; - result.emergency_stop_active = TriState::False; - result.protective_stop_active = TriState::False; - result.fault_active = TriState::False; - return result; -} - -TEST(SafetySnapshotStoreTest, RejectsOldGenerationAndSequence) -{ - SafetySnapshotStore store; - ASSERT_TRUE(store.registerDevice(descriptor(), 3)); - EXPECT_FALSE(store.publish(sample(2, 1))); - ASSERT_TRUE(store.publish(sample(3, 2))); - EXPECT_FALSE(store.publish(sample(3, 2))); - EXPECT_FALSE(store.publish(sample(3, 1))); - EXPECT_TRUE(store.publish(sample(4, 1))); - - const auto view = store.get("arm"); - ASSERT_TRUE(view.registered); - EXPECT_EQ(view.snapshot.device_generation, 4U); - EXPECT_EQ(view.snapshot.sample_sequence, 1U); -} - -TEST(SafetySnapshotStoreTest, FreshnessUsesMonotonicObservationTime) -{ - SafetySnapshotStore store; - ASSERT_TRUE(store.registerDevice(descriptor())); - const auto observed = SafetyClock::now(); - ASSERT_TRUE(store.publish(sample(1, 1, observed))); - - EXPECT_TRUE(store.get( - "arm", observed + std::chrono::milliseconds(49)).fresh); - const auto stale = store.get( - "arm", observed + std::chrono::milliseconds(51)); - EXPECT_FALSE(stale.fresh); - EXPECT_EQ(stale.sample_age, std::chrono::milliseconds(51)); -} - -TEST(SafetySnapshotStoreTest, BumpGenerationRequiresANewSample) -{ - SafetySnapshotStore store; - ASSERT_TRUE(store.registerDevice(descriptor())); - ASSERT_TRUE(store.publish(sample(1, 8))); - ASSERT_EQ(store.bumpGeneration("arm"), 2U); - - const auto view = store.get("arm"); - EXPECT_FALSE(view.has_sample); - EXPECT_FALSE(view.fresh); - EXPECT_EQ(view.snapshot.device_generation, 2U); - EXPECT_FALSE(store.publish(sample(1, 9))); - EXPECT_TRUE(store.publish(sample(2, 1))); -} - -TEST(SafetySnapshotStoreTest, WaiterObservesOnlyANewerSample) -{ - SafetySnapshotStore store; - ASSERT_TRUE(store.registerDevice(descriptor())); - ASSERT_TRUE(store.publish(sample(1, 1))); - - std::atomic published{false}; - std::thread writer([&] { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - published.store(store.publish(sample(1, 2))); - }); - - SafetySnapshotView view; - EXPECT_TRUE(store.waitForNewerSample( - "arm", 1, SafetyClock::now() + std::chrono::seconds(1), view)); - writer.join(); - EXPECT_TRUE(published.load()); - EXPECT_EQ(view.snapshot.sample_sequence, 2U); -} - -} // namespace -} // namespace cmvr::safety diff --git a/cmvr-es/manager/task_manager/CMakeLists.txt b/cmvr-es/manager/task_manager/CMakeLists.txt index 6f4185a8..40b1eb48 100644 --- a/cmvr-es/manager/task_manager/CMakeLists.txt +++ b/cmvr-es/manager/task_manager/CMakeLists.txt @@ -11,35 +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) install(TARGETS task_manager LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - add_executable(task_manager_lifecycle_test - tests/task_manager_lifecycle_test.cpp - ) - target_link_libraries(task_manager_lifecycle_test PRIVATE - cmvr_es::task_manager - cmvr_es::stop_all_admission_gate - gtest - gtest_main - pthread - ) - add_test( - NAME task_manager_lifecycle_test - COMMAND task_manager_lifecycle_test - ) - set(_task_manager_lifecycle_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _task_manager_lifecycle_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(task_manager_lifecycle_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_task_manager_lifecycle_test_environment}" - ) -endif() diff --git a/cmvr-es/manager/task_manager/include/task_manager.h b/cmvr-es/manager/task_manager/include/task_manager.h index aa80e503..b1d0609f 100644 --- a/cmvr-es/manager/task_manager/include/task_manager.h +++ b/cmvr-es/manager/task_manager/include/task_manager.h @@ -8,7 +8,6 @@ #include #include #include -#include #include "task/task.h" #include "task/touch_screen_task/include/touch_screen_task.h" @@ -24,31 +23,21 @@ 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(); - bool startRunTask(double control_period_s = 0.001); + void 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; - bool initTasks(); + void initTasks(); void runTaskLoop(double control_period_s); static TaskRunMode toTaskRunMode(config::TaskConfigEntry::TaskRunMode run_mode); @@ -60,10 +49,8 @@ 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 12e0a53b..a8af771c 100644 --- a/cmvr-es/manager/task_manager/src/task_manager.cpp +++ b/cmvr-es/manager/task_manager/src/task_manager.cpp @@ -1,6 +1,5 @@ #include "manager/task_manager/include/task_manager.h" -#include #include #include #include @@ -9,7 +8,6 @@ #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; @@ -42,8 +40,6 @@ 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"; @@ -63,21 +59,6 @@ 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; @@ -89,11 +70,7 @@ TaskManager::TaskManager(const config::TaskManagerConfig& cfg) logSection("Task Plan"); logTaskPlan(); logSection("Initialize Tasks"); - initialized_ = initTasks(); - if (!initialized_) { - CMVR_LOG(ERROR) << "[TaskManager] Initialization failed for at least " - "one enabled task"; - } + initTasks(); } TaskManager::~TaskManager() @@ -121,46 +98,11 @@ TaskManager& TaskManager::getInstance() void TaskManager::destroyInstance() { - std::shared_ptr instance; - { - std::lock_guard lock(init_mutex_); - instance = instance_; + std::lock_guard lock(init_mutex_); + if (instance_) { + instance_->stopRunTask(); } - 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>{}; + instance_.reset(); } std::shared_ptr TaskManager::getTouchScreenTask(const std::string& task_id) const @@ -184,84 +126,16 @@ std::shared_ptr TaskManager::getTask(const std::string& task_id) const return it->second; } -bool TaskManager::stopAllActivities(std::vector* failures) +void TaskManager::startRunTask(const double control_period_s) { - 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(); - } - - 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; + return; } - if (running_.load()) { - return admission_current(); + + bool expected = false; + if (!running_.compare_exchange_strong(expected, true)) { + return; } std::vector> tasks; @@ -277,98 +151,39 @@ bool TaskManager::startRunTask(const double control_period_s) std::vector> started_tasks; for (const auto& task : tasks) { - 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 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) { + if (!task->start()) { CMVR_LOG(ERROR) << "[TaskManager] task start failed: " << task->id(); - stopTaskNoThrow(task); - for (auto it = started_tasks.rbegin(); - it != started_tasks.rend(); ++it) { - stopTaskNoThrow(*it); + for (const auto& started_task : started_tasks) { + try { + started_task->stop(); + } catch (...) { + } } running_.store(false); - return false; + return; } 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 { - // 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); - } + 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 (auto it = started_tasks.rbegin(); - it != started_tasks.rend(); ++it) { - stopTaskNoThrow(*it); + for (const auto& task : started_tasks) { + try { + task->stop(); + } catch (...) { + } } - 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; + return; } - 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() { - std::lock_guard lifecycle_lock(lifecycle_mutex_); - if (!running_.exchange(false)) { + bool expected = true; + if (!running_.compare_exchange_strong(expected, false)) { return; } @@ -386,24 +201,22 @@ 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) { - stopTaskNoThrow(task); + try { + task->stop(); + } catch (...) { + } } } -bool TaskManager::initTasks() +void 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; } @@ -413,30 +226,9 @@ bool TaskManager::initTasks() << ", run_mode=" << taskConfigRunModeToString(entry.run_mode()) << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); - 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; - } + auto task = TaskFactory::create(entry); 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()); @@ -444,39 +236,23 @@ bool 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; } - 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) { + if (!task->init()) { 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) { @@ -485,7 +261,6 @@ bool TaskManager::initTasks() tasks_.emplace(entry.id(), std::move(task)); } } - return all_initialized; } void TaskManager::logTaskPlan() const diff --git a/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp b/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp deleted file mode 100644 index bc7a4bf8..00000000 --- a/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp +++ /dev/null @@ -1,428 +0,0 @@ -#include "manager/task_manager/include/task_manager.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" -#include "task/task_factory.h" - -namespace { - -struct TaskBehavior { - bool init_result{true}; - bool start_result{true}; - bool throw_on_start{false}; - bool stop_activity_result{true}; - bool block_start{false}; - bool query_snapshot_on_stop{false}; -}; - -TaskBehavior task_behavior; -std::vector stop_order; - -class LifecycleTask final : public cmvr::task::Task { -public: - explicit LifecycleTask( - std::string id, - const cmvr::task::TaskShutdownPhase shutdown_phase = - cmvr::task::TaskShutdownPhase::DEPENDENT_ACTIVITY) - : id_(std::move(id)), - shutdown_phase_(shutdown_phase) - { - } - - const std::string& id() const override { return id_; } - cmvr::task::TaskRunMode runMode() const override - { - return cmvr::task::TaskRunMode::BLOCKING_SERVICE; - } - cmvr::task::TaskShutdownPhase shutdownPhase() const override - { - return shutdown_phase_; - } - - bool init() override - { - ++init_calls; - state_ = task_behavior.init_result - ? cmvr::task::TaskState::IDLE - : cmvr::task::TaskState::FAILED; - return task_behavior.init_result; - } - - bool start() override - { - ++start_calls; - { - std::unique_lock lock(start_mutex); - start_entered = true; - start_condition.notify_all(); - start_condition.wait(lock, [] { - return !task_behavior.block_start; - }); - } - if (task_behavior.throw_on_start) { - throw std::runtime_error("start failure"); - } - state_ = task_behavior.start_result - ? cmvr::task::TaskState::RUNNING - : cmvr::task::TaskState::FAILED; - return task_behavior.start_result; - } - - bool step(double) override { return true; } - - void stop() override - { - ++stop_calls; - if (task_behavior.query_snapshot_on_stop) { - (void)cmvr::task::TaskManager::activitySnapshotIfInitialized(); - } - stop_order.push_back(id_); - state_ = cmvr::task::TaskState::STOPPED; - } - - bool stopActivity() override - { - ++stop_activity_calls; - return task_behavior.stop_activity_result; - } - - cmvr::task::TaskState state() const override { return state_; } - bool isBusy() const override - { - return state_ == cmvr::task::TaskState::RUNNING; - } - bool isFinished() const override - { - return state_ == cmvr::task::TaskState::STOPPED; - } - bool isFailed() const override - { - return state_ == cmvr::task::TaskState::FAILED; - } - std::string stateString() const override - { - return cmvr::task::taskStateToString(state_); - } - std::string detailStatusString() const override - { - return stateString(); - } - - int init_calls{0}; - int start_calls{0}; - int stop_calls{0}; - int stop_activity_calls{0}; - std::mutex start_mutex; - std::condition_variable start_condition; - bool start_entered{false}; - -private: - std::string id_; - cmvr::task::TaskShutdownPhase shutdown_phase_; - cmvr::task::TaskState state_{ - cmvr::task::TaskState::UNINITIALIZED}; -}; - -std::shared_ptr created_task; -std::shared_ptr created_ingress_task; - -cmvr::config::TaskManagerConfig enabledTaskConfig() -{ - cmvr::config::TaskManagerConfig config; - auto* entry = config.add_tasks(); - entry->set_id("lifecycle_task"); - entry->set_type( - cmvr::config::TaskConfigEntry::TASK_TYPE_UME_TELEOP); - entry->set_enable(true); - entry->set_run_mode( - cmvr::config::TaskConfigEntry:: - TASK_RUN_MODE_BLOCKING_SERVICE); - return config; -} - -class TaskManagerLifecycleTest : public ::testing::Test { -protected: - void SetUp() override - { - cmvr::task::TaskManager::destroyInstance(); - cmvr::service::globalStopAllAdmissionGate().clearForTesting(); - task_behavior = {}; - created_task.reset(); - created_ingress_task.reset(); - stop_order.clear(); - cmvr::task::TaskFactory::registerCreator( - cmvr::config::TaskConfigEntry::TASK_TYPE_UME_TELEOP, - [](const cmvr::config::TaskConfigEntry& entry) { - created_task = - std::make_shared(entry.id()); - return created_task; - }); - } - - void TearDown() override - { - if (created_task) { - { - std::lock_guard lock(created_task->start_mutex); - task_behavior.block_start = false; - } - created_task->start_condition.notify_all(); - } - cmvr::task::TaskManager::destroyInstance(); - cmvr::service::globalStopAllAdmissionGate().clearForTesting(); - created_task.reset(); - created_ingress_task.reset(); - stop_order.clear(); - } -}; - -TEST_F(TaskManagerLifecycleTest, - EnabledTaskInitFailureMarksInitializationFailed) -{ - task_behavior.init_result = false; - auto& manager = - cmvr::task::TaskManager::getInstance(enabledTaskConfig()); - - ASSERT_NE(created_task, nullptr); - EXPECT_FALSE(manager.initialized()); - EXPECT_FALSE(manager.startRunTask()); - EXPECT_FALSE(manager.running()); - EXPECT_EQ(created_task->init_calls, 1); - EXPECT_EQ(created_task->start_calls, 0); - EXPECT_EQ(created_task->stop_calls, 1); -} - -TEST_F(TaskManagerLifecycleTest, - TaskStartFailureIsReturnedAndRunningRemainsFalse) -{ - task_behavior.start_result = false; - auto& manager = - cmvr::task::TaskManager::getInstance(enabledTaskConfig()); - - ASSERT_TRUE(manager.initialized()); - ASSERT_NE(created_task, nullptr); - EXPECT_FALSE(manager.startRunTask()); - EXPECT_FALSE(manager.running()); - EXPECT_EQ(created_task->start_calls, 1); - EXPECT_EQ(created_task->stop_calls, 1); -} - -TEST_F(TaskManagerLifecycleTest, - TaskStartExceptionIsReturnedAndRunningRemainsFalse) -{ - task_behavior.throw_on_start = true; - auto& manager = - cmvr::task::TaskManager::getInstance(enabledTaskConfig()); - - ASSERT_TRUE(manager.initialized()); - ASSERT_NE(created_task, nullptr); - EXPECT_FALSE(manager.startRunTask()); - EXPECT_FALSE(manager.running()); - EXPECT_EQ(created_task->start_calls, 1); - EXPECT_EQ(created_task->stop_calls, 1); -} - -TEST_F(TaskManagerLifecycleTest, SuccessfulStartAndStopAreReported) -{ - auto& manager = - cmvr::task::TaskManager::getInstance(enabledTaskConfig()); - - ASSERT_TRUE(manager.initialized()); - ASSERT_NE(created_task, nullptr); - EXPECT_TRUE(manager.startRunTask()); - EXPECT_TRUE(manager.running()); - manager.stopRunTask(); - EXPECT_FALSE(manager.running()); - EXPECT_EQ(created_task->start_calls, 1); - EXPECT_EQ(created_task->stop_calls, 1); -} - -TEST_F(TaskManagerLifecycleTest, - CommandIngressStopsBeforeDependentTaskActivity) -{ - cmvr::task::TaskFactory::registerCreator( - cmvr::config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER, - [](const cmvr::config::TaskConfigEntry& entry) { - created_ingress_task = std::make_shared( - entry.id(), - cmvr::task::TaskShutdownPhase::COMMAND_INGRESS); - return created_ingress_task; - }); - - auto config = enabledTaskConfig(); - auto* ingress = config.add_tasks(); - ingress->set_id("control_ingress"); - ingress->set_type( - cmvr::config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER); - ingress->set_enable(true); - ingress->set_run_mode( - cmvr::config::TaskConfigEntry::TASK_RUN_MODE_BLOCKING_SERVICE); - - auto& manager = cmvr::task::TaskManager::getInstance(config); - ASSERT_TRUE(manager.initialized()); - ASSERT_TRUE(manager.startRunTask()); - manager.stopRunTask(); - - ASSERT_EQ(stop_order.size(), 2U); - EXPECT_EQ(stop_order[0], "control_ingress"); - EXPECT_EQ(stop_order[1], "lifecycle_task"); -} - -TEST_F(TaskManagerLifecycleTest, - DestroyDoesNotHoldSingletonLockWhileStoppingTasks) -{ - task_behavior.query_snapshot_on_stop = true; - auto& manager = - cmvr::task::TaskManager::getInstance(enabledTaskConfig()); - ASSERT_TRUE(manager.initialized()); - ASSERT_TRUE(manager.startRunTask()); - - auto destroy = std::async(std::launch::async, [] { - cmvr::task::TaskManager::destroyInstance(); - }); - ASSERT_EQ( - destroy.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - destroy.get(); - - ASSERT_NE(created_task, nullptr); - EXPECT_EQ(created_task->stop_calls, 1); -} - -TEST_F(TaskManagerLifecycleTest, - StopAllActivitiesDoesNotStopSchedulerOrTaskLifecycle) -{ - auto& manager = - cmvr::task::TaskManager::getInstance(enabledTaskConfig()); - - ASSERT_TRUE(manager.initialized()); - ASSERT_TRUE(manager.startRunTask()); - ASSERT_NE(created_task, nullptr); - EXPECT_TRUE(manager.stopAllActivities()); - EXPECT_TRUE(manager.running()); - EXPECT_EQ(created_task->stop_activity_calls, 1); - EXPECT_EQ(created_task->stop_calls, 0); - - manager.stopRunTask(); - EXPECT_EQ(created_task->stop_calls, 1); -} - -TEST_F(TaskManagerLifecycleTest, - StopAllActivitiesReportsUnconfirmedOperationalStop) -{ - task_behavior.stop_activity_result = false; - auto& manager = - cmvr::task::TaskManager::getInstance(enabledTaskConfig()); - - std::vector failures; - EXPECT_FALSE(manager.stopAllActivities(&failures)); - ASSERT_EQ(failures.size(), 1U); - EXPECT_NE(failures.front().find("lifecycle_task"), std::string::npos); - EXPECT_EQ(created_task->stop_calls, 0); -} - -TEST_F(TaskManagerLifecycleTest, - StopAllActivitiesIsSuccessfulWhenManagerIsNotInitialized) -{ - cmvr::task::TaskManager::destroyInstance(); - std::vector failures; - EXPECT_TRUE( - cmvr::task::TaskManager::stopAllActivitiesIfInitialized(&failures)); - EXPECT_TRUE(failures.empty()); -} - -TEST_F(TaskManagerLifecycleTest, StartIsRejectedWhileStopAllAdmissionIsClosed) -{ - auto& manager = - cmvr::task::TaskManager::getInstance(enabledTaskConfig()); - auto& gate = cmvr::service::globalStopAllAdmissionGate(); - const auto ticket = gate.beginStopAll(); - - EXPECT_FALSE(manager.startRunTask()); - EXPECT_FALSE(manager.running()); - ASSERT_NE(created_task, nullptr); - EXPECT_EQ(created_task->start_calls, 0); - - EXPECT_TRUE(gate.finishStopAll(ticket, true)); - EXPECT_TRUE(manager.startRunTask()); - EXPECT_TRUE(manager.running()); -} - -TEST_F(TaskManagerLifecycleTest, - StartCrossingStopAllGenerationRollsBackStartedTasks) -{ - task_behavior.block_start = true; - auto& manager = - cmvr::task::TaskManager::getInstance(enabledTaskConfig()); - ASSERT_NE(created_task, nullptr); - - std::atomic start_result{true}; - std::thread starter([&] { - start_result.store(manager.startRunTask()); - }); - bool start_entered = false; - { - std::unique_lock lock(created_task->start_mutex); - start_entered = created_task->start_condition.wait_for( - lock, std::chrono::seconds(2), [&] { - return created_task->start_entered; - }); - } - if (!start_entered) { - { - std::lock_guard lock(created_task->start_mutex); - task_behavior.block_start = false; - } - created_task->start_condition.notify_all(); - starter.join(); - FAIL() << "task start did not reach the generation-race barrier"; - } - - auto& gate = cmvr::service::globalStopAllAdmissionGate(); - const auto ticket = gate.beginStopAll(); - { - std::lock_guard lock(created_task->start_mutex); - task_behavior.block_start = false; - } - created_task->start_condition.notify_all(); - starter.join(); - - EXPECT_FALSE(start_result.load()); - EXPECT_FALSE(manager.running()); - EXPECT_EQ(created_task->start_calls, 1); - EXPECT_EQ(created_task->stop_calls, 1); - - EXPECT_TRUE(gate.finishStopAll(ticket, true)); - EXPECT_TRUE(manager.startRunTask()); -} - -TEST_F(TaskManagerLifecycleTest, - FailedStopAllKeepsTaskStartupRejectedUntilSuccessfulRound) -{ - auto& manager = - cmvr::task::TaskManager::getInstance(enabledTaskConfig()); - auto& gate = cmvr::service::globalStopAllAdmissionGate(); - auto ticket = gate.beginStopAll(); - EXPECT_FALSE(gate.finishStopAll(ticket, false)); - - EXPECT_FALSE(manager.startRunTask()); - ASSERT_NE(created_task, nullptr); - EXPECT_EQ(created_task->start_calls, 0); - - ticket = gate.beginStopAll(); - EXPECT_TRUE(gate.finishStopAll(ticket, true)); - EXPECT_TRUE(manager.startRunTask()); -} - -} // namespace diff --git a/cmvr-es/runtime/CMakeLists.txt b/cmvr-es/runtime/CMakeLists.txt index 91c68a2c..f8a9a628 100644 --- a/cmvr-es/runtime/CMakeLists.txt +++ b/cmvr-es/runtime/CMakeLists.txt @@ -13,39 +13,8 @@ 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 ) add_library(cmvr_es::runtime ALIAS cmvr_runtime) install(TARGETS cmvr_runtime LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) - -if(BUILD_TESTING) - add_executable(runtime_lifecycle_test - tests/runtime_lifecycle_test.cpp - ) - target_compile_features(runtime_lifecycle_test PRIVATE cxx_std_17) - target_include_directories(runtime_lifecycle_test PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(runtime_lifecycle_test PRIVATE - cmvr_es::runtime - gtest - gtest_main - pthread - ) - add_test( - NAME runtime_lifecycle_test - COMMAND runtime_lifecycle_test - ) - set(_runtime_lifecycle_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _runtime_lifecycle_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(runtime_lifecycle_test PROPERTIES - TIMEOUT 20 - ENVIRONMENT "${_runtime_lifecycle_test_environment}" - ) -endif() diff --git a/cmvr-es/runtime/src/cmvr_runtime.cpp b/cmvr-es/runtime/src/cmvr_runtime.cpp index b39d51d7..6f0378ae 100644 --- a/cmvr-es/runtime/src/cmvr_runtime.cpp +++ b/cmvr-es/runtime/src/cmvr_runtime.cpp @@ -12,7 +12,6 @@ #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 { @@ -101,27 +100,13 @@ bool Runtime::init_(const std::string& config_path, return false; } CMVR_LOG(INFO) << "[Startup] Initialize DeviceManager"; - 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(); - }; + device::DeviceManager::getInstance(device_manager_root.device_manager()); 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"); @@ -131,19 +116,11 @@ 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"; - 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; - } + task::TaskManager::getInstance(task_manager_root.task_manager()); initialized_ = true; return true; } @@ -157,26 +134,9 @@ 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"; - 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; - } + task::TaskManager::getInstance().startRunTask(control_period_s); tasks_started_ = true; return true; } diff --git a/cmvr-es/runtime/tests/runtime_lifecycle_test.cpp b/cmvr-es/runtime/tests/runtime_lifecycle_test.cpp deleted file mode 100644 index 60f8d7ca..00000000 --- a/cmvr-es/runtime/tests/runtime_lifecycle_test.cpp +++ /dev/null @@ -1,266 +0,0 @@ -#include "runtime/include/cmvr_runtime.h" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include "cmvr/config/cmvr_es_config/cmvr_es_config.pb.h" -#include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" -#include "cmvr/config/logger_config/logger_config.pb.h" -#include "cmvr/config/task_manager_config/task_manager_config.pb.h" -#include "common/io/proto_file_io.h" - -namespace { - -class TempConfigTree { -public: - TempConfigTree() - { - std::array pattern{}; - const std::string value = - "/tmp/cmvr-runtime-lifecycle-XXXXXX"; - std::copy(value.begin(), value.end(), pattern.begin()); - char* created = ::mkdtemp(pattern.data()); - if (created) { - root_ = created; - } - } - - ~TempConfigTree() - { - if (root_.empty()) { - return; - } - std::error_code error; - std::filesystem::remove_all(root_, error); - } - - bool valid() const { return !root_.empty(); } - - template - bool write(const std::string& name, const Message& message) const - { - return ProtoMessageIo::setProtoToAsciiFile( - message, (root_ / name).string()); - } - - std::string path(const std::string& name) const - { - return (root_ / name).string(); - } - - bool writeLogger() const - { - cmvr::config::LoggerRootConfig root; - auto* logger = root.mutable_logger(); - logger->set_minimum_level( - cmvr::config::LOG_LEVEL_INFO); - auto* route = logger->add_routes(); - route->set_level(cmvr::config::LOG_LEVEL_INFO); - route->set_terminal(false); - route->set_file(false); - return write("logger.pb.txt", root); - } - - bool writeRoot() const - { - cmvr::config::CMVRESRootConfig root; - auto* config = root.mutable_cmvr_es(); - config->set_logger_config_file("logger.pb.txt"); - config->set_device_manager_config_file( - "device_manager.pb.txt"); - config->set_task_manager_config_file( - "task_manager.pb.txt"); - return write("cmvr_es.pb.txt", root); - } - -private: - std::filesystem::path root_; -}; - -class OccupiedTcpPort { -public: - OccupiedTcpPort() - { - fd_ = ::socket(AF_INET, SOCK_STREAM | SOCK_CLOEXEC, 0); - if (fd_ < 0) { - return; - } - - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); - address.sin_port = 0; - if (::bind(fd_, reinterpret_cast(&address), - sizeof(address)) != 0 || - ::listen(fd_, 1) != 0) { - ::close(fd_); - fd_ = -1; - return; - } - - socklen_t length = sizeof(address); - if (::getsockname(fd_, - reinterpret_cast(&address), - &length) != 0) { - ::close(fd_); - fd_ = -1; - return; - } - port_ = ntohs(address.sin_port); - } - - ~OccupiedTcpPort() - { - if (fd_ >= 0) { - ::close(fd_); - } - } - - bool valid() const { return fd_ >= 0 && port_ != 0; } - std::string port() const { return std::to_string(port_); } - -private: - int fd_{-1}; - unsigned short port_{0}; -}; - -TEST(RuntimeLifecycleTest, EnabledDeviceInitFailureFailsRuntimeInit) -{ - TempConfigTree tree; - ASSERT_TRUE(tree.valid()); - ASSERT_TRUE(tree.writeLogger()); - ASSERT_TRUE(tree.writeRoot()); - - cmvr::config::DeviceManagerRootConfig devices; - auto* entry = - devices.mutable_device_manager()->add_devices(); - entry->set_id("unsupported"); - entry->set_type( - cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN); - entry->set_enable(true); - ASSERT_TRUE(tree.write("device_manager.pb.txt", devices)); - - cmvr::config::TaskManagerRootConfig tasks; - ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks)); - - cmvr::Runtime runtime; - EXPECT_FALSE(runtime.init(tree.path("cmvr_es.pb.txt"))); - EXPECT_FALSE(runtime.initialized()); - EXPECT_FALSE(runtime.tasksStarted()); -} - -TEST(RuntimeLifecycleTest, EnabledTaskInitFailureFailsRuntimeInit) -{ - TempConfigTree tree; - ASSERT_TRUE(tree.valid()); - ASSERT_TRUE(tree.writeLogger()); - ASSERT_TRUE(tree.writeRoot()); - - cmvr::config::DeviceManagerRootConfig devices; - ASSERT_TRUE(tree.write("device_manager.pb.txt", devices)); - - cmvr::config::TaskManagerRootConfig tasks; - auto* entry = tasks.mutable_task_manager()->add_tasks(); - entry->set_id("unsupported"); - entry->set_type( - cmvr::config::TaskConfigEntry::TASK_TYPE_UNKNOWN); - entry->set_enable(true); - entry->set_run_mode( - cmvr::config::TaskConfigEntry:: - TASK_RUN_MODE_BLOCKING_SERVICE); - ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks)); - - cmvr::Runtime runtime; - EXPECT_FALSE(runtime.init(tree.path("cmvr_es.pb.txt"))); - EXPECT_FALSE(runtime.initialized()); - EXPECT_FALSE(runtime.tasksStarted()); -} - -TEST(RuntimeLifecycleTest, - TaskConfigLoadFailureRollsBackDeviceManagerSingleton) -{ - TempConfigTree first_tree; - ASSERT_TRUE(first_tree.valid()); - ASSERT_TRUE(first_tree.writeLogger()); - ASSERT_TRUE(first_tree.writeRoot()); - - cmvr::config::DeviceManagerRootConfig first_devices; - first_devices.mutable_device_manager()->set_name("first"); - ASSERT_TRUE(first_tree.write( - "device_manager.pb.txt", first_devices)); - // Deliberately do not create task_manager.pb.txt. - - cmvr::Runtime runtime; - ASSERT_FALSE( - runtime.init(first_tree.path("cmvr_es.pb.txt"))); - ASSERT_FALSE(runtime.initialized()); - - TempConfigTree second_tree; - ASSERT_TRUE(second_tree.valid()); - ASSERT_TRUE(second_tree.writeLogger()); - ASSERT_TRUE(second_tree.writeRoot()); - - cmvr::config::DeviceManagerRootConfig second_devices; - second_devices.mutable_device_manager()->set_name("second"); - ASSERT_TRUE(second_tree.write( - "device_manager.pb.txt", second_devices)); - cmvr::config::TaskManagerRootConfig second_tasks; - ASSERT_TRUE(second_tree.write( - "task_manager.pb.txt", second_tasks)); - - ASSERT_TRUE( - runtime.init(second_tree.path("cmvr_es.pb.txt"))); - EXPECT_EQ(runtime.deviceManager().name(), "second"); -} - -TEST(RuntimeLifecycleTest, GrpcBindFailureDoesNotMarkTasksStarted) -{ - OccupiedTcpPort occupied_port; - ASSERT_TRUE(occupied_port.valid()); - - TempConfigTree tree; - ASSERT_TRUE(tree.valid()); - ASSERT_TRUE(tree.writeLogger()); - ASSERT_TRUE(tree.writeRoot()); - - cmvr::config::DeviceManagerRootConfig devices; - ASSERT_TRUE(tree.write("device_manager.pb.txt", devices)); - - cmvr::config::GRPCServerRootConfig grpc; - auto* grpc_config = grpc.mutable_grpc_server(); - grpc_config->set_id("grpc_server"); - grpc_config->set_host("127.0.0.1"); - grpc_config->set_port(occupied_port.port()); - ASSERT_TRUE(tree.write("grpc.pb.txt", grpc)); - - cmvr::config::TaskManagerRootConfig tasks; - auto* entry = tasks.mutable_task_manager()->add_tasks(); - entry->set_id("grpc_server"); - entry->set_type( - cmvr::config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER); - entry->set_enable(true); - entry->set_run_mode( - cmvr::config::TaskConfigEntry:: - TASK_RUN_MODE_BLOCKING_SERVICE); - entry->set_config_file("grpc.pb.txt"); - ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks)); - - cmvr::Runtime runtime; - ASSERT_TRUE(runtime.init(tree.path("cmvr_es.pb.txt"))); - EXPECT_FALSE(runtime.startTasks()); - EXPECT_FALSE(runtime.tasksStarted()); - EXPECT_FALSE(runtime.taskManager().running()); -} - -} // namespace diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 4e0971cf..24fad685 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -1,4 +1,101 @@ -# Service is intentionally split by transport. The QUIC edge target is added -# from the project root before task targets; the gRPC tree is added here after -# its manager and task dependencies are available. -add_subdirectory(grpc) + +add_library(service + grpc/src/grpc_camera_service.cpp + grpc/src/grpc_system_service.cpp + grpc/src/grpc_speaker_service.cpp + grpc/src/grpc_microphone_service.cpp + grpc/src/grpc_head_service.cpp + grpc/src/grpc_dexhand_service.cpp + grpc/src/grpc_arm_service.cpp + grpc/src/grpc_agv_service.cpp + grpc/src/grpc_hlc_service.cpp + ../task/grpc_server_task/src/grpc_server_task.cpp +) + +target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(service PRIVATE + cmvr_es::proto + osqp + cmvr_es::device_manager + cmvr_es::task_manager + cmvr_es::algorithms::controller + cmvr_es::task + cmvr_es::media_source_hub + cmvr_es::device_media_source_adapter + protobuf::libprotobuf +) + +add_library(cmvr_es::service ALIAS service) +install(TARGETS service LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(grpc_camera_stream_policy_test + grpc/tests/grpc_camera_stream_policy_test.cpp + ) + target_include_directories(grpc_camera_stream_policy_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + add_test( + NAME grpc_camera_stream_policy_test + COMMAND grpc_camera_stream_policy_test + ) + set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10) +endif() + +# -------------------------------------------------------- +# Unit test +# -------------------------------------------------------- +find_package(OpenCV REQUIRED) + +include_directories( + ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include +) + +link_directories( + ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib +) + + +add_executable(grpc_arm_client_test + grpc/src/grpc_arm_client_test.cpp +) + + +target_link_libraries(grpc_arm_client_test + PRIVATE + cmvr_es::device::canbus + cmvr_es::device::ti5_canopen_motor_driver + osqp + gtest + gtest_main + pthread + glog + cmvr_es::proto + ccd + fcl + cmvr_es::device_manager + ${OpenCV_LIBS} +) + + +add_executable(grpc_hlc_client_test + grpc/src/grpc_hlc_client_test.cpp +) + + +target_link_libraries(grpc_hlc_client_test + PRIVATE + cmvr_es::device::canbus + cmvr_es::device::ti5_canopen_motor_driver + osqp + gtest + gtest_main + pthread + glog + cmvr_es::proto + ccd + fcl + cmvr_es::device_manager +) diff --git a/cmvr-es/service/README.md b/cmvr-es/service/README.md index 39a1ee13..0ed71894 100644 --- a/cmvr-es/service/README.md +++ b/cmvr-es/service/README.md @@ -6,33 +6,13 @@ ## 当前结构 -`service/` 顶层只按传输协议保留两个子目录：`grpc/` 和 `quic_edge/`。gRPC -内部再按运行角色分层，避免把队列、停止控制、客户端和服务端实现混在同一层。 - -```text -service/ -├── grpc/ -│ ├── action/ # ActionQueue 校验、账本和 FIFO 执行器 -│ ├── client/ # 边缘端使用的 gRPC client -│ ├── server/ # gRPC service、协调器和安全扩展 -│ │ ├── include/ -│ │ ├── src/ -│ │ └── tests/ -│ └── stop_all/ # 高优先级 StopAll 通道 -└── quic_edge/ # QUIC client、控制状态机和媒体 packetizer -``` - | 目录 | 职责 | | --- | --- | -| `grpc/action/` | SystemService ActionQueue 的校验、幂等账本和边缘端 FIFO 执行器 | -| `grpc/client/` | 面向边缘端内部调用的 gRPC client | -| `grpc/server/` | 入站设备控制、状态查询、兼容流式接口和安全控制面 | -| `grpc/stop_all/` | 不进入普通命令队列的高优先级停止通道 | +| `grpc/` | 入站设备控制、状态查询和兼容流式接口 | | `quic_edge/` | 边缘端主动连接平台的 QUIC client、控制状态机和媒体 packetizer | | `quic_edge/tests/` | 已登记到 CTest 的 QUIC 协议测试 | -两个遗留 gRPC client test 位于 `grpc/server/tests/*_client_test.cpp`，当前没有通过 -`add_test()` 登记；它们是历史可执行文件，不代表默认自动覆盖。 +两个遗留 gRPC client test 位于 `grpc/src/*_client_test.cpp`，当前没有通过 `add_test()` 登记。 gRPC 和 QUIC 的职责边界： @@ -41,105 +21,6 @@ gRPC 和 QUIC 的职责边界： - 实时音视频使用 QUIC DATAGRAM； - `quic_edge/` 不是平台 Gateway，也不是浏览器服务器。 -## SystemService 设备清单 - -`SystemService/GetDeviceList` 返回 `DeviceManager` 的当前只读快照，只包含 -`enabled=true` 的设备。启用但创建、初始化、启动或健康检查失败的设备仍会返回， -并通过 `manager_state`、`health`、`has_error` 和 `error_message` 描述异常。 -接口同时返回稳定的 `device_type` 和仅用于展示/诊断的具体 `type_name`；调用方 -不得使用 `type_name` 做设备类别判断。 - -该 RPC 不修改配置、不动态注册设备，也不触发设备生命周期操作。启用 reflection -后可直接查询： - -```bash -grpcurl -plaintext \ - -d '{}' \ - 127.0.0.1:50052 \ - cmvr.api.SystemService/GetDeviceList -``` - -## SystemService ActionQueue - -`SystemService/ExecuteActionQueue` 接收一个完整的有限动作序列，在边缘端排队并 -逐步串行执行，所有步骤结束后返回最终结果。平台只需要提交一次请求，因此连续机械臂 -动作不会再受到每个单独 gRPC 往返和 Wi-Fi 抖动的影响。 - -当前 v1 仅允许以下 `ActionStep.command`： - -- 机械臂同步 `MoveJ`、`MoveL`； -- AGV 同步 `navigateToPose`、`navigateToStation`、`followPath`； -- 边缘端本地 `delay`。 - -机械臂和 AGV 请求复用各自已有的类型化 Request，目标设备仍由每一步的 -`header.device_id` 指定。所有运动步骤必须设置 `asynchronous=false`；`MoveL` v1 仅接受 -Base frame；AGV 后端还必须明确支持同步导航终态确认。`speedJ`、`speedL`、`servoJ`、 -AGV `translate`、速度控制、查询和流式 RPC 都不属于 ActionQueue v1。 - -ActionQueue 遵循以下执行语义： - -- 平台先调用 `GetSystemInfo` 读取 `action_service_instance_id`，并在每次提交和重试中填入 - `expected_service_instance_id`。ActionQueue 账本随服务实例重建；若断线期间边缘服务重启， - 旧实例 ID 会被拒绝，平台必须先对账，不能用新 ID 自动重放不确定的动作； -- `action_id` 是必填的全局唯一幂等键；同一服务实例内，相同内容的已受理请求不会重复下发 - 设备命令，相同 ID 但内容不同的请求必须拒绝。服务端缓存最近 4096 个完整结果，更早的 - 已执行 ID 由精确 retired-ID 账本 fail-closed 拒绝、不会重跑；单实例最多记录 - 262144 个已受理 ID，达到容量后仅拒绝新 ID，已有 ID 仍可查询； -- 入队前校验全部步骤、设备、参数和同步能力，校验失败时不会执行任何步骤； -- v1 每个请求最多 256 步、序列化大小最多 512 KiB、排队或执行中的 Action 最多 64 个、 - 同时提交或等待结果的 RPC 最多 256 个；Action 与单步超时上限均为 24 小时， - `total_timeout_ms=0` 使用 30 分钟默认值，AGV 路径最多 4096 段； -- `total_timeout_ms` 包含排队与执行时间，单步 `timeout_ms=0` 时继承 Action 剩余时间 - 或服务端默认值；所有超时值均由服务端施加上限； -- 任一步失败、取消或超时后立即停止序列，不再执行后续步骤；`completed_steps` 表示此前 - 成功完成的步骤数，`failed_step_index` 仅在存在对应失败步骤时出现； -- Action 一旦受理，不因平台连接中断而自动取消；断线只结束该 RPC waiter，边缘动作继续。 - 平台可用相同 `action_id` 重试并取得仍在缓存中的同一次执行结果； -- `StopAll`、机械臂 `stopMotion`、AGV `cancelNavigation` 和软件急停不进入 FIFO，必须 - 作为高优先级安全/抢占路径执行。它们仍不具备功能安全等级。 -- 机械臂步骤超时会立即走 typed `stopMotion` 并等待停车确认；若无法确认停车，设备控制权 - 保持隔离，不会继续后续步骤或接受新的普通控制命令；需先按设备安全流程确认状态，再 - 重启边缘服务恢复控制。 -- AGV 的取消 ACK、零速度 ACK 均不等于停稳；ActionQueue 和安全停止 RPC 只有在导航任务 - 终态且底盘连续零速度采样确认后才释放控制权，否则同样保留隔离。 - -已知的执行完成、业务失败、取消、超时和预校验拒绝由 `ActionResultCode` 与 -`CommandHeader.Feedback` 表达。`ActionDeduplicationStatus` 结构化区分新受理、合并等待、 -缓存结果、已淘汰结果、账本耗尽、ID 冲突和服务实例不匹配；平台不得通过解析错误字符串 -判断动作是否执行过。 -`ACTION_RESULT_CODE_UNSPECIFIED` 不得作为服务端最终结果。 - -## MotorService - -`MotorService` 将 gRPC 电机命令适配到已经由 `DeviceManager` 创建的 -`MotorManager` 和 `AbstractMotor`，不直接持有现场总线或厂商驱动。 - -关键文件： - -- Proto：[`../../protos/cmvr/api/motor_service.proto`](../../protos/cmvr/api/motor_service.proto) - 和 [`../../protos/cmvr/api/motor_command.proto`](../../protos/cmvr/api/motor_command.proto) -- 实现：[`grpc/server/include/grpc_motor_service.h`](grpc/server/include/grpc_motor_service.h) - 和 [`grpc/server/src/grpc_motor_service.cpp`](grpc/server/src/grpc_motor_service.cpp) -- 注册：[`../task/grpc_server_task/src/grpc_server_task.cpp`](../task/grpc_server_task/src/grpc_server_task.cpp) -- 单元测试：[`grpc/server/tests/grpc_motor_service_test.cpp`](grpc/server/tests/grpc_motor_service_test.cpp) - -服务按单电机仲裁。同步 Profile 命令、Cyclic Position/Velocity 双向流、 -`setEnabled`、状态读取和软件 `emergencyStop` 共用同一控制权状态： - -- 同一电机已有 owner 时拒绝新的控制调用； -- cyclic 流首帧必须是 `open`，后续 setpoint sequence 必须严格递增； -- reader 使用 latest-wins 邮箱，客户端必须持续并发读取反馈； -- 取消、deadline、watchdog、非法帧、后端拒绝或写失败都会触发 Quick Stop； -- 任何清理 Quick Stop 未确认时，服务进入 fail-closed 锁存； -- 只有成功执行 `setEnabled(true)` 才解除服务内软件急停锁存； -- 服务层 Quick Stop 和 `emergencyStop` 都不具备功能安全等级。 - -AUBO 控制柜 IO 不经过 `MotorService`，由 -`ArmService/ExecuteJsonCommand` 转发到目标 `RobotArm`。厂商命令和安全约束见 -[AUBO 控制柜 IO](../devices/arm/aubo_arm/README.md)。 -旧的 `SystemService/ExecuteJsonCommand` 已移除；相机 PTZ 应使用类型化的 -`CameraService/ControlPtz`。 - ## 新增 gRPC Service 当前没有动态 service registry，必须完成以下全部步骤。 @@ -159,9 +40,8 @@ import 路径必须相对于 `protos/`。兼容规则见 [`../../protos/README.m ```text service/grpc/ -└── server/ - ├── include/grpc_example_service.h - └── src/grpc_example_service.cpp +├── include/grpc_example_service.h +└── src/grpc_example_service.cpp ``` 实现类继承生成的： @@ -223,7 +103,7 @@ cmvr::api::ExampleService::Service - 检查 `context->IsCancelled()`； - 检查 `Read()` / `Write()` 返回； -- 使用 RAII 或 MediaSourceManager Subscription 释放 producer lease； +- 使用 RAII 或 MediaSourceHub Subscription 释放 producer lease； - 不持有设备状态锁进行网络写； - 为 wait/read 使用有限 timeout； - 慢客户端不能阻塞设备生产线程； @@ -231,7 +111,7 @@ cmvr::api::ExampleService::Service - gRPC RGB 流在积压超过 `camera_stream_max_pending_frames` 或帧龄超过 `camera_stream_max_frame_age_ms` 时主动丢弃旧帧，请求 IDR，并从下一个关键帧恢复。 -当前仅 gRPC RGB 和麦克风流使用 MediaSourceManager；Depth/RGBD 仍直接读取设备帧。 +当前仅 gRPC RGB 和麦克风流使用 MediaSourceHub；Depth/RGBD 仍直接读取设备帧。 gRPC 相机实时流默认最多保留 2 帧积压、最大允许 250 ms 帧龄。两个配置项填 0 时使用上述默认值。该策略以低延迟为目标，不保证每个视频帧都到达客户端；控制命令 diff --git a/cmvr-es/service/grpc/CMakeLists.txt b/cmvr-es/service/grpc/CMakeLists.txt deleted file mode 100644 index cff56bd2..00000000 --- a/cmvr-es/service/grpc/CMakeLists.txt +++ /dev/null @@ -1,550 +0,0 @@ - -add_library(service - stop_all/src/stop_operation_dispatcher.cpp - action/src/action_queue_executor.cpp - server/src/camera_ptz_activity_registry.cpp - server/src/media_activity_coordinator.cpp - server/src/motor_activity_coordinator.cpp - server/src/grpc_camera_service.cpp - server/src/grpc_command_transaction.cpp - server/src/grpc_error_logging_interceptor.cpp - server/src/grpc_recovery_audit.cpp - server/src/grpc_safety_proto.cpp - server/src/grpc_safety_participants.cpp - server/src/grpc_security.cpp - server/src/grpc_system_service.cpp - server/src/grpc_speaker_service.cpp - server/src/grpc_microphone_service.cpp - server/src/grpc_head_service.cpp - server/src/grpc_dexhand_service.cpp - server/src/grpc_arm_service.cpp - server/src/grpc_arm_teleop_service.cpp - server/src/grpc_robot_arm_teleop_backend.cpp - server/src/grpc_motor_service.cpp - server/src/grpc_agv_service.cpp - server/src/grpc_hlc_service.cpp - ../../task/grpc_server_task/src/grpc_server_task.cpp -) - -target_include_directories(service PUBLIC - ${CMAKE_SOURCE_DIR}/cmvr-es -) - -target_link_libraries(service PRIVATE - cmvr_es::proto - cmvr_es::stop_all_admission_gate - cmvr_es::camera_operational_activity_registry - osqp - cmvr_es::control_authority_manager - cmvr_es::device_manager - cmvr_es::task_manager - cmvr_es::algorithms::controller - cmvr_es::task - cmvr_es::media_source_manager - cmvr_es::device_media_source_adapter - protobuf::libprotobuf -) - -add_library(cmvr_es::service ALIAS service) -install(TARGETS service LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - add_executable(stop_all_admission_gate_test - stop_all/tests/stop_all_admission_gate_test.cpp - stop_all/src/stop_all_admission_gate.cpp - ) - target_include_directories(stop_all_admission_gate_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(stop_all_admission_gate_test PRIVATE - gtest - gtest_main - pthread - ) - add_test( - NAME stop_all_admission_gate_test - COMMAND stop_all_admission_gate_test - ) - set_tests_properties(stop_all_admission_gate_test PROPERTIES TIMEOUT 10) - - add_executable(stop_operation_dispatcher_test - stop_all/tests/stop_operation_dispatcher_test.cpp - stop_all/src/stop_operation_dispatcher.cpp - ) - target_include_directories(stop_operation_dispatcher_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(stop_operation_dispatcher_test PRIVATE - gtest - gtest_main - pthread - ) - add_test( - NAME stop_operation_dispatcher_test - COMMAND stop_operation_dispatcher_test - ) - set_tests_properties(stop_operation_dispatcher_test PROPERTIES TIMEOUT 10) - - add_executable(camera_operational_activity_registry_test - server/tests/camera_operational_activity_registry_test.cpp - ) - target_include_directories(camera_operational_activity_registry_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(camera_operational_activity_registry_test PRIVATE - cmvr_es::proto - cmvr_es::camera_operational_activity_registry - gtest - gtest_main - pthread - ) - add_test( - NAME camera_operational_activity_registry_test - COMMAND camera_operational_activity_registry_test - ) - set_tests_properties(camera_operational_activity_registry_test PROPERTIES - TIMEOUT 10) - - add_executable(camera_ptz_activity_registry_test - server/tests/camera_ptz_activity_registry_test.cpp - server/src/camera_ptz_activity_registry.cpp - stop_all/src/stop_all_admission_gate.cpp - ) - target_include_directories(camera_ptz_activity_registry_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(camera_ptz_activity_registry_test PRIVATE - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME camera_ptz_activity_registry_test - COMMAND camera_ptz_activity_registry_test - ) - set_tests_properties(camera_ptz_activity_registry_test PROPERTIES TIMEOUT 10) - - add_executable(media_activity_coordinator_test - server/tests/media_activity_coordinator_test.cpp - server/src/media_activity_coordinator.cpp - stop_all/src/stop_all_admission_gate.cpp - ) - target_include_directories(media_activity_coordinator_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(media_activity_coordinator_test PRIVATE - cmvr_es::logging - pthread - ) - add_test( - NAME media_activity_coordinator_test - COMMAND media_activity_coordinator_test - ) - set(_grpc_media_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_media_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(media_activity_coordinator_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_media_test_environment}" - ) - - add_executable(motor_activity_coordinator_test - server/tests/motor_activity_coordinator_test.cpp - server/src/motor_activity_coordinator.cpp - ) - target_include_directories(motor_activity_coordinator_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(motor_activity_coordinator_test PRIVATE - cmvr_es::logging - pthread - ) - add_test( - NAME motor_activity_coordinator_test - COMMAND motor_activity_coordinator_test - ) - set_tests_properties(motor_activity_coordinator_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_media_test_environment}" - ) - - add_executable(grpc_camera_stream_policy_test - server/tests/grpc_camera_stream_policy_test.cpp - ) - target_include_directories(grpc_camera_stream_policy_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - add_test( - NAME grpc_camera_stream_policy_test - COMMAND grpc_camera_stream_policy_test - ) - set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10) - - add_executable(grpc_system_service_test - server/tests/grpc_system_service_test.cpp - ) - target_include_directories(grpc_system_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_system_service_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_system_service_test - COMMAND grpc_system_service_test - ) - set(_grpc_system_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_system_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_system_service_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - - add_executable(grpc_error_logging_interceptor_test - server/tests/grpc_error_logging_interceptor_test.cpp - server/src/grpc_error_logging_interceptor.cpp - ) - target_include_directories(grpc_error_logging_interceptor_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(grpc_error_logging_interceptor_test - PRIVATE - cmvr_es::logging - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_error_logging_interceptor_test - COMMAND grpc_error_logging_interceptor_test - ) - set_tests_properties(grpc_error_logging_interceptor_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - - add_executable(grpc_security_test - server/tests/grpc_security_test.cpp - server/src/grpc_security.cpp - ) - target_include_directories(grpc_security_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(grpc_security_test - PRIVATE - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_security_test - COMMAND grpc_security_test - ) - set_tests_properties(grpc_security_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - - add_executable(grpc_command_transaction_test - server/tests/grpc_command_transaction_test.cpp - server/src/grpc_command_transaction.cpp - server/src/grpc_safety_proto.cpp - server/src/grpc_security.cpp - ) - target_include_directories(grpc_command_transaction_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(grpc_command_transaction_test PRIVATE - cmvr_es::safety_manager - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_command_transaction_test - COMMAND grpc_command_transaction_test - ) - set_tests_properties(grpc_command_transaction_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - - add_executable(grpc_arm_service_test - server/tests/grpc_arm_service_test.cpp - ) - target_include_directories(grpc_arm_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_arm_service_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_arm_service_test - COMMAND grpc_arm_service_test - ) - set_tests_properties(grpc_arm_service_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - - add_executable(grpc_arm_teleop_service_test - server/tests/grpc_arm_teleop_service_test.cpp - ) - target_include_directories(grpc_arm_teleop_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(grpc_arm_teleop_service_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_arm_teleop_service_test - COMMAND grpc_arm_teleop_service_test - ) - set(_grpc_arm_teleop_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_arm_teleop_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_arm_teleop_service_test PROPERTIES - TIMEOUT 20 - ENVIRONMENT "${_grpc_arm_teleop_test_environment}" - ) - - add_executable(grpc_robot_arm_teleop_backend_test - server/tests/grpc_robot_arm_teleop_backend_test.cpp - ) - target_include_directories(grpc_robot_arm_teleop_backend_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ) - target_link_libraries(grpc_robot_arm_teleop_backend_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_robot_arm_teleop_backend_test - COMMAND grpc_robot_arm_teleop_backend_test - ) - set_tests_properties(grpc_robot_arm_teleop_backend_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_arm_teleop_test_environment}" - ) - - add_executable(grpc_motor_service_test - server/tests/grpc_motor_service_test.cpp - ) - target_include_directories(grpc_motor_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_motor_service_test - PRIVATE - service - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_motor_service_test - COMMAND grpc_motor_service_test - ) - set(_grpc_motor_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_motor_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_motor_service_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_motor_test_environment}" - ) - - add_executable(grpc_agv_service_test - server/tests/grpc_agv_service_test.cpp - ) - target_include_directories(grpc_agv_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_agv_service_test - PRIVATE - service - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_agv_service_test - COMMAND grpc_agv_service_test - ) - set(_grpc_agv_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_agv_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_agv_service_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_grpc_agv_test_environment}" - ) - - add_executable(grpc_head_service_test - server/tests/grpc_head_service_test.cpp - ) - target_include_directories(grpc_head_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_head_service_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_head_service_test - COMMAND grpc_head_service_test - ) - set_tests_properties(grpc_head_service_test PROPERTIES - TIMEOUT 15 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - - add_executable(grpc_dexhand_service_test - server/tests/grpc_dexhand_service_test.cpp - ) - target_include_directories(grpc_dexhand_service_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ) - target_link_libraries(grpc_dexhand_service_test - PRIVATE - service - cmvr_es::proto - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_dexhand_service_test - COMMAND grpc_dexhand_service_test - ) - set_tests_properties(grpc_dexhand_service_test PROPERTIES - TIMEOUT 15 - ENVIRONMENT "${_grpc_system_test_environment}" - ) - -endif() - -# -------------------------------------------------------- -# Unit test -# -------------------------------------------------------- -find_package(OpenCV REQUIRED) - -include_directories( - ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include -) - -link_directories( - ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib -) - - -add_executable(grpc_arm_client_test - server/tests/grpc_arm_client_test.cpp -) - - -target_link_libraries(grpc_arm_client_test - PRIVATE - cmvr_es::device::canbus - cmvr_es::device::ti5_canopen_motor_driver - osqp - gtest - gtest_main - pthread - glog - cmvr_es::proto - ccd - fcl - cmvr_es::device_manager - ${OpenCV_LIBS} -) - - -add_executable(grpc_hlc_client_test - server/tests/grpc_hlc_client_test.cpp -) - - -target_link_libraries(grpc_hlc_client_test - PRIVATE - cmvr_es::device::canbus - cmvr_es::device::ti5_canopen_motor_driver - osqp - gtest - gtest_main - pthread - glog - cmvr_es::proto - ccd - fcl - cmvr_es::device_manager -) diff --git a/cmvr-es/service/grpc/action/include/action_queue_executor.h b/cmvr-es/service/grpc/action/include/action_queue_executor.h deleted file mode 100644 index a80eb1be..00000000 --- a/cmvr-es/service/grpc/action/include/action_queue_executor.h +++ /dev/null @@ -1,102 +0,0 @@ -#ifndef CMVR_ES_ACTION_QUEUE_EXECUTOR_H -#define CMVR_ES_ACTION_QUEUE_EXECUTOR_H - -#include -#include -#include -#include -#include -#include - -#include "cmvr/api/system_command.pb.h" -#include "manager/safety_manager/include/safety_types.h" - -namespace cmvr::device { -class DeviceManager; -} - -namespace cmvr::service { - -// Owns the process-local FIFO used by SystemService ActionQueue requests. -// The executor intentionally has no grpc::ServerContext dependency: once a -// request is accepted, loss of the platform connection must not cancel device -// motion on the edge. -class ActionQueueExecutor final { -public: - static constexpr std::size_t kDefaultMaxAcceptedActionIds = - 256U * 1024U; - - enum class WaitResult { - Terminal, - CanceledBeforeAdmission, - CanceledAfterAdmission, - }; - - // Identifies one participant in a StopAll round. Multiple concurrent - // StopAll callers join the same round; ActionQueue admission resumes only - // after every ticket in that round has been completed successfully. - struct StopAllTicket { - std::uint64_t generation{0}; - std::uint64_t ticket_id{0}; - bool active_action_stop_confirmed{false}; - - bool valid() const noexcept - { - return generation != 0U && ticket_id != 0U; - } - }; - - explicit ActionQueueExecutor( - device::DeviceManager& device_manager, - std::size_t max_accepted_action_ids = - kDefaultMaxAcceptedActionIds); - ~ActionQueueExecutor(); - - ActionQueueExecutor(const ActionQueueExecutor&) = delete; - ActionQueueExecutor& operator=(const ActionQueueExecutor&) = delete; - - // Validates, idempotently enqueues, and waits for the terminal result. - // Protocol and execution outcomes are represented in Feedback. - WaitResult submitAndWait( - const api::ActionQueueCommand_Request& request, - api::ActionQueueCommand_Feedback& feedback, - const std::function& waiter_canceled = {}, - safety::CommandActor actor = {}); - - // Starts (or joins) a temporary StopAll round. New action IDs are rejected - // and queued/active actions are canceled. By default the executor also - // requests a typed stop for the active action. SystemService delegates that - // stop to its whole-machine sweep so one slow Action backend cannot delay - // stop requests for every other device. - // Existing action IDs remain queryable for idempotent reconciliation. An - // invalid ticket means permanent shutdown has already started. - StopAllTicket beginStopAll(bool delegate_active_stop = false); - - // Completes a StopAll participant after the caller has stopped and - // confirmed all other devices. The queue resumes only when every ticket in - // the current round reports success and the worker is idle. True means this - // ticket was completed successfully; another concurrent ticket may still - // keep the queue paused. False is fail-closed: the ticket was stale, - // shutdown won the race, a stop was not confirmed, or the executor was not - // idle when the last ticket completed. - bool finishStopAll( - const StopAllTicket& ticket, - bool all_devices_stop_confirmed); - - // Permanently rejects new actions, cancels queued/active work, requests a - // typed stop, and tells the worker to exit after canceled work is drained. - // This transition is irreversible for this executor instance. Returns true - // when the active Action devices reported a confirmed stop. - bool disableForShutdown(); - - bool waitForIdle(std::chrono::milliseconds timeout); - const std::string& instanceId() const noexcept; - -private: - struct Impl; - std::unique_ptr impl_; -}; - -} // namespace cmvr::service - -#endif // CMVR_ES_ACTION_QUEUE_EXECUTOR_H diff --git a/cmvr-es/service/grpc/action/src/action_queue_executor.cpp b/cmvr-es/service/grpc/action/src/action_queue_executor.cpp deleted file mode 100644 index 07289f7f..00000000 --- a/cmvr-es/service/grpc/action/src/action_queue_executor.cpp +++ /dev/null @@ -1,2525 +0,0 @@ -#include "service/grpc/action/include/action_queue_executor.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#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 "devices/agv/abstract_agv.h" -#include "devices/arm/robot_arm.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" -#include "manager/device_manager/include/device_manager.h" -#include "manager/safety_manager/include/safety_manager.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { -namespace { - -using Clock = std::chrono::steady_clock; -using Milliseconds = std::chrono::milliseconds; - -constexpr std::size_t kMaxSteps = 256; -constexpr std::size_t kMaxQueuedActions = 64; -constexpr std::size_t kMaxTerminalResults = 4096; -constexpr std::size_t kMaxRequestBytes = 512U * 1024U; -constexpr std::size_t kMaxIdentifierBytes = 128; -constexpr std::size_t kMaxPathSegments = 4096; -constexpr std::size_t kMaxConcurrentSubmitters = 256; -constexpr Milliseconds kDefaultTotalTimeout = std::chrono::minutes(30); -constexpr Milliseconds kMaximumTimeout = std::chrono::hours(24); -constexpr Milliseconds kLeaseRetryPeriod{10}; -constexpr Milliseconds kWaiterCancellationPollPeriod{20}; -constexpr Milliseconds kArmWatchdogPollPeriod{5}; -constexpr Milliseconds kArmIdleConfirmationPollPeriod{10}; -constexpr auto kConcurrentArmStopConfirmationTimeout = - std::chrono::seconds(7); -constexpr auto kControlLeaseTtl = std::chrono::hours(25); - -enum class PreparedStepKind { - ArmMoveJ, - ArmMoveL, - AgvNavigateToPose, - AgvNavigateToStation, - AgvFollowPath, - Delay, -}; - -struct PreparedStep { - PreparedStepKind kind{PreparedStepKind::Delay}; - std::string device_id; - std::shared_ptr arm; - std::shared_ptr agv; -}; - -struct PreparedAction { - std::vector steps; - std::vector resource_ids; -}; - -safety::CommandDescriptor safetyDescriptorFor( - const PreparedStepKind kind) -{ - using safety::CommandIntent; - using safety::SafetyPolicyFamily; - switch (kind) { - case PreparedStepKind::ArmMoveJ: - return {"/cmvr.api.ArmService/moveJ", CommandIntent::Actuate, - SafetyPolicyFamily::Control, true, false}; - case PreparedStepKind::ArmMoveL: - return {"/cmvr.api.ArmService/moveL", CommandIntent::Actuate, - SafetyPolicyFamily::Control, true, false}; - case PreparedStepKind::AgvNavigateToPose: - return {"/cmvr.api.AgvService/navigateToPose", - CommandIntent::Actuate, SafetyPolicyFamily::Control, - true, false}; - case PreparedStepKind::AgvNavigateToStation: - return {"/cmvr.api.AgvService/navigateToStation", - CommandIntent::Actuate, SafetyPolicyFamily::Control, - true, false}; - case PreparedStepKind::AgvFollowPath: - return {"/cmvr.api.AgvService/followPath", - CommandIntent::Actuate, SafetyPolicyFamily::Control, - true, false}; - case PreparedStepKind::Delay: - break; - } - return {}; -} - -const api::CommandHeader_Request* commandHeaderFor( - const api::ActionStep& step) -{ - switch (step.command_case()) { - case api::ActionStep::kArmMoveJ: - return &step.arm_move_j().header(); - case api::ActionStep::kArmMoveL: - return &step.arm_move_l().header(); - case api::ActionStep::kAgvNavigateToPose: - return &step.agv_navigate_to_pose().header(); - case api::ActionStep::kAgvNavigateToStation: - return &step.agv_navigate_to_station().header(); - case api::ActionStep::kAgvFollowPath: - return &step.agv_follow_path().header(); - case api::ActionStep::kDelay: - case api::ActionStep::COMMAND_NOT_SET: - return nullptr; - } - return nullptr; -} - -std::string actionStepCommandId( - const std::string& action_id, - const std::string& step_id) -{ - return "action:" + std::to_string(action_id.size()) + ':' + - action_id + ":step:" + std::to_string(step_id.size()) + ':' + - step_id; -} - -struct ValidationResult { - bool valid{false}; - std::string error; - int failed_step_index{-1}; - PreparedAction prepared; -}; - -struct RequestFingerprint { - std::array words{}; - std::uint64_t serialized_size{0}; - - bool operator==(const RequestFingerprint& other) const noexcept - { - return serialized_size == other.serialized_size && - words == other.words; - } - - bool operator!=(const RequestFingerprint& other) const noexcept - { - return !(*this == other); - } -}; - -std::uint64_t stableHash( - const std::string& value, - const std::uint64_t seed) noexcept -{ - std::uint64_t hash = 1469598103934665603ULL ^ seed; - for (const unsigned char byte : value) { - hash ^= static_cast(byte); - hash *= 1099511628211ULL; - } - hash ^= hash >> 33U; - hash *= 0xff51afd7ed558ccdULL; - hash ^= hash >> 33U; - hash *= 0xc4ceb9fe1a85ec53ULL; - hash ^= hash >> 33U; - return hash; -} - -std::string generateServiceInstanceId() -{ - std::array bytes{}; - std::size_t offset = 0; - while (offset < bytes.size()) { - const auto received = ::getrandom( - bytes.data() + offset, - bytes.size() - offset, - 0); - if (received < 0) { - if (errno == EINTR) { - continue; - } - throw std::system_error( - errno, - std::generic_category(), - "could not generate the ActionQueue service instance id"); - } - if (received == 0) { - throw std::runtime_error( - "could not generate the ActionQueue service instance id"); - } - offset += static_cast(received); - } - - // RFC 4122 variant and version bits make the epoch recognizable as a - // random UUID without reducing its collision resistance materially. - bytes[6] = static_cast((bytes[6] & 0x0fU) | 0x40U); - bytes[8] = static_cast((bytes[8] & 0x3fU) | 0x80U); - std::ostringstream stream; - stream << std::hex << std::setfill('0'); - for (std::size_t index = 0; index < bytes.size(); ++index) { - if (index == 4U || index == 6U || index == 8U || index == 10U) { - stream << '-'; - } - stream << std::setw(2) << static_cast(bytes[index]); - } - return stream.str(); -} - -std::size_t validatedAcceptedActionLimit(const std::size_t limit) -{ - if (limit == 0U) { - throw std::invalid_argument( - "ActionQueue accepted-action limit must be positive"); - } - return limit; -} - -void fillTerminalFeedback( - api::ActionQueueCommand_Feedback& feedback, - const std::string& action_id, - const api::ActionResultCode result, - const std::uint32_t completed_steps, - const std::string& message, - const int failed_step_index = -1) -{ - feedback.Clear(); - feedback.set_action_id(action_id); - feedback.set_result(result); - feedback.set_completed_steps(completed_steps); - if (failed_step_index >= 0) { - feedback.set_failed_step_index( - static_cast(failed_step_index)); - } - auto* header = feedback.mutable_header(); - header->set_success(result == api::ACTION_RESULT_CODE_COMPLETED); - header->set_error_message(message); - *header->mutable_timestamp() = - google::protobuf::util::TimeUtil::GetCurrentTime(); -} - -bool finite(const double value) noexcept -{ - return std::isfinite(value); -} - -bool finiteNonNegative(const double value) noexcept -{ - return finite(value) && value >= 0.0; -} - -bool validArmOptions( - const api::MotionOptions& options, - std::string& error) -{ - if (options.asynchronous()) { - error = "ActionQueue requires synchronous RobotArm motion"; - return false; - } - if (!finiteNonNegative(options.velocity()) || - !finiteNonNegative(options.acceleration()) || - !finiteNonNegative(options.blend_radius()) || - !finiteNonNegative(options.jerk())) { - error = "RobotArm motion options must be finite and non-negative"; - return false; - } - for (const double limit : options.joint_velocity_limits()) { - if (!finiteNonNegative(limit)) { - error = "RobotArm joint velocity limits must be finite and non-negative"; - return false; - } - } - return true; -} - -bool validAgvOptions( - const msgs::AgvMotionOptions& options, - std::string& error) -{ - if (options.asynchronous()) { - error = "ActionQueue requires synchronous AGV navigation"; - return false; - } - if (!finiteNonNegative(options.max_speed()) || - !finiteNonNegative(options.max_angular_speed()) || - !finiteNonNegative(options.max_acceleration()) || - !finiteNonNegative(options.max_angular_acceleration()) || - !finiteNonNegative(options.reach_distance()) || - !finiteNonNegative(options.reach_angle()) || - !finiteNonNegative(options.speed_ratio())) { - error = "AGV motion options must be finite and non-negative"; - return false; - } - if (options.speed_ratio() > 1.0) { - error = "AGV speed_ratio must be in [0, 1]"; - return false; - } - if (options.wait_timeout_ms() < 0 || - options.poll_interval_ms() < 0) { - error = "AGV timeout and poll interval must be non-negative"; - return false; - } - if (options.poll_interval_ms() > 5000) { - error = "AGV poll_interval_ms must not exceed 5000"; - return false; - } - if (options.wait_timeout_ms() > 0 && - options.poll_interval_ms() > options.wait_timeout_ms()) { - error = "AGV poll_interval_ms must not exceed wait_timeout_ms"; - return false; - } - return true; -} - -device::FrameType toFrameType(const api::ArmFrameType frame) -{ - switch (frame) { - case api::ARM_FRAME_BASE: - return device::FrameType::Base; - 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; - } - return device::FrameType::Base; -} - -device::MotionOptions toArmMotionOptions( - const api::MotionOptions& source) -{ - device::MotionOptions destination; - destination.velocity = source.velocity(); - destination.acceleration = source.acceleration(); - destination.blend_radius = source.blend_radius(); - destination.jerk = source.jerk() > 0.0 ? source.jerk() : 5.0; - destination.joint_velocity_limits.assign( - source.joint_velocity_limits().begin(), - source.joint_velocity_limits().end()); - destination.asynchronous = source.asynchronous(); - return destination; -} - -device::AgvAdapterParams toAgvAdapterParams( - const msgs::AgvAdapterParams& source) -{ - device::AgvAdapterParams destination; - for (const auto& item : source.values()) { - destination.values.emplace(item.first, item.second); - } - return destination; -} - -device::AgvMotionOptions toAgvMotionOptions( - const msgs::AgvMotionOptions& source) -{ - device::AgvMotionOptions destination; - destination.max_speed = source.max_speed(); - destination.max_angular_speed = source.max_angular_speed(); - destination.max_acceleration = source.max_acceleration(); - destination.max_angular_acceleration = - source.max_angular_acceleration(); - destination.reach_distance = source.reach_distance(); - destination.reach_angle = source.reach_angle(); - destination.speed_ratio = - source.speed_ratio() > 0.0 ? source.speed_ratio() : 1.0; - destination.asynchronous = source.asynchronous(); - destination.wait_timeout_ms = source.wait_timeout_ms(); - destination.poll_interval_ms = source.poll_interval_ms(); - return destination; -} - -std::string stepPrefix( - const int index, - const api::ActionStep& step) -{ - std::string prefix = "step " + std::to_string(index); - if (!step.step_id().empty()) { - prefix += " (" + step.step_id() + ")"; - } - return prefix + ": "; -} - -std::string canonicalRequest( - const api::ActionQueueCommand_Request& request) -{ - api::ActionQueueCommand_Request normalized(request); - normalized.DiscardUnknownFields(); - for (auto& step : *normalized.mutable_steps()) { - switch (step.command_case()) { - case api::ActionStep::kArmMoveJ: - step.mutable_arm_move_j() - ->mutable_header()->clear_timestamp(); - break; - case api::ActionStep::kArmMoveL: - step.mutable_arm_move_l() - ->mutable_header()->clear_timestamp(); - break; - case api::ActionStep::kAgvNavigateToPose: - step.mutable_agv_navigate_to_pose() - ->mutable_header()->clear_timestamp(); - break; - case api::ActionStep::kAgvNavigateToStation: - step.mutable_agv_navigate_to_station() - ->mutable_header()->clear_timestamp(); - break; - case api::ActionStep::kAgvFollowPath: - step.mutable_agv_follow_path() - ->mutable_header()->clear_timestamp(); - break; - case api::ActionStep::kDelay: - case api::ActionStep::COMMAND_NOT_SET: - break; - } - } - - std::string serialized; - google::protobuf::io::StringOutputStream stream(&serialized); - google::protobuf::io::CodedOutputStream coded_stream(&stream); - coded_stream.SetSerializationDeterministic(true); - if (!normalized.SerializeToCodedStream(&coded_stream)) { - return {}; - } - coded_stream.Trim(); - return serialized; -} - -std::optional fingerprintRequest( - const api::ActionQueueCommand_Request& request) -{ - const std::string serialized = canonicalRequest(request); - if (serialized.empty()) { - return std::nullopt; - } - RequestFingerprint fingerprint; - fingerprint.serialized_size = serialized.size(); - constexpr std::array seeds{ - 0xa4093822299f31d0ULL, - 0x082efa98ec4e6c89ULL, - 0x452821e638d01377ULL, - 0xbe5466cf34e90c6cULL}; - for (std::size_t index = 0; index < seeds.size(); ++index) { - fingerprint.words[index] = stableHash(serialized, seeds[index]); - } - return fingerprint; -} - -std::string validateRequestEnvelope( - const api::ActionQueueCommand_Request& request) -{ - if (request.action_id().empty()) { - return "action_id is required"; - } - if (request.action_id().size() > kMaxIdentifierBytes) { - return "action_id is too long"; - } - if (request.expected_service_instance_id().empty()) { - return "expected_service_instance_id is required; obtain it from GetSystemInfo"; - } - if (request.expected_service_instance_id().size() > - kMaxIdentifierBytes) { - return "expected_service_instance_id is too long"; - } - if (request.steps().empty()) { - return "ActionQueue requires at least one step"; - } - if (static_cast(request.steps_size()) > kMaxSteps) { - return "ActionQueue exceeds the maximum step count"; - } - if (request.ByteSizeLong() > kMaxRequestBytes) { - return "ActionQueue request exceeds the 512 KiB application limit"; - } - if (request.total_timeout_ms() > - static_cast(kMaximumTimeout.count())) { - return "ActionQueue total timeout exceeds the server limit"; - } - return {}; -} - -ValidationResult validateRequest( - device::DeviceManager& device_manager, - const api::ActionQueueCommand_Request& request) -{ - ValidationResult result; - result.error = validateRequestEnvelope(request); - if (!result.error.empty()) { - return result; - } - - std::unordered_set step_ids; - std::set resources; - result.prepared.steps.reserve( - static_cast(request.steps_size())); - - for (int index = 0; index < request.steps_size(); ++index) { - result.failed_step_index = index; - const auto& step = request.steps(index); - const std::string prefix = stepPrefix(index, step); - if (step.step_id().empty()) { - result.error = prefix + "step_id is required"; - return result; - } - if (step.step_id().size() > kMaxIdentifierBytes) { - result.error = prefix + "step_id is too long"; - return result; - } - if (!step_ids.emplace(step.step_id()).second) { - result.error = prefix + "step_id must be unique"; - return result; - } - if (step.timeout_ms() > - static_cast(kMaximumTimeout.count())) { - result.error = prefix + "timeout exceeds the server limit"; - return result; - } - - PreparedStep prepared; - std::string options_error; - switch (step.command_case()) { - case api::ActionStep::kArmMoveJ: { - prepared.kind = PreparedStepKind::ArmMoveJ; - const auto& command = step.arm_move_j(); - prepared.device_id = command.header().device_id(); - if (prepared.device_id.empty()) { - result.error = prefix + "RobotArm device_id is required"; - return result; - } - prepared.arm = device_manager.getDevice( - prepared.device_id); - if (!prepared.arm) { - result.error = prefix + "RobotArm device not found: " + - prepared.device_id; - return result; - } - if (!prepared.arm->supportsActionQueueMotion()) { - result.error = prefix + - "RobotArm backend does not support safe ActionQueue motion: " + - prepared.device_id; - return result; - } - if (!validArmOptions(command.options(), options_error)) { - result.error = prefix + options_error; - return result; - } - const auto dof = prepared.arm->getDof(); - if (dof == 0U || command.target().position_size() != - static_cast(dof)) { - result.error = prefix + - "MoveJ target size does not match RobotArm DOF"; - return result; - } - if (!command.options().joint_velocity_limits().empty() && - command.options().joint_velocity_limits_size() != - static_cast(dof)) { - result.error = prefix + - "MoveJ joint velocity limit size does not match RobotArm DOF"; - return result; - } - for (const double position : command.target().position()) { - if (!finite(position)) { - result.error = prefix + - "MoveJ target must contain finite values"; - return result; - } - } - resources.emplace(prepared.device_id); - break; - } - case api::ActionStep::kArmMoveL: { - prepared.kind = PreparedStepKind::ArmMoveL; - const auto& command = step.arm_move_l(); - prepared.device_id = command.header().device_id(); - if (prepared.device_id.empty()) { - result.error = prefix + "RobotArm device_id is required"; - return result; - } - prepared.arm = device_manager.getDevice( - prepared.device_id); - if (!prepared.arm) { - result.error = prefix + "RobotArm device not found: " + - prepared.device_id; - return result; - } - if (!prepared.arm->supportsActionQueueMotion()) { - result.error = prefix + - "RobotArm backend does not support safe ActionQueue motion: " + - prepared.device_id; - return result; - } - if (!validArmOptions(command.options(), options_error)) { - result.error = prefix + options_error; - return result; - } - if (!api::ArmFrameType_IsValid(command.frame())) { - result.error = prefix + "MoveL frame is invalid"; - return result; - } - if (command.frame() != api::ARM_FRAME_BASE) { - result.error = prefix + - "ActionQueue MoveL currently supports the Base frame only"; - return result; - } - const auto dof = prepared.arm->getDof(); - if (!command.options().joint_velocity_limits().empty() && - command.options().joint_velocity_limits_size() != - static_cast(dof)) { - result.error = prefix + - "MoveL joint velocity limit size does not match RobotArm DOF"; - return result; - } - const auto& target = command.target(); - if (!finite(target.x()) || !finite(target.y()) || - !finite(target.z()) || !finite(target.rx()) || - !finite(target.ry()) || !finite(target.rz())) { - result.error = prefix + - "MoveL target must contain finite values"; - return result; - } - resources.emplace(prepared.device_id); - break; - } - case api::ActionStep::kAgvNavigateToPose: { - prepared.kind = PreparedStepKind::AgvNavigateToPose; - const auto& command = step.agv_navigate_to_pose(); - prepared.device_id = command.header().device_id(); - if (prepared.device_id.empty()) { - result.error = prefix + "AGV device_id is required"; - return result; - } - prepared.agv = device_manager.getDevice( - prepared.device_id); - if (!prepared.agv) { - result.error = prefix + "AGV device not found: " + - prepared.device_id; - return result; - } - if (!prepared.agv->supportsSynchronousAction( - device::AgvActionKind::NavigateToPose)) { - result.error = prefix + - "AGV backend cannot confirm synchronous pose navigation: " + - prepared.device_id; - return result; - } - if (!validAgvOptions(command.options(), options_error)) { - result.error = prefix + options_error; - return result; - } - if (!finite(command.pose().x()) || - !finite(command.pose().y()) || - !finite(command.pose().theta())) { - result.error = prefix + - "AGV pose must contain finite values"; - return result; - } - resources.emplace(prepared.device_id); - break; - } - case api::ActionStep::kAgvNavigateToStation: { - prepared.kind = PreparedStepKind::AgvNavigateToStation; - const auto& command = step.agv_navigate_to_station(); - prepared.device_id = command.header().device_id(); - if (prepared.device_id.empty()) { - result.error = prefix + "AGV device_id is required"; - return result; - } - if (command.station_id().empty()) { - result.error = prefix + "AGV station_id is required"; - return result; - } - prepared.agv = device_manager.getDevice( - prepared.device_id); - if (!prepared.agv) { - result.error = prefix + "AGV device not found: " + - prepared.device_id; - return result; - } - if (!prepared.agv->supportsSynchronousAction( - device::AgvActionKind::NavigateToStation)) { - result.error = prefix + - "AGV backend cannot confirm synchronous station navigation: " + - prepared.device_id; - return result; - } - if (!validAgvOptions(command.options(), options_error)) { - result.error = prefix + options_error; - return result; - } - resources.emplace(prepared.device_id); - break; - } - case api::ActionStep::kAgvFollowPath: { - prepared.kind = PreparedStepKind::AgvFollowPath; - const auto& command = step.agv_follow_path(); - prepared.device_id = command.header().device_id(); - if (prepared.device_id.empty()) { - result.error = prefix + "AGV device_id is required"; - return result; - } - if (command.path().empty()) { - result.error = prefix + "AGV path must not be empty"; - return result; - } - if (static_cast(command.path_size()) > - kMaxPathSegments) { - result.error = prefix + "AGV path is too large"; - return result; - } - for (const auto& segment : command.path()) { - if (segment.source_station().empty() || - segment.target_station().empty()) { - result.error = prefix + - "AGV path station ids must not be empty"; - return result; - } - } - prepared.agv = device_manager.getDevice( - prepared.device_id); - if (!prepared.agv) { - result.error = prefix + "AGV device not found: " + - prepared.device_id; - return result; - } - if (!prepared.agv->supportsSynchronousAction( - device::AgvActionKind::FollowPath)) { - result.error = prefix + - "AGV backend cannot confirm synchronous path navigation: " + - prepared.device_id; - return result; - } - if (!validAgvOptions(command.options(), options_error)) { - result.error = prefix + options_error; - return result; - } - resources.emplace(prepared.device_id); - break; - } - case api::ActionStep::kDelay: - prepared.kind = PreparedStepKind::Delay; - if (step.delay().duration_ms() > - static_cast(kMaximumTimeout.count())) { - result.error = prefix + - "delay exceeds the server limit"; - return result; - } - break; - case api::ActionStep::COMMAND_NOT_SET: - result.error = prefix + "command is not set"; - return result; - } - result.prepared.steps.push_back(std::move(prepared)); - } - - result.prepared.resource_ids.assign( - resources.begin(), resources.end()); - result.failed_step_index = -1; - result.valid = true; - return result; -} - -device::AgvActionKind toAgvActionKind( - const PreparedStepKind kind) -{ - switch (kind) { - case PreparedStepKind::AgvNavigateToPose: - return device::AgvActionKind::NavigateToPose; - case PreparedStepKind::AgvNavigateToStation: - return device::AgvActionKind::NavigateToStation; - case PreparedStepKind::AgvFollowPath: - return device::AgvActionKind::FollowPath; - default: - return device::AgvActionKind::NavigateToPose; - } -} - -} // namespace - -struct ActionQueueExecutor::Impl { - enum class RunState { - Accepting, - PausedForStopAll, - ShuttingDown, - }; - - struct Record { - api::ActionQueueCommand_Request request; - RequestFingerprint fingerprint; - PreparedAction prepared; - safety::CommandActor actor; - Clock::time_point deadline; - std::atomic cancel_requested{false}; - std::atomic timed_out{false}; - // -1 means that no device step is currently inside a backend call. - // StopAll reads this without taking Record::mutex so it can preempt - // the physically active device before stopping the remaining action - // resources. - std::atomic active_step_index{-1}; - // Exact normal leases acquired for this execution. Typed-stop paths - // may convert only these generations into safety barriers, so a stale - // Action can never preempt a later command on the same device. - std::unordered_map - control_tokens; - // Fallback for an invariant or manager failure which prevents an exact - // lease from being converted into a permanent quarantine. - std::atomic retain_control_leases{false}; - std::mutex mutex; - std::condition_variable condition; - bool done{false}; - std::string stop_error; - api::ActionQueueCommand_Feedback feedback; - }; - - struct TerminalResult { - RequestFingerprint fingerprint; - api::ActionQueueCommand_Feedback feedback; - }; - - struct LeaseSet { - explicit LeaseSet(std::shared_ptr owner_record) - : record(std::move(owner_record)) - { - } - - ~LeaseSet() - { - if (record && record->retain_control_leases.load( - std::memory_order_acquire)) { - return; - } - auto& manager = - control::ControlAuthorityManager::instance(); - for (const auto& token : tokens) { - manager.release(token); - } - } - - const control::ControlLeaseToken* find( - const std::string& resource_id) const - { - const auto found = std::find_if( - tokens.begin(), tokens.end(), - [&resource_id](const auto& token) { - return token.resource_id == resource_id; - }); - return found == tokens.end() ? nullptr : &*found; - } - - std::vector tokens; - std::shared_ptr record; - }; - - struct SubmitterGuard { - explicit SubmitterGuard(std::atomic& value) - : count(value) - { - } - - ~SubmitterGuard() - { - count.fetch_sub(1U, std::memory_order_acq_rel); - } - - std::atomic& count; - }; - - struct DeduplicationFeedbackGuard { - ~DeduplicationFeedbackGuard() - { - feedback.set_deduplication_status(status); - } - - api::ActionQueueCommand_Feedback& feedback; - api::ActionDeduplicationStatus& status; - }; - - explicit Impl( - device::DeviceManager& manager, - const std::size_t accepted_action_limit) - : device_manager(manager), - instance_id(generateServiceInstanceId()), - max_accepted_action_ids( - validatedAcceptedActionLimit(accepted_action_limit)), - worker([this]() { workerLoop(); }) - { - } - - ~Impl() - { - shutdown(); - } - - void shutdown() - { - if (joined) { - return; - } - (void)disableForShutdown(); - if (worker.joinable()) { - worker.join(); - } - joined = true; - } - - void cancelAllLocked(std::shared_ptr& active_record) - { - for (const auto& record : queue) { - record->cancel_requested.store( - true, std::memory_order_release); - record->condition.notify_all(); - } - active_record = active; - if (active_record) { - active_record->cancel_requested.store( - true, std::memory_order_release); - active_record->condition.notify_all(); - } - } - - ActionQueueExecutor::StopAllTicket beginStopAll( - const bool delegate_active_stop) - { - ActionQueueExecutor::StopAllTicket ticket; - std::shared_ptr active_record; - { - std::lock_guard lock(mutex); - if (run_state == RunState::ShuttingDown) { - return ticket; - } - if (run_state == RunState::Accepting || - outstanding_stop_all_tickets.empty()) { - run_state = RunState::PausedForStopAll; - ++admission_generation; - ++stop_all_generation; - stop_all_failed = false; - } - ticket.generation = stop_all_generation; - ticket.ticket_id = ++next_stop_all_ticket_id; - outstanding_stop_all_tickets.emplace(ticket.ticket_id); - cancelAllLocked(active_record); - } - - if (delegate_active_stop) { - // finishStopAll(all_devices_stop_confirmed=true) is the external - // confirmation for the active Action device in this mode. - ticket.active_action_stop_confirmed = true; - } else { - try { - ticket.active_action_stop_confirmed = - requestTypedStop(active_record); - } catch (const std::exception& error) { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] StopAll typed stop threw, error=" - << error.what(); - } catch (...) { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] StopAll typed stop threw"; - } - } - queue_condition.notify_all(); - return ticket; - } - - bool finishStopAll( - const ActionQueueExecutor::StopAllTicket& ticket, - const bool all_devices_stop_confirmed) - { - std::lock_guard lock(mutex); - if (run_state != RunState::PausedForStopAll || - !ticket.valid() || - ticket.generation != stop_all_generation || - outstanding_stop_all_tickets.erase(ticket.ticket_id) == 0U) { - return false; - } - - const bool caller_confirmed = - ticket.active_action_stop_confirmed && - all_devices_stop_confirmed; - if (!caller_confirmed) { - stop_all_failed = true; - } - if (!outstanding_stop_all_tickets.empty()) { - return caller_confirmed; - } - if (stop_all_failed || !queue.empty() || active) { - stop_all_failed = true; - return false; - } - - run_state = RunState::Accepting; - return true; - } - - bool disableForShutdown() - { - std::shared_ptr active_record; - { - std::lock_guard lock(mutex); - if (run_state != RunState::ShuttingDown) { - run_state = RunState::ShuttingDown; - ++admission_generation; - ++stop_all_generation; - outstanding_stop_all_tickets.clear(); - stop_all_failed = true; - } - cancelAllLocked(active_record); - } - - bool stopped = false; - try { - stopped = requestTypedStop(active_record); - } catch (const std::exception& error) { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] shutdown typed stop threw, error=" - << error.what(); - } catch (...) { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] shutdown typed stop threw"; - } - queue_condition.notify_all(); - return stopped; - } - - static bool waitForArmIdle( - const std::shared_ptr& arm, - const Clock::duration timeout) - { - const auto deadline = Clock::now() + timeout; - while (Clock::now() < deadline) { - try { - if (!arm->busy()) { - return true; - } - } catch (...) { - return false; - } - std::this_thread::sleep_for( - kArmIdleConfirmationPollPeriod); - } - try { - return !arm->busy(); - } catch (...) { - return false; - } - } - - enum class FailClosedResult { - Quarantined, - AlreadyFenced, - LeaseRetained, - }; - - static FailClosedResult quarantineOrRetainLease( - const std::shared_ptr& record, - const control::ControlLeaseToken& expected_token) noexcept - { - auto& authority = - control::ControlAuthorityManager::instance(); - if (authority.quarantineIfCurrent(expected_token)) { - // LeaseSet must release the displaced handler token when it exits; - // that release is what makes this quarantine recoverable. - return FailClosedResult::Quarantined; - } - try { - if (!authority.validate(expected_token)) { - return FailClosedResult::AlreadyFenced; - } - } catch (...) { - } - - // No independently managed barrier could be confirmed. Retaining the - // exact Action lease is the last fail-closed fallback. - record->retain_control_leases.store( - true, std::memory_order_release); - return FailClosedResult::LeaseRetained; - } - - bool requestTypedStop(const std::shared_ptr& record) - { - if (!record) { - return true; - } - bool all_stopped = true; - std::unordered_set stopped_arms; - std::unordered_set stopped_agvs; - std::vector stop_order; - stop_order.reserve(record->prepared.steps.size()); - const int active_step_index = - record->active_step_index.load(std::memory_order_acquire); - if (active_step_index >= 0 && - static_cast(active_step_index) < - record->prepared.steps.size()) { - stop_order.push_back( - static_cast(active_step_index)); - } - for (std::size_t index = 0; - index < record->prepared.steps.size(); ++index) { - if (active_step_index >= 0 && - index == static_cast(active_step_index)) { - continue; - } - stop_order.push_back(index); - } - for (const std::size_t index : stop_order) { - const auto& step = record->prepared.steps[index]; - const bool is_arm = step.arm && - stopped_arms.emplace(step.device_id).second; - const bool is_agv = step.agv && - stopped_agvs.emplace(step.device_id).second; - if (!is_arm && !is_agv) { - continue; - } - auto& authority = - control::ControlAuthorityManager::instance(); - const std::string owner = - "grpc-system:action-cancel:" + - record->request.action_id() + ":" + - std::to_string( - sequence.fetch_add( - 1U, std::memory_order_relaxed) + 1U); - const auto ttl = std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>( - std::chrono::minutes(1)); - std::optional expected_token; - { - std::lock_guard lock(record->mutex); - const auto found = - record->control_tokens.find(step.device_id); - if (found != record->control_tokens.end()) { - expected_token = found->second; - } - } - if (!expected_token) { - // Cancellation may race an Action which is still waiting to - // acquire its device set. No Action-owned motion has been - // submitted in that state, so there is nothing to stop. - if (active_step_index >= 0) { - all_stopped = false; - record->retain_control_leases.store( - true, std::memory_order_release); - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] active device has no exact " - "control token; retaining Action leases, id=" - << step.device_id; - } - continue; - } - - control::ControlAcquireResult barrier; - try { - barrier = authority.preemptAcquireIfCurrent( - *expected_token, owner, ttl); - } catch (const std::exception& error) { - all_stopped = false; - (void)quarantineOrRetainLease(record, *expected_token); - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] could not establish typed stop " - "barrier; control remains quarantined, id=" - << step.device_id << ", error=" << error.what(); - continue; - } catch (...) { - all_stopped = false; - (void)quarantineOrRetainLease(record, *expected_token); - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] could not establish typed stop " - "barrier; control remains quarantined, id=" - << step.device_id; - continue; - } - if (!barrier.acquired) { - const auto fail_closed = - quarantineOrRetainLease(record, *expected_token); - if (fail_closed != FailClosedResult::AlreadyFenced) { - all_stopped = false; - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] exact typed stop barrier was " - "not established while the Action lease remained " - "current; control remains quarantined, id=" - << step.device_id - << ", detail=" << barrier.detail; - } - continue; - } - - bool stop_confirmed = false; - try { - if (is_arm) { - const auto result = step.arm->stopMotion(); - stop_confirmed = result.ok(); - if (!stop_confirmed) { - CMVR_LOG(WARNING) - << "[ActionQueueExecutor] typed RobotArm stop failed, id=" - << step.device_id << ", error=" << result.message; - stop_confirmed = waitForArmIdle( - step.arm, - kConcurrentArmStopConfirmationTimeout); - } - } else { - const auto cancel = step.agv->cancelNavigation(); - if (!cancel.ok()) { - CMVR_LOG(WARNING) - << "[ActionQueueExecutor] typed AGV cancel failed, id=" - << step.device_id << ", error=" << cancel.message; - } - const auto velocity_stop = - step.agv->stopVelocityControl(); - if (!velocity_stop.ok() && - velocity_stop.code != - device::AgvErrorCode::UnsupportedCommand) { - CMVR_LOG(WARNING) - << "[ActionQueueExecutor] typed AGV velocity stop failed, id=" - << step.device_id - << ", error=" << velocity_stop.message; - } - const auto stopped = - step.agv->confirmMotionStopped(); - stop_confirmed = stopped.ok(); - if (!stop_confirmed) { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] AGV stopped state was " - "not confirmed, id=" - << step.device_id - << ", error=" << stopped.message; - } - } - } catch (const std::exception& error) { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] typed stop threw, id=" - << step.device_id << ", error=" << error.what(); - } catch (...) { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] typed stop threw, id=" - << step.device_id; - } - if (stop_confirmed) { - authority.release(barrier.token); - } else { - all_stopped = false; - if (!authority.retireSafetyHolder(barrier.token)) { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] failed to retire an " - "unconfirmed typed-stop barrier, id=" - << step.device_id; - } - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] typed stop was not confirmed; " - "control remains quarantined, id=" - << step.device_id; - // Retain this safety holder fail-closed. A normal lease must - // not be admitted while the physical outcome is unknown. - } - } - return all_stopped; - } - - static bool waiterCanceled( - const std::function& waiter_canceled) noexcept - { - if (!waiter_canceled) { - return false; - } - try { - return waiter_canceled(); - } catch (...) { - // A broken waiter must not cancel an admitted edge action. End only - // this caller's wait and leave the worker-owned Record untouched. - return true; - } - } - - ActionQueueExecutor::WaitResult submitAndWait( - const api::ActionQueueCommand_Request& request, - api::ActionQueueCommand_Feedback& feedback, - const std::function& waiter_canceled, - safety::CommandActor actor) - { - api::ActionDeduplicationStatus deduplication_status = - api::ACTION_DEDUPLICATION_STATUS_UNSPECIFIED; - DeduplicationFeedbackGuard deduplication_feedback{ - feedback, deduplication_status}; - const auto previous_submitters = concurrent_submitters.fetch_add( - 1U, std::memory_order_acq_rel); - if (previous_submitters >= kMaxConcurrentSubmitters) { - concurrent_submitters.fetch_sub(1U, std::memory_order_acq_rel); - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - "ActionQueue has too many concurrent submitters"); - return ActionQueueExecutor::WaitResult::Terminal; - } - SubmitterGuard submitter_guard(concurrent_submitters); - - const std::string envelope_error = - validateRequestEnvelope(request); - if (!envelope_error.empty()) { - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - envelope_error); - return ActionQueueExecutor::WaitResult::Terminal; - } - if (request.expected_service_instance_id() != instance_id) { - deduplication_status = - api::ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH; - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - "expected_service_instance_id does not match the active ActionQueue service instance; reconcile the prior action before submitting a new id"); - return ActionQueueExecutor::WaitResult::Terminal; - } - const auto fingerprint = fingerprintRequest(request); - if (!fingerprint) { - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - "ActionQueue request could not be serialized"); - return ActionQueueExecutor::WaitResult::Terminal; - } - std::shared_ptr record; - const auto lookup_existing_locked = [&]() { - const auto found = records.find(request.action_id()); - if (found != records.end()) { - if (found->second->fingerprint != *fingerprint) { - deduplication_status = - api::ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT; - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - "action_id is already associated with a different request"); - return true; - } - record = found->second; - { - std::lock_guard record_lock(record->mutex); - deduplication_status = record->done - ? api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT - : api::ACTION_DEDUPLICATION_STATUS_JOINED_IN_FLIGHT; - } - return false; - } - - const auto terminal = terminal_results.find(request.action_id()); - if (terminal != terminal_results.end()) { - if (terminal->second.fingerprint != *fingerprint) { - deduplication_status = - api::ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT; - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - "action_id is already associated with a different request"); - } else { - feedback = terminal->second.feedback; - deduplication_status = - api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT; - } - return true; - } - - if (retired_action_ids.find(request.action_id()) != - retired_action_ids.end()) { - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - "action_id was already completed but its result is no longer cached; it will not be re-executed"); - deduplication_status = - api::ACTION_DEDUPLICATION_STATUS_RESULT_EVICTED; - return true; - } - return false; - }; - - bool handled = false; - std::uint64_t observed_admission_generation = 0U; - std::uint64_t observed_system_admission_generation = 0U; - const bool initially_canceled = waiterCanceled(waiter_canceled); - { - auto system_admission = - globalStopAllAdmissionGate().lockAdmission(); - std::lock_guard lock(mutex); - handled = lookup_existing_locked(); - if (!handled && !record) { - if (!system_admission.accepting() || - run_state != RunState::Accepting) { - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - run_state == RunState::ShuttingDown - ? "ActionQueue is disabled because the service is shutting down" - : "ActionQueue is temporarily paused by StopAll"); - handled = true; - } else { - observed_admission_generation = admission_generation; - observed_system_admission_generation = - system_admission.generation(); - } - } - } - if (handled) { - return ActionQueueExecutor::WaitResult::Terminal; - } - if (record) { - return waitForRecord(record, feedback, waiter_canceled); - } - if (initially_canceled) { - return ActionQueueExecutor::WaitResult::CanceledBeforeAdmission; - } - - const auto validation = validateRequest(device_manager, request); - if (!validation.valid) { - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - validation.error, - validation.failed_step_index); - return ActionQueueExecutor::WaitResult::Terminal; - } - const auto total_timeout = request.total_timeout_ms() == 0U - ? kDefaultTotalTimeout - : Milliseconds(request.total_timeout_ms()); - auto candidate = std::make_shared(); - candidate->request = request; - candidate->fingerprint = *fingerprint; - candidate->prepared = validation.prepared; - candidate->actor = std::move(actor); - candidate->deadline = Clock::now() + total_timeout; - - const bool canceled_before_admission = - waiterCanceled(waiter_canceled); - bool canceled_without_record = false; - { - auto system_admission = - globalStopAllAdmissionGate().lockAdmission(); - std::lock_guard lock(mutex); - handled = lookup_existing_locked(); - if (!handled && !record && canceled_before_admission) { - canceled_without_record = true; - } else if (!handled && !record && - (!system_admission.accepting() || - system_admission.generation() != - observed_system_admission_generation || - run_state != RunState::Accepting || - admission_generation != - observed_admission_generation)) { - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - run_state == RunState::ShuttingDown - ? "ActionQueue is disabled because the service is shutting down" - : "ActionQueue admission was interrupted by StopAll; retry after StopAll completes"); - handled = true; - } else if (!handled && !record && - queue.size() + (active ? 1U : 0U) >= - kMaxQueuedActions) { - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - "ActionQueue is full"); - handled = true; - } else if (!handled && !record && - records.size() + terminal_results.size() + - retired_action_ids.size() >= - max_accepted_action_ids) { - fillTerminalFeedback( - feedback, request.action_id(), - api::ACTION_RESULT_CODE_REJECTED, 0, - "ActionQueue idempotency ledger capacity is exhausted; restart with a new service instance only after reconciling prior actions"); - deduplication_status = - api::ACTION_DEDUPLICATION_STATUS_LEDGER_EXHAUSTED; - handled = true; - } else if (!handled && !record) { - record = std::move(candidate); - const auto inserted = - records.emplace(request.action_id(), record); - if (!inserted.second) { - throw std::logic_error( - "ActionQueue admission record already exists"); - } - try { - queue.push_back(record); - } catch (...) { - // No other thread can observe the record while the queue - // mutex is held. Roll it back so a failed deque allocation - // cannot leave an ID which waits forever without work. - records.erase(inserted.first); - record.reset(); - throw; - } - deduplication_status = - api::ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW; - queue_condition.notify_one(); - } - } - if (handled) { - return ActionQueueExecutor::WaitResult::Terminal; - } - if (record) { - return waitForRecord(record, feedback, waiter_canceled); - } - if (canceled_without_record) { - return ActionQueueExecutor::WaitResult::CanceledBeforeAdmission; - } - return waitForRecord(record, feedback, waiter_canceled); - } - - static ActionQueueExecutor::WaitResult waitForRecord( - const std::shared_ptr& record, - api::ActionQueueCommand_Feedback& feedback, - const std::function& waiter_canceled) - { - std::unique_lock lock(record->mutex); - if (!waiter_canceled) { - record->condition.wait(lock, [&record]() { - return record->done; - }); - feedback = record->feedback; - return ActionQueueExecutor::WaitResult::Terminal; - } - - for (;;) { - if (record->done) { - feedback = record->feedback; - return ActionQueueExecutor::WaitResult::Terminal; - } - lock.unlock(); - if (waiterCanceled(waiter_canceled)) { - return ActionQueueExecutor::WaitResult::CanceledAfterAdmission; - } - lock.lock(); - record->condition.wait_for( - lock, - kWaiterCancellationPollPeriod, - [&record]() { return record->done; }); - } - } - - bool waitForIdle(const Milliseconds timeout) - { - std::unique_lock lock(mutex); - return idle_condition.wait_for(lock, timeout, [this]() { - return queue.empty() && !active; - }); - } - - void workerLoop() - { - for (;;) { - std::shared_ptr record; - { - std::unique_lock lock(mutex); - queue_condition.wait(lock, [this]() { - return run_state == RunState::ShuttingDown || - !queue.empty(); - }); - if (run_state == RunState::ShuttingDown && queue.empty()) { - return; - } - record = queue.front(); - queue.pop_front(); - active = record; - } - - try { - execute(record); - } catch (const std::exception& error) { - complete( - record, api::ACTION_RESULT_CODE_FAILED, 0, - std::string("ActionQueue worker failed: ") + - error.what()); - } catch (...) { - complete( - record, api::ACTION_RESULT_CODE_FAILED, 0, - "ActionQueue worker failed with an unknown exception"); - } - { - std::lock_guard lock(mutex); - if (active == record) { - active.reset(); - } - try { - TerminalResult terminal; - terminal.fingerprint = record->fingerprint; - { - std::lock_guard record_lock(record->mutex); - terminal.feedback = record->feedback; - } - const std::string action_id = - record->request.action_id(); - const auto live = records.find(action_id); - terminal_result_order.push_back(action_id); - try { - const auto inserted = terminal_results.emplace( - action_id, std::move(terminal)); - if (!inserted.second) { - throw std::logic_error( - "ActionQueue terminal result already exists"); - } - } catch (...) { - // Keep the full live record as the source of truth if - // the compact cache cannot be committed atomically. - terminal_result_order.pop_back(); - throw; - } - if (live != records.end()) { - records.erase(live); - } - while (terminal_result_order.size() > - kMaxTerminalResults) { - const std::string retired = - terminal_result_order.front(); - // Insert into the exact ledger before dropping the - // cached result. Allocation failure must retain the - // old result rather than create a replay window. - retired_action_ids.emplace(retired); - terminal_results.erase(retired); - terminal_result_order.pop_front(); - } - } catch (const std::exception& error) { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] could not compact terminal " - "idempotency state; retaining existing state: " - << error.what(); - } catch (...) { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] could not compact terminal " - "idempotency state; retaining existing state"; - } - idle_condition.notify_all(); - } - } - } - - void complete( - const std::shared_ptr& record, - const api::ActionResultCode result, - const std::uint32_t completed_steps, - const std::string& message, - const int failed_step_index = -1) - { - { - std::lock_guard lock(record->mutex); - fillTerminalFeedback( - record->feedback, - record->request.action_id(), - result, - completed_steps, - message, - failed_step_index); - record->done = true; - } - record->condition.notify_all(); - } - - bool acquireLeases( - const std::shared_ptr& record, - LeaseSet& leases, - std::string& error) - { - auto& authority = - control::ControlAuthorityManager::instance(); - const std::string owner = - "grpc-system:action:" + record->request.action_id() + ":" + - std::to_string( - sequence.fetch_add(1U, std::memory_order_relaxed) + 1U); - const auto ttl = std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>(kControlLeaseTtl); - - while (Clock::now() < record->deadline) { - if (record->cancel_requested.load(std::memory_order_acquire)) { - error = "ActionQueue was canceled before device reservation"; - return false; - } - bool all_acquired = true; - std::string conflict; - for (const auto& resource_id : record->prepared.resource_ids) { - auto acquired = authority.tryAcquire( - resource_id, owner, ttl); - if (!acquired.acquired) { - all_acquired = false; - conflict = std::move(acquired.detail); - break; - } - leases.tokens.push_back(std::move(acquired.token)); - } - if (all_acquired) { - std::lock_guard record_lock(record->mutex); - record->control_tokens.clear(); - for (const auto& token : leases.tokens) { - record->control_tokens.emplace( - token.resource_id, token); - } - return true; - } - for (const auto& token : leases.tokens) { - authority.release(token); - } - leases.tokens.clear(); - error = conflict.empty() - ? "device control is unavailable" - : std::move(conflict); - - std::unique_lock record_lock(record->mutex); - const auto wake_at = std::min( - record->deadline, - Clock::now() + kLeaseRetryPeriod); - record->condition.wait_until( - record_lock, wake_at, [&record]() { - return record->cancel_requested.load( - std::memory_order_acquire); - }); - } - record->timed_out.store(true, std::memory_order_release); - error = "ActionQueue timed out while waiting for device control"; - return false; - } - - bool validateLeases(const LeaseSet& leases) const - { - auto& authority = - control::ControlAuthorityManager::instance(); - return std::all_of( - leases.tokens.begin(), leases.tokens.end(), - [&authority](const auto& token) { - return authority.validate(token); - }); - } - - bool validateDevicesIdle( - const std::shared_ptr& record, - std::string& error) const - { - std::unordered_set inspected_arms; - std::unordered_set inspected_agvs; - for (const auto& step : record->prepared.steps) { - if (step.arm && - inspected_arms.emplace(step.device_id).second && - step.arm->busy()) { - error = "RobotArm already has active motion before ActionQueue execution: " + - step.device_id; - return false; - } - if (!step.agv || - !inspected_agvs.emplace(step.device_id).second) { - continue; - } - const auto runtime = step.agv->runtimeState(); - const auto navigation = step.agv->navigationStatus(); - if (runtime.emergency_stopped || runtime.fault) { - error = "AGV is faulted or emergency-stopped before ActionQueue execution: " + - step.device_id; - return false; - } - if (runtime.moving || - navigation.state == device::AgvTaskState::Waiting || - navigation.state == device::AgvTaskState::Running || - navigation.state == device::AgvTaskState::Paused) { - error = "AGV already has active motion before ActionQueue execution: " + - step.device_id; - return false; - } - } - return true; - } - - Clock::time_point stepDeadline( - const std::shared_ptr& record, - const api::ActionStep& step) const - { - if (step.timeout_ms() == 0U) { - return record->deadline; - } - return std::min( - record->deadline, - Clock::now() + Milliseconds(step.timeout_ms())); - } - - void armTimeoutWatchdog( - const std::shared_ptr& record, - const PreparedStep& step, - const Clock::time_point deadline, - const std::shared_ptr>& disarmed, - const std::shared_ptr>& stop_started) - { - while (!disarmed->load(std::memory_order_acquire)) { - const auto now = Clock::now(); - if (now >= deadline) { - record->timed_out.store(true, std::memory_order_release); - requestTimedOutArmStop(record, step, stop_started); - record->condition.notify_all(); - return; - } - std::this_thread::sleep_for(std::min( - kArmWatchdogPollPeriod, - std::chrono::duration_cast(deadline - now))); - } - } - - void requestTimedOutArmStop( - const std::shared_ptr& record, - const PreparedStep& step, - const std::shared_ptr>& stop_started) - { - bool expected = false; - if (!stop_started->compare_exchange_strong( - expected, true, - std::memory_order_acq_rel, - std::memory_order_acquire)) { - return; - } - auto& authority = - control::ControlAuthorityManager::instance(); - const std::string owner = - "grpc-system:action-timeout:" + - record->request.action_id(); - const auto ttl = std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>( - std::chrono::minutes(1)); - std::optional expected_token; - { - std::lock_guard lock(record->mutex); - const auto found = record->control_tokens.find(step.device_id); - if (found != record->control_tokens.end()) { - expected_token = found->second; - } - } - if (!step.arm || !expected_token) { - record->retain_control_leases.store( - true, std::memory_order_release); - const std::string detail = - "could not establish timed-out RobotArm stop barrier: " - "the active Action lease is unavailable"; - { - std::lock_guard lock(record->mutex); - record->stop_error = detail; - } - CMVR_LOG(ERROR) << "[ActionQueueExecutor] " << detail - << ", id=" << step.device_id; - return; - } - - control::ControlAcquireResult barrier; - try { - barrier = authority.preemptAcquireIfCurrent( - *expected_token, owner, ttl); - } catch (...) { - (void)quarantineOrRetainLease(record, *expected_token); - throw; - } - if (!barrier.acquired) { - // A direct Stop or another Action stop may already have converted - // our lease. Do not join that barrier and, critically, do not - // preempt a successor which acquired control after it completed. - const auto fail_closed = - quarantineOrRetainLease(record, *expected_token); - if (fail_closed != FailClosedResult::AlreadyFenced) { - const std::string detail = - "could not establish timed-out RobotArm stop barrier " - "while the Action lease remained current: " + - barrier.detail; - { - std::lock_guard lock(record->mutex); - record->stop_error = detail; - } - CMVR_LOG(ERROR) << "[ActionQueueExecutor] " << detail - << ", id=" << step.device_id; - } - return; - } - bool stop_confirmed = false; - std::string stop_error; - try { - const auto stop = step.arm->stopMotion(); - stop_confirmed = stop.ok(); - if (!stop_confirmed) { - stop_error = stop.message.empty() - ? "RobotArm stop did not confirm idle" - : stop.message; - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] timed-out RobotArm stop failed, id=" - << step.device_id << ", error=" << stop.message; - } - } catch (const std::exception& error) { - stop_error = error.what(); - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] timed-out RobotArm stop threw, id=" - << step.device_id << ", error=" << error.what(); - } catch (...) { - stop_error = "RobotArm stop threw an unknown exception"; - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] timed-out RobotArm stop threw, id=" - << step.device_id; - } - if (!stop_confirmed) { - // A user Stop/StopAll may already own the driver termination state. - // In that case this second stop call is expected to be rejected. - // Keep our safety holder while waiting for the first owner to - // publish the driver's confirmed-idle state; only quarantine if - // that bounded confirmation also fails. - stop_confirmed = waitForArmIdle( - step.arm, - kConcurrentArmStopConfirmationTimeout); - } - if (stop_confirmed) { - authority.release(barrier.token); - } else { - // Deliberately retain the safety barrier when idle was not - // confirmed. Releasing it would allow a new command to overlap an - // unknown physical outcome. Retiring the token keeps the barrier - // fail-closed while allowing a later confirmed System StopAll - // recovery round to clear it. - (void)authority.retireSafetyHolder(barrier.token); - std::lock_guard lock(record->mutex); - record->stop_error = - "RobotArm stop was not confirmed; control remains quarantined: " + - stop_error; - } - } - - enum class StepOutcome { - Completed, - Rejected, - Failed, - Canceled, - TimedOut, - }; - - struct StepResult { - StepOutcome outcome{StepOutcome::Failed}; - std::string message; - }; - - safety::AdmissionResult admitStep( - const std::shared_ptr& record, - const PreparedStep& prepared, - const api::ActionStep& source, - const control::ControlLeaseToken& token, - const Clock::time_point deadline) - { - safety::AdmissionRequest request; - request.command = safetyDescriptorFor(prepared.kind); - request.actor = record->actor; - request.command_id = actionStepCommandId( - record->request.action_id(), source.step_id()); - request.device_id = prepared.device_id; - if (const auto* header = commandHeaderFor(source); - header && header->has_expected_device_generation()) { - request.expected_device_generation = - header->expected_device_generation(); - } - request.authority_generation = token.generation; - request.deadline = deadline; - return device_manager.safetyManager().admit(request); - } - - static std::string admissionFailure( - const safety::AdmissionDecision& decision) - { - return decision.detail.empty() - ? std::string("ActionQueue step safety admission was rejected: ") + - safety::toString(decision.reason) - : "ActionQueue step safety admission was rejected: " + - decision.detail; - } - - static std::string dispatchFailure( - const safety::HardwareCheckResult& result) - { - return result.detail.empty() - ? std::string("ActionQueue step final safety check failed: ") + - safety::toString(result.reason) - : "ActionQueue step final safety check failed: " + - result.detail; - } - - StepResult executeArmStep( - const std::shared_ptr& record, - const PreparedStep& prepared, - const api::ActionStep& source, - const control::ControlLeaseToken& token, - const Clock::time_point deadline) - { - auto& authority = - control::ControlAuthorityManager::instance(); - auto safety_admission = admitStep( - record, prepared, source, token, deadline); - if (!safety_admission.permit) { - return { - StepOutcome::Rejected, - admissionFailure(safety_admission.decision)}; - } - auto authority_dispatch = authority.tryBeginDispatch(token); - if (!authority_dispatch.acquired()) { - return { - StepOutcome::Canceled, - "RobotArm ActionQueue control was preempted before dispatch"}; - } - auto safety_dispatch = device_manager.safetyManager() - .beginDispatch(*safety_admission.permit); - if (!safety_dispatch.acquired()) { - return { - StepOutcome::Rejected, - dispatchFailure(safety_dispatch.hardwareCheck())}; - } - auto cancellation_requested = - [record, token, deadline, &authority]() { - return record->cancel_requested.load( - std::memory_order_acquire) || - Clock::now() >= deadline || - !authority.validate(token); - }; - - auto disarmed = std::make_shared>(false); - auto timeout_stop_started = - std::make_shared>(false); - std::thread watchdog( - [this, record, prepared, deadline, disarmed, - timeout_stop_started]() { - try { - armTimeoutWatchdog( - record, prepared, deadline, disarmed, - timeout_stop_started); - } catch (const std::exception& error) { - try { - std::lock_guard lock(record->mutex); - record->stop_error = - "RobotArm timeout watchdog failed: " + - std::string(error.what()); - } catch (...) { - } - try { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] RobotArm timeout " - "watchdog failed, id=" - << prepared.device_id - << ", error=" << error.what(); - } catch (...) { - } - } catch (...) { - try { - std::lock_guard lock(record->mutex); - record->stop_error = - "RobotArm timeout watchdog failed with an unknown exception"; - } catch (...) { - } - try { - CMVR_LOG(ERROR) - << "[ActionQueueExecutor] RobotArm timeout " - "watchdog failed, id=" - << prepared.device_id; - } catch (...) { - } - } - }); - - device::Result motion_result; - try { - if (prepared.kind == PreparedStepKind::ArmMoveJ) { - const auto& command = source.arm_move_j(); - device::JointPositionCommand target; - target.position.assign( - command.target().position().begin(), - command.target().position().end()); - auto options = toArmMotionOptions(command.options()); - options.cancellation_requested = cancellation_requested; - motion_result = prepared.arm->moveJ(target, options); - } else { - const auto& command = source.arm_move_l(); - const device::CartesianPose target{ - command.target().x(), command.target().y(), - command.target().z(), command.target().rx(), - command.target().ry(), command.target().rz()}; - auto options = toArmMotionOptions(command.options()); - options.cancellation_requested = cancellation_requested; - motion_result = prepared.arm->moveL( - target, options, toFrameType(command.frame())); - } - } catch (const std::exception& error) { - motion_result = device::Result::failure( - device::ArmErrorCode::CommandFailed, error.what()); - } catch (...) { - motion_result = device::Result::failure( - device::ArmErrorCode::CommandFailed, - "RobotArm command threw an unknown exception"); - } - - const bool expired = - record->timed_out.load(std::memory_order_acquire) || - Clock::now() >= deadline; - if (expired) { - // The device callback can observe the deadline and return before - // the watchdog gets scheduled. Preserve the invariant that every - // timed-out synchronous arm command goes through a typed Stop. - record->timed_out.store(true, std::memory_order_release); - requestTimedOutArmStop( - record, prepared, timeout_stop_started); - } - disarmed->store(true, std::memory_order_release); - if (watchdog.joinable()) { - watchdog.join(); - } - if (expired || - record->timed_out.load(std::memory_order_acquire)) { - std::string message = "RobotArm ActionQueue step timed out"; - { - std::lock_guard lock(record->mutex); - if (!record->stop_error.empty()) { - message += "; " + record->stop_error; - } - } - return {StepOutcome::TimedOut, - std::move(message)}; - } - if (record->cancel_requested.load(std::memory_order_acquire) || - !authority.validate(token)) { - std::string message = - "RobotArm ActionQueue step was canceled or preempted"; - if (!requestTypedStop(record)) { - message += - "; one or more Action devices did not confirm a stopped " - "state and remain quarantined"; - } - return {StepOutcome::Canceled, std::move(message)}; - } - if (!motion_result.ok()) { - std::string message = motion_result.message; - if (!requestTypedStop(record)) { - message += - "; one or more Action devices did not confirm a stopped " - "state and remain quarantined"; - } - return {StepOutcome::Failed, std::move(message)}; - } - return {StepOutcome::Completed, {}}; - } - - StepResult executeAgvStep( - const std::shared_ptr& record, - const PreparedStep& prepared, - const api::ActionStep& source, - const control::ControlLeaseToken& token, - const Clock::time_point deadline) - { - auto& authority = - control::ControlAuthorityManager::instance(); - auto safety_admission = admitStep( - record, prepared, source, token, deadline); - if (!safety_admission.permit) { - return { - StepOutcome::Rejected, - admissionFailure(safety_admission.decision)}; - } - auto authority_dispatch = authority.tryBeginDispatch(token); - if (!authority_dispatch.acquired()) { - return { - StepOutcome::Canceled, - "AGV ActionQueue control was preempted before dispatch"}; - } - auto safety_dispatch = device_manager.safetyManager() - .beginDispatch(*safety_admission.permit); - if (!safety_dispatch.acquired()) { - return { - StepOutcome::Rejected, - dispatchFailure(safety_dispatch.hardwareCheck())}; - } - auto cancellation_requested = - [record, token, deadline, &authority]() { - return record->cancel_requested.load( - std::memory_order_acquire) || - Clock::now() >= deadline || - !authority.validate(token); - }; - - device::AgvResult navigation_result; - try { - if (prepared.kind == PreparedStepKind::AgvNavigateToPose) { - const auto& command = source.agv_navigate_to_pose(); - auto options = toAgvMotionOptions(command.options()); - applyAgvDeadline(options, deadline); - options.cancellation_requested = cancellation_requested; - navigation_result = prepared.agv->navigateToPose( - {command.pose().x(), command.pose().y(), - command.pose().theta()}, - options, - toAgvAdapterParams(command.adapter_params())); - } else if (prepared.kind == - PreparedStepKind::AgvNavigateToStation) { - const auto& command = source.agv_navigate_to_station(); - auto options = toAgvMotionOptions(command.options()); - applyAgvDeadline(options, deadline); - options.cancellation_requested = cancellation_requested; - navigation_result = prepared.agv->navigateToStation( - command.station_id(), - options, - toAgvAdapterParams(command.adapter_params())); - } else { - const auto& command = source.agv_follow_path(); - std::vector path; - path.reserve(static_cast(command.path_size())); - for (const auto& segment : command.path()) { - path.push_back({ - segment.source_station(), - segment.target_station()}); - } - auto options = toAgvMotionOptions(command.options()); - applyAgvDeadline(options, deadline); - options.cancellation_requested = cancellation_requested; - navigation_result = prepared.agv->followPath(path, options); - } - } catch (const std::exception& error) { - navigation_result = device::AgvResult::failure( - device::AgvErrorCode::CommandFailed, error.what()); - } catch (...) { - navigation_result = device::AgvResult::failure( - device::AgvErrorCode::CommandFailed, - "AGV command threw an unknown exception"); - } - - const auto stopOrQuarantine = - [this, &record](std::string message) { - if (!requestTypedStop(record)) { - message += - "; one or more Action devices did not confirm a " - "stopped state and remain quarantined"; - } - return message; - }; - - if (Clock::now() >= deadline || - navigation_result.code == device::AgvErrorCode::Timeout) { - record->timed_out.store(true, std::memory_order_release); - return {StepOutcome::TimedOut, - stopOrQuarantine( - navigation_result.message.empty() - ? "AGV ActionQueue step timed out" - : navigation_result.message)}; - } - if (record->cancel_requested.load(std::memory_order_acquire) || - !authority.validate(token) || - navigation_result.code == device::AgvErrorCode::TaskCanceled) { - return {StepOutcome::Canceled, - stopOrQuarantine( - navigation_result.message.empty() - ? "AGV ActionQueue step was canceled or preempted" - : navigation_result.message)}; - } - if (!navigation_result.ok()) { - return {StepOutcome::Failed, - stopOrQuarantine(navigation_result.message)}; - } - return {StepOutcome::Completed, {}}; - } - - static void applyAgvDeadline( - device::AgvMotionOptions& options, - const Clock::time_point deadline) - { - const auto remaining = std::max( - 1, - std::chrono::duration_cast( - deadline - Clock::now()).count()); - const int bounded = static_cast(std::min( - remaining, - std::numeric_limits::max())); - if (options.wait_timeout_ms <= 0 || - options.wait_timeout_ms > bounded) { - options.wait_timeout_ms = bounded; - } - if (options.poll_interval_ms > options.wait_timeout_ms) { - options.poll_interval_ms = options.wait_timeout_ms; - } - } - - StepResult executeDelay( - const std::shared_ptr& record, - const api::ActionStep& step, - const Clock::time_point deadline) - { - const auto delay_deadline = std::min( - deadline, - Clock::now() + Milliseconds(step.delay().duration_ms())); - std::unique_lock lock(record->mutex); - const bool canceled = record->condition.wait_until( - lock, delay_deadline, [&record]() { - return record->cancel_requested.load( - std::memory_order_acquire); - }); - if (canceled) { - return {StepOutcome::Canceled, - "ActionQueue delay was canceled"}; - } - if (Clock::now() >= deadline) { - record->timed_out.store(true, std::memory_order_release); - return {StepOutcome::TimedOut, - "ActionQueue delay timed out"}; - } - return {StepOutcome::Completed, {}}; - } - - void execute(const std::shared_ptr& record) - { - if (record->cancel_requested.load(std::memory_order_acquire)) { - complete(record, api::ACTION_RESULT_CODE_CANCELED, 0, - "ActionQueue was canceled before execution"); - return; - } - if (Clock::now() >= record->deadline) { - complete(record, api::ACTION_RESULT_CODE_TIMED_OUT, 0, - "ActionQueue expired while waiting in the queue"); - return; - } - - LeaseSet leases(record); - std::string lease_error; - if (!acquireLeases(record, leases, lease_error)) { - if (record->timed_out.load(std::memory_order_acquire)) { - complete(record, api::ACTION_RESULT_CODE_TIMED_OUT, 0, - lease_error); - } else { - complete(record, api::ACTION_RESULT_CODE_CANCELED, 0, - lease_error); - } - return; - } - if (!validateLeases(leases)) { - complete(record, api::ACTION_RESULT_CODE_CANCELED, 0, - "ActionQueue device control was preempted before execution"); - return; - } - - std::string dynamic_error; - if (!validateDevicesIdle(record, dynamic_error)) { - complete(record, api::ACTION_RESULT_CODE_REJECTED, 0, - dynamic_error); - return; - } - - std::uint32_t completed_steps = 0; - for (int index = 0; index < record->request.steps_size(); ++index) { - if (record->cancel_requested.load(std::memory_order_acquire) || - !validateLeases(leases)) { - complete( - record, api::ACTION_RESULT_CODE_CANCELED, - completed_steps, - "ActionQueue was canceled or device control was preempted"); - return; - } - if (Clock::now() >= record->deadline) { - complete( - record, api::ACTION_RESULT_CODE_TIMED_OUT, - completed_steps, - "ActionQueue total timeout elapsed"); - return; - } - - const auto& source = record->request.steps(index); - const auto& prepared = record->prepared.steps[ - static_cast(index)]; - const auto deadline = stepDeadline(record, source); - StepResult step_result; - if (prepared.kind == PreparedStepKind::Delay) { - record->active_step_index.store( - -1, std::memory_order_release); - step_result = executeDelay(record, source, deadline); - } else { - const auto* token = leases.find(prepared.device_id); - if (!token) { - complete( - record, api::ACTION_RESULT_CODE_FAILED, - completed_steps, - "ActionQueue internal device reservation is missing", - index); - return; - } - if (prepared.arm) { - record->active_step_index.store( - index, std::memory_order_release); - step_result = executeArmStep( - record, prepared, source, *token, deadline); - } else { - if (!prepared.agv->supportsSynchronousAction( - toAgvActionKind(prepared.kind))) { - complete( - record, api::ACTION_RESULT_CODE_REJECTED, - completed_steps, - "AGV ActionQueue capability changed before execution", - index); - return; - } - record->active_step_index.store( - index, std::memory_order_release); - step_result = executeAgvStep( - record, prepared, source, *token, deadline); - } - record->active_step_index.store( - -1, std::memory_order_release); - } - - switch (step_result.outcome) { - case StepOutcome::Completed: - ++completed_steps; - break; - case StepOutcome::Rejected: - complete( - record, api::ACTION_RESULT_CODE_REJECTED, - completed_steps, step_result.message, index); - return; - case StepOutcome::Failed: - complete( - record, api::ACTION_RESULT_CODE_FAILED, - completed_steps, step_result.message, index); - return; - case StepOutcome::Canceled: - complete( - record, api::ACTION_RESULT_CODE_CANCELED, - completed_steps, step_result.message, index); - return; - case StepOutcome::TimedOut: - complete( - record, api::ACTION_RESULT_CODE_TIMED_OUT, - completed_steps, step_result.message, index); - return; - } - } - - complete(record, api::ACTION_RESULT_CODE_COMPLETED, - completed_steps, {}); - CMVR_LOG(DEBUG) - << "[ActionQueueExecutor] action completed, id=" - << record->request.action_id() - << ", steps=" << completed_steps; - } - - device::DeviceManager& device_manager; - const std::string instance_id; - const std::size_t max_accepted_action_ids; - std::mutex mutex; - std::condition_variable queue_condition; - std::condition_variable idle_condition; - std::deque> queue; - std::unordered_map> records; - std::unordered_map terminal_results; - std::deque terminal_result_order; - std::unordered_set retired_action_ids; - std::shared_ptr active; - RunState run_state{RunState::Accepting}; - std::uint64_t admission_generation{1U}; - std::uint64_t stop_all_generation{0U}; - std::uint64_t next_stop_all_ticket_id{0U}; - std::unordered_set outstanding_stop_all_tickets; - bool stop_all_failed{false}; - bool joined{false}; - std::atomic sequence{0}; - std::atomic concurrent_submitters{0}; - std::thread worker; -}; - -ActionQueueExecutor::ActionQueueExecutor( - device::DeviceManager& device_manager, - const std::size_t max_accepted_action_ids) - : impl_(std::make_unique( - device_manager, max_accepted_action_ids)) -{ -} - -ActionQueueExecutor::~ActionQueueExecutor() = default; - -ActionQueueExecutor::WaitResult ActionQueueExecutor::submitAndWait( - const api::ActionQueueCommand_Request& request, - api::ActionQueueCommand_Feedback& feedback, - const std::function& waiter_canceled, - safety::CommandActor actor) -{ - const auto result = impl_->submitAndWait( - request, feedback, waiter_canceled, std::move(actor)); - feedback.set_service_instance_id(impl_->instance_id); - return result; -} - -ActionQueueExecutor::StopAllTicket ActionQueueExecutor::beginStopAll( - const bool delegate_active_stop) -{ - return impl_->beginStopAll(delegate_active_stop); -} - -bool ActionQueueExecutor::finishStopAll( - const StopAllTicket& ticket, - const bool all_devices_stop_confirmed) -{ - return impl_->finishStopAll( - ticket, all_devices_stop_confirmed); -} - -bool ActionQueueExecutor::disableForShutdown() -{ - return impl_->disableForShutdown(); -} - -bool ActionQueueExecutor::waitForIdle( - const std::chrono::milliseconds timeout) -{ - return impl_->waitForIdle(timeout); -} - -const std::string& ActionQueueExecutor::instanceId() const noexcept -{ - return impl_->instance_id; -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/client/CMakeLists.txt b/cmvr-es/service/grpc/client/CMakeLists.txt deleted file mode 100644 index 8dd1b377..00000000 --- a/cmvr-es/service/grpc/client/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -find_package(Threads REQUIRED) - -add_library(arm_teleop_client STATIC - src/grpc_arm_teleop_client.cpp -) -target_compile_features(arm_teleop_client PUBLIC cxx_std_17) -target_include_directories(arm_teleop_client PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-es) -target_link_libraries(arm_teleop_client - PUBLIC - cmvr_es::proto - PRIVATE - Threads::Threads -) - -add_library(cmvr_es::arm_teleop_client ALIAS arm_teleop_client) -install(TARGETS arm_teleop_client ARCHIVE DESTINATION lib) - -if(BUILD_TESTING) - add_executable(grpc_arm_teleop_client_test - tests/grpc_arm_teleop_client_test.cpp - ) - target_compile_features(grpc_arm_teleop_client_test PRIVATE cxx_std_17) - target_link_libraries(grpc_arm_teleop_client_test - PRIVATE - cmvr_es::arm_teleop_client - Threads::Threads - ) - add_test(NAME grpc_arm_teleop_client_test COMMAND grpc_arm_teleop_client_test) - set(_arm_teleop_client_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _arm_teleop_client_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_arm_teleop_client_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_arm_teleop_client_test_environment}") -endif() diff --git a/cmvr-es/service/grpc/client/include/grpc_arm_teleop_client.h b/cmvr-es/service/grpc/client/include/grpc_arm_teleop_client.h deleted file mode 100644 index 731a718d..00000000 --- a/cmvr-es/service/grpc/client/include/grpc_arm_teleop_client.h +++ /dev/null @@ -1,80 +0,0 @@ -#ifndef CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H -#define CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "cmvr/api/arm_teleop_v1.grpc.pb.h" - -namespace cmvr::teleop { - -// One synchronous gRPC stream/session. Connection retry and worker ownership -// belong to UmeTeleopTask; robot algorithms and kinematics belong to UME. -class GrpcArmTeleopClient final { -public: - using Api = api::armteleop::v1::ArmTeleopService; - using ClientFrame = api::armteleop::v1::ClientFrame; - using ServerFrame = api::armteleop::v1::ServerFrame; - using OpenSession = api::armteleop::v1::OpenSession; - using JointSetpoint = api::armteleop::v1::JointSetpoint; - using ClientHeartbeat = api::armteleop::v1::ClientHeartbeat; - using StopSession = api::armteleop::v1::StopSession; - using FrameCallback = std::function; - using CancelPredicate = std::function; - - explicit GrpcArmTeleopClient( - std::shared_ptr channel); - ~GrpcArmTeleopClient(); - - GrpcArmTeleopClient(const GrpcArmTeleopClient&) = delete; - GrpcArmTeleopClient& operator=(const GrpcArmTeleopClient&) = delete; - - // Blocks until the peer closes the stream or tryCancel() is called. The - // OpenSession frame is always the first client frame. - grpc::Status runSession(const OpenSession& open_session, - FrameCallback callback = {}, - CancelPredicate cancel_requested = {}); - - // These methods only transport already-computed protocol values. - bool sendSetpoint(const JointSetpoint& setpoint, - std::uint64_t expected_session_generation = 0); - bool sendHeartbeat(const ClientHeartbeat& heartbeat, - std::uint64_t expected_session_generation = 0); - bool sendStop(const StopSession& stop, - std::uint64_t expected_session_generation = 0); - - bool isSessionActive() const; - std::uint64_t activeSessionGeneration() const; - - // Thread-safe and intentionally named after the gRPC primitive used. It - // interrupts a blocked Read/Write/Finish so the owning Task can join. - void tryCancel(); - -private: - using Stream = grpc::ClientReaderWriterInterface; - - bool writeFrame(const ClientFrame& frame, - std::uint64_t expected_session_generation); - void clearSession(const std::shared_ptr& context, - const std::shared_ptr& stream); - - std::unique_ptr stub_; - - mutable std::mutex lifecycle_mutex_; - std::mutex write_mutex_; - std::shared_ptr active_context_; - std::shared_ptr active_stream_; - std::uint64_t next_session_generation_{0}; - std::uint64_t active_session_generation_{0}; -}; - -} // namespace cmvr::teleop - -#endif // CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H diff --git a/cmvr-es/service/grpc/client/src/grpc_arm_teleop_client.cpp b/cmvr-es/service/grpc/client/src/grpc_arm_teleop_client.cpp deleted file mode 100644 index 5f12d043..00000000 --- a/cmvr-es/service/grpc/client/src/grpc_arm_teleop_client.cpp +++ /dev/null @@ -1,223 +0,0 @@ -#include "service/grpc/client/include/grpc_arm_teleop_client.h" - -#include -#include -#include - -namespace cmvr::teleop { -namespace { - -grpc::Status clientStatus(const grpc::StatusCode code, const char* detail) -{ - return grpc::Status(code, detail); -} - -} // namespace - -GrpcArmTeleopClient::GrpcArmTeleopClient( - std::shared_ptr channel) -{ - if (channel) { - stub_ = Api::NewStub(channel); - } -} - -GrpcArmTeleopClient::~GrpcArmTeleopClient() -{ - tryCancel(); -} - -grpc::Status GrpcArmTeleopClient::runSession( - const OpenSession& open_session, - FrameCallback callback, - CancelPredicate cancel_requested) -{ - if (!stub_) { - return clientStatus( - grpc::StatusCode::FAILED_PRECONDITION, - "arm teleop client has no channel"); - } - if (cancel_requested && cancel_requested()) { - return clientStatus( - grpc::StatusCode::CANCELLED, - "arm teleop session cancelled before start"); - } - - auto context = std::make_shared(); - { - std::lock_guard lock(lifecycle_mutex_); - if (active_context_) { - return clientStatus( - grpc::StatusCode::ALREADY_EXISTS, - "arm teleop session is already active"); - } - // Publish the context before opening/writing the stream so tryCancel() - // can interrupt every blocking phase of the synchronous RPC. - active_context_ = context; - } - // Closes the small race where the owning Task requests stop immediately - // before active_context_ becomes visible to tryCancel(). - if (cancel_requested && cancel_requested()) { - context->TryCancel(); - } - - auto unique_stream = stub_->Teleoperate(context.get()); - if (!unique_stream) { - clearSession(context, {}); - return clientStatus( - grpc::StatusCode::UNAVAILABLE, - "failed to create arm teleop stream"); - } - auto stream = std::shared_ptr(std::move(unique_stream)); - - ClientFrame first_frame; - *first_frame.mutable_open() = open_session; - { - std::lock_guard write_lock(write_mutex_); - if (!stream->Write(first_frame)) { - const grpc::Status status = stream->Finish(); - clearSession(context, stream); - return status.ok() - ? clientStatus( - grpc::StatusCode::UNAVAILABLE, - "peer closed before OpenSession was written") - : status; - } - } - - { - std::lock_guard lock(lifecycle_mutex_); - // Cancellation can race the initial Write. Keeping the stream visible - // is safe; subsequent writes will fail and runSession will clean it. - if (active_context_ == context) { - active_stream_ = stream; - active_session_generation_ = ++next_session_generation_; - } - } - - bool callback_failed = false; - std::string callback_error; - ServerFrame frame; - while (stream->Read(&frame)) { - if (!callback) { - continue; - } - try { - callback(frame); - } catch (const std::exception& error) { - callback_failed = true; - callback_error = error.what(); - context->TryCancel(); - break; - } catch (...) { - callback_failed = true; - callback_error = "server-frame callback raised an unknown exception"; - context->TryCancel(); - break; - } - } - - { - std::lock_guard write_lock(write_mutex_); - stream->WritesDone(); - } - const grpc::Status status = stream->Finish(); - clearSession(context, stream); - - if (callback_failed) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - "arm teleop callback failed: " + callback_error); - } - return status; -} - -bool GrpcArmTeleopClient::sendSetpoint( - const JointSetpoint& setpoint, - const std::uint64_t expected_session_generation) -{ - ClientFrame frame; - *frame.mutable_setpoint() = setpoint; - return writeFrame(frame, expected_session_generation); -} - -bool GrpcArmTeleopClient::sendHeartbeat( - const ClientHeartbeat& heartbeat, - const std::uint64_t expected_session_generation) -{ - ClientFrame frame; - *frame.mutable_heartbeat() = heartbeat; - return writeFrame(frame, expected_session_generation); -} - -bool GrpcArmTeleopClient::sendStop( - const StopSession& stop, - const std::uint64_t expected_session_generation) -{ - ClientFrame frame; - *frame.mutable_stop() = stop; - return writeFrame(frame, expected_session_generation); -} - -bool GrpcArmTeleopClient::isSessionActive() const -{ - std::lock_guard lock(lifecycle_mutex_); - return active_stream_ != nullptr; -} - -std::uint64_t GrpcArmTeleopClient::activeSessionGeneration() const -{ - std::lock_guard lock(lifecycle_mutex_); - return active_session_generation_; -} - -void GrpcArmTeleopClient::tryCancel() -{ - std::shared_ptr context; - { - std::lock_guard lock(lifecycle_mutex_); - context = active_context_; - } - if (context) { - context->TryCancel(); - } -} - -bool GrpcArmTeleopClient::writeFrame( - const ClientFrame& frame, - const std::uint64_t expected_session_generation) -{ - std::shared_ptr stream; - { - std::lock_guard lock(lifecycle_mutex_); - if (expected_session_generation != 0 && - expected_session_generation != active_session_generation_) { - return false; - } - stream = active_stream_; - } - if (!stream) { - return false; - } - - // gRPC permits one read and one write concurrently, but concurrent writes - // must be serialized by the application. - std::lock_guard write_lock(write_mutex_); - return stream->Write(frame); -} - -void GrpcArmTeleopClient::clearSession( - const std::shared_ptr& context, - const std::shared_ptr& stream) -{ - std::lock_guard lock(lifecycle_mutex_); - if (active_context_ == context) { - active_context_.reset(); - } - if (!stream || active_stream_ == stream) { - active_stream_.reset(); - active_session_generation_ = 0; - } -} - -} // namespace cmvr::teleop diff --git a/cmvr-es/service/grpc/client/tests/grpc_arm_teleop_client_test.cpp b/cmvr-es/service/grpc/client/tests/grpc_arm_teleop_client_test.cpp deleted file mode 100644 index 3e1caa7c..00000000 --- a/cmvr-es/service/grpc/client/tests/grpc_arm_teleop_client_test.cpp +++ /dev/null @@ -1,220 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "cmvr/api/arm_teleop_v1.grpc.pb.h" -#include "service/grpc/client/include/grpc_arm_teleop_client.h" - -namespace { - -using namespace std::chrono_literals; -namespace api = cmvr::api::armteleop::v1; - -class TestArmTeleopService final : public api::ArmTeleopService::Service { -public: - grpc::Status Teleoperate( - grpc::ServerContext*, - grpc::ServerReaderWriter* stream) override - { - api::ClientFrame frame; - if (!stream->Read(&frame) || !frame.has_open()) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "OpenSession must be first"); - } - - { - std::lock_guard lock(mutex_); - open_received_ = true; - } - condition_.notify_all(); - - api::ServerFrame opened; - opened.mutable_status()->set_session_id("client-test-session"); - opened.mutable_status()->set_phase(api::SESSION_PHASE_OPENED); - if (!stream->Write(opened)) { - return grpc::Status::OK; - } - - while (stream->Read(&frame)) { - if (frame.has_heartbeat()) { - heartbeat_received_.store(true); - condition_.notify_all(); - } - } - handler_finished_.store(true); - condition_.notify_all(); - return grpc::Status::OK; - } - - bool waitForOpen(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for(lock, timeout, [this] { return open_received_; }); - } - - bool waitForHeartbeat(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [this] { return heartbeat_received_.load(); }); - } - - bool waitForHandlerFinish(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [this] { return handler_finished_.load(); }); - } - -private: - std::mutex mutex_; - std::condition_variable condition_; - bool open_received_{false}; - std::atomic heartbeat_received_{false}; - std::atomic handler_finished_{false}; -}; - -int fail(const std::string& detail) -{ - std::cerr << "grpc_arm_teleop_client_test: " << detail << '\n'; - return 1; -} - -} // namespace - -int main() -{ - TestArmTeleopService service; - const std::string socket_path = - "/tmp/cmvr_arm_teleop_client_test_" + - std::to_string(static_cast(::getpid())) + ".sock"; - std::remove(socket_path.c_str()); - const std::string endpoint = "unix:" + socket_path; - - grpc::ServerBuilder builder; - builder.AddListeningPort( - endpoint, - grpc::InsecureServerCredentials()); - builder.RegisterService(&service); - std::unique_ptr server = builder.BuildAndStart(); - if (!server) { - return fail("failed to start in-process gRPC server"); - } - - auto channel = grpc::CreateChannel( - endpoint, - grpc::InsecureChannelCredentials()); - cmvr::teleop::GrpcArmTeleopClient client(channel); - - api::OpenSession open; - open.set_protocol_major(1); - open.set_protocol_minor(0); - open.set_client_instance_id("grpc-client-test"); - open.mutable_expected_robot()->set_robot_id("test-arm"); - open.set_watchdog_timeout_ms(100); - open.set_requested_lease_ms(500); - - std::mutex frame_mutex; - std::condition_variable frame_condition; - bool opened_received = false; - grpc::Status session_status; - std::thread session_thread([&] { - session_status = client.runSession( - open, - [&](const api::ServerFrame& frame) { - if (frame.has_status() && - frame.status().phase() == api::SESSION_PHASE_OPENED) { - { - std::lock_guard lock(frame_mutex); - opened_received = true; - } - frame_condition.notify_all(); - } - }); - }); - - if (!service.waitForOpen(2s)) { - client.tryCancel(); - session_thread.join(); - server->Shutdown(); - return fail("server did not receive OpenSession"); - } - { - std::unique_lock lock(frame_mutex); - if (!frame_condition.wait_for(lock, 2s, [&] { return opened_received; })) { - client.tryCancel(); - session_thread.join(); - server->Shutdown(); - return fail("client did not receive OPENED status"); - } - } - if (!client.isSessionActive()) { - client.tryCancel(); - session_thread.join(); - server->Shutdown(); - return fail("client did not expose an active session"); - } - const std::uint64_t generation = - client.activeSessionGeneration(); - if (generation == 0) { - client.tryCancel(); - session_thread.join(); - server->Shutdown(); - return fail("active stream did not expose a session generation"); - } - - api::ClientHeartbeat heartbeat; - heartbeat.set_sequence(1); - if (client.sendHeartbeat(heartbeat, generation + 1)) { - client.tryCancel(); - session_thread.join(); - server->Shutdown(); - return fail("stale session generation was allowed to write"); - } - if (!client.sendHeartbeat(heartbeat, generation) || - !service.waitForHeartbeat(2s)) { - client.tryCancel(); - session_thread.join(); - server->Shutdown(); - return fail("heartbeat did not traverse the active stream"); - } - - const auto cancel_begin = std::chrono::steady_clock::now(); - client.tryCancel(); - session_thread.join(); - const auto cancel_elapsed = std::chrono::steady_clock::now() - cancel_begin; - - if (cancel_elapsed > 2s) { - server->Shutdown(); - return fail("TryCancel did not unblock and join the session promptly"); - } - if (session_status.error_code() != grpc::StatusCode::CANCELLED) { - server->Shutdown(); - return fail( - "cancelled session returned unexpected status: " + - std::to_string(session_status.error_code())); - } - if (client.isSessionActive()) { - server->Shutdown(); - return fail("client retained an active stream after cancellation"); - } - if (!service.waitForHandlerFinish(2s)) { - server->Shutdown(); - return fail("server handler did not observe client cancellation"); - } - - server->Shutdown(); - std::remove(socket_path.c_str()); - std::cout << "grpc_arm_teleop_client_test: PASS\n"; - return 0; -} diff --git a/cmvr-es/service/grpc/server/include/grpc_agv_service.h b/cmvr-es/service/grpc/include/grpc_agv_service.h similarity index 92% rename from cmvr-es/service/grpc/server/include/grpc_agv_service.h rename to cmvr-es/service/grpc/include/grpc_agv_service.h index d7975948..6ec4e65d 100644 --- a/cmvr-es/service/grpc/server/include/grpc_agv_service.h +++ b/cmvr-es/service/grpc/include/grpc_agv_service.h @@ -1,21 +1,15 @@ #ifndef CMVR_ES_GRPC_AGV_SERVICE_H #define CMVR_ES_GRPC_AGV_SERVICE_H -#include - #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 GrpcSecurityGateway; - class gRPCAgvServiceImpl final : public api::AgvService::Service { public: gRPCAgvServiceImpl(); - explicit gRPCAgvServiceImpl( - std::shared_ptr security_gateway); ~gRPCAgvServiceImpl() override = default; grpc::Status getRuntimeState(grpc::ServerContext* context, @@ -78,14 +72,9 @@ public: grpc::Status stopMapping(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) override; - grpc::Status translate( - grpc::ServerContext* context, - const api::AgvTranslateCommand_Request* request, - api::AgvTranslateCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; - std::shared_ptr security_gateway_; }; } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_arm_service.h b/cmvr-es/service/grpc/include/grpc_arm_service.h similarity index 88% rename from cmvr-es/service/grpc/server/include/grpc_arm_service.h rename to cmvr-es/service/grpc/include/grpc_arm_service.h index 1af87826..3133cc0c 100644 --- a/cmvr-es/service/grpc/server/include/grpc_arm_service.h +++ b/cmvr-es/service/grpc/include/grpc_arm_service.h @@ -1,20 +1,14 @@ #pragma once -#include - #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 GrpcSecurityGateway; - class gRPCArmServiceImpl final : public api::ArmService::Service { public: gRPCArmServiceImpl(); - explicit gRPCArmServiceImpl( - std::shared_ptr security_gateway); ~gRPCArmServiceImpl() override = default; grpc::Status torqueOff(grpc::ServerContext* context, @@ -59,16 +53,12 @@ public: grpc::Status computeForwardKinematics(grpc::ServerContext* context, const api::ComputeForwardKinematics_Request* request, api::ComputeForwardKinematics_Response* response) override; - grpc::Status ExecuteJsonCommand(grpc::ServerContext* context, - const api::JsonDeviceCommand_Request* request, - api::JsonDeviceCommand_Feedback* response) override; grpc::Status clearFault(grpc::ServerContext *context, const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override; private: device::DeviceManager& dmgr_; - std::shared_ptr security_gateway_; }; } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_camera_service.h b/cmvr-es/service/grpc/include/grpc_camera_service.h similarity index 90% rename from cmvr-es/service/grpc/server/include/grpc_camera_service.h rename to cmvr-es/service/grpc/include/grpc_camera_service.h index 799647e0..62e72277 100644 --- a/cmvr-es/service/grpc/server/include/grpc_camera_service.h +++ b/cmvr-es/service/grpc/include/grpc_camera_service.h @@ -5,23 +5,18 @@ #ifndef GRPC_CAMERA_SERVICE_H #define GRPC_CAMERA_SERVICE_H -#include - #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/server/include/grpc_camera_stream_policy.h" +#include "service/grpc/include/grpc_camera_stream_policy.h" namespace cmvr::service { - class GrpcSecurityGateway; - class gRPCCameraServiceImpl final: public api::CameraService::Service { public: explicit gRPCCameraServiceImpl( - CameraStreamLowLatencyConfig stream_config = {}, - std::shared_ptr security_gateway = nullptr); + 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; @@ -38,7 +33,6 @@ namespace cmvr::service { private: device::DeviceManager& dmgr_; CameraStreamLowLatencyConfig stream_config_; - std::shared_ptr security_gateway_; //双向流读写线程 std::shared_ptr read_thread_ = nullptr; diff --git a/cmvr-es/service/grpc/server/include/grpc_camera_stream_policy.h b/cmvr-es/service/grpc/include/grpc_camera_stream_policy.h similarity index 100% rename from cmvr-es/service/grpc/server/include/grpc_camera_stream_policy.h rename to cmvr-es/service/grpc/include/grpc_camera_stream_policy.h diff --git a/cmvr-es/service/grpc/server/include/grpc_dexhand_service.h b/cmvr-es/service/grpc/include/grpc_dexhand_service.h similarity index 90% rename from cmvr-es/service/grpc/server/include/grpc_dexhand_service.h rename to cmvr-es/service/grpc/include/grpc_dexhand_service.h index 39dc8930..865a9870 100644 --- a/cmvr-es/service/grpc/server/include/grpc_dexhand_service.h +++ b/cmvr-es/service/grpc/include/grpc_dexhand_service.h @@ -5,19 +5,14 @@ #ifndef GRPC_DEXHAND_SERVICE_H #define GRPC_DEXHAND_SERVICE_H -#include - #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 GrpcSecurityGateway; class gRPCDexHandServiceImpl final: public api::DexHandService::Service { public: gRPCDexHandServiceImpl(); - explicit gRPCDexHandServiceImpl( - std::shared_ptr security_gateway); ~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; @@ -29,7 +24,6 @@ namespace cmvr::service { grpc::Status GetSensorDataStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; private: device::DeviceManager& dmgr_; - std::shared_ptr security_gateway_; }; } diff --git a/cmvr-es/service/grpc/server/include/grpc_head_service.h b/cmvr-es/service/grpc/include/grpc_head_service.h similarity index 92% rename from cmvr-es/service/grpc/server/include/grpc_head_service.h rename to cmvr-es/service/grpc/include/grpc_head_service.h index 04b4b5bf..4b8b20c3 100644 --- a/cmvr-es/service/grpc/server/include/grpc_head_service.h +++ b/cmvr-es/service/grpc/include/grpc_head_service.h @@ -1,20 +1,15 @@ #ifndef BIO_HEAD_SERVICE_H #define BIO_HEAD_SERVICE_H -#include - #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 GrpcSecurityGateway; class gRPCMBioHeadServiceImpl : public api::BioHeadService::Service { public: gRPCMBioHeadServiceImpl(); - explicit gRPCMBioHeadServiceImpl( - std::shared_ptr security_gateway); ~gRPCMBioHeadServiceImpl() override = default; grpc::Status SetExpression(grpc::ServerContext* context, @@ -49,7 +44,6 @@ namespace cmvr::service grpc::Status ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response) override; private: device::DeviceManager& dmgr_; - std::shared_ptr security_gateway_; }; } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_hlc_service.h b/cmvr-es/service/grpc/include/grpc_hlc_service.h similarity index 57% rename from cmvr-es/service/grpc/server/include/grpc_hlc_service.h rename to cmvr-es/service/grpc/include/grpc_hlc_service.h index 607e049d..9fc593cc 100644 --- a/cmvr-es/service/grpc/server/include/grpc_hlc_service.h +++ b/cmvr-es/service/grpc/include/grpc_hlc_service.h @@ -3,24 +3,15 @@ // #pragma once -#include - #include "cmvr/api/hlc_service.grpc.pb.h" -#include "manager/device_manager/include/device_manager.h" namespace cmvr { namespace service { - class GrpcSecurityGateway; class gRPCHlcServiceImpl final : public api::HlcService::Service { public: gRPCHlcServiceImpl(); - explicit gRPCHlcServiceImpl( - std::shared_ptr security_gateway); ~gRPCHlcServiceImpl() = default; grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override; - private: - device::DeviceManager& dmgr_; - std::shared_ptr security_gateway_; }; diff --git a/cmvr-es/service/grpc/server/include/grpc_microphone_service.h b/cmvr-es/service/grpc/include/grpc_microphone_service.h similarity index 90% rename from cmvr-es/service/grpc/server/include/grpc_microphone_service.h rename to cmvr-es/service/grpc/include/grpc_microphone_service.h index afbe34d7..1e4eac04 100644 --- a/cmvr-es/service/grpc/server/include/grpc_microphone_service.h +++ b/cmvr-es/service/grpc/include/grpc_microphone_service.h @@ -5,7 +5,6 @@ #ifndef GRPC_MICROPHONE_SERVICE_H #define GRPC_MICROPHONE_SERVICE_H -#include #include "cmvr/api/microphone_service.grpc.pb.h" #include "manager/device_manager/include/device_manager.h" #include "devices/microphone/abstract_microphone.h" @@ -13,13 +12,10 @@ namespace cmvr::service { - class GrpcSecurityGateway; class gRPCMicroPhoneServiceImpl: public api::MicPhoneService::Service { public: gRPCMicroPhoneServiceImpl(); - explicit gRPCMicroPhoneServiceImpl( - std::shared_ptr security_gateway); ~gRPCMicroPhoneServiceImpl() override = default; 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; @@ -31,7 +27,6 @@ namespace cmvr::service grpc::Status GetVolume(grpc::ServerContext* context, const api::GetMicPhoneVolumeCommand_Request* request,api::GetMicPhoneVolumeCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; - std::shared_ptr security_gateway_; }; } #endif //GRPC_MICROPHONE_SERVICE_H diff --git a/cmvr-es/service/grpc/server/include/grpc_speaker_service.h b/cmvr-es/service/grpc/include/grpc_speaker_service.h similarity index 89% rename from cmvr-es/service/grpc/server/include/grpc_speaker_service.h rename to cmvr-es/service/grpc/include/grpc_speaker_service.h index daf10eaa..2bf0e4c1 100644 --- a/cmvr-es/service/grpc/server/include/grpc_speaker_service.h +++ b/cmvr-es/service/grpc/include/grpc_speaker_service.h @@ -5,20 +5,15 @@ #ifndef GRPC_SPEAKER_SERVICE_H #define GRPC_SPEAKER_SERVICE_H -#include - #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 GrpcSecurityGateway; class gRPCSpeakerServiceImpl: public api::SpeakerService::Service { public: gRPCSpeakerServiceImpl(); - explicit gRPCSpeakerServiceImpl( - std::shared_ptr security_gateway); ~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; @@ -30,7 +25,6 @@ namespace cmvr::service { grpc::Status GetVolume(grpc::ServerContext* context, const api::GetSpeakerVolumeCommand_Request* request,api::GetSpeakerVolumeCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; - std::shared_ptr security_gateway_; }; } diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h new file mode 100644 index 00000000..41fb2646 --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -0,0 +1,28 @@ +// +// 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/server/include/camera_operational_activity_registry.h b/cmvr-es/service/grpc/server/include/camera_operational_activity_registry.h deleted file mode 100644 index cf63aedb..00000000 --- a/cmvr-es/service/grpc/server/include/camera_operational_activity_registry.h +++ /dev/null @@ -1,114 +0,0 @@ -#ifndef CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H -#define CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "devices/camera/abstract_camera.h" - -namespace cmvr::service { - -// Tracks successful CameraService::StartCamera calls. StopAll uses this -// registry to stop the corresponding operational pipelines without invoking -// AbstractDevice::stop(). -class CameraOperationalActivityRegistry final { -public: - enum class DispatchResult { - Success, - RejectedByStopAll, - RejectedByDispatchFence, - DeviceFailure, - }; - - using DispatchFence = std::function; - - struct ActivityToken { - std::string device_id; - std::uint64_t activity_generation{0U}; - bool owns_start{false}; - - bool valid() const noexcept - { - return !device_id.empty() && activity_generation != 0U; - } - }; - - DispatchResult start( - const std::string& device_id, - const std::shared_ptr& camera, - ActivityToken* token = nullptr, - DispatchFence dispatch_fence = {}); - - // Rolls back only the exact activity created by start(). A newer start for - // the same device is never stopped by an older request finishing late. - bool stopIfCurrent(const ActivityToken& token); - - // Explicit StopCamera retains its legacy lifecycle behavior, but is - // serialized here so it cannot race an operational StopAll stop. - DispatchResult stopLifecycle( - const std::string& device_id, - const std::shared_ptr& camera, - DispatchFence dispatch_fence = {}); - - // Reconciles a lifecycle stop performed outside CameraService. - void markCameraStopped(const std::string& device_id); - - // StopAll must close the process-wide admission gate first. Successful - // entries are removed. Failed entries remain quarantined for a later - // StopAll retry. - bool stopAllActivities(std::vector* failures = nullptr); - - // StopAll must close the process-wide admission gate first. Stops only the - // operational pipeline tracked for device_id and never invokes the camera - // lifecycle stop(). - bool stopActivitiesForDevice( - const std::string& device_id, - std::vector* failures = nullptr); - - // If no StartCamera activity is tracked, StopAll can still quiesce the - // camera currently present in DeviceManager's inventory. A tracked camera - // takes precedence over the fallback. Repeated calls in one StopAll round - // do not stop the same instance twice. - bool stopActivitiesForDevice( - const std::string& device_id, - const std::shared_ptr& fallback_camera, - std::vector* failures = nullptr); - - std::size_t activeCameraCount() const; - // Does not wait for per-device driver I/O. This conservative snapshot - // includes devices with an admitted or historical dispatch state, allowing - // StopAll to cover activity absent from the DeviceManager snapshot. - std::vector trackedDeviceIds() const; - std::vector activeDeviceIds() const; - void clearForTesting(); - -private: - struct DeviceState { - mutable std::mutex mutex; - std::shared_ptr active_camera; - std::weak_ptr last_stopped_camera; - std::uint64_t last_stopped_generation{0U}; - std::uint64_t activity_generation{0U}; - std::atomic active{false}; - }; - - std::shared_ptr stateForDevice( - const std::string& device_id, - bool create); - - mutable std::mutex states_mutex_; - std::unordered_map> states_; -}; - -CameraOperationalActivityRegistry& -globalCameraOperationalActivityRegistry(); - -} // namespace cmvr::service - -#endif // CMVR_ES_CAMERA_OPERATIONAL_ACTIVITY_REGISTRY_H diff --git a/cmvr-es/service/grpc/server/include/camera_ptz_activity_registry.h b/cmvr-es/service/grpc/server/include/camera_ptz_activity_registry.h deleted file mode 100644 index 7cbbf006..00000000 --- a/cmvr-es/service/grpc/server/include/camera_ptz_activity_registry.h +++ /dev/null @@ -1,96 +0,0 @@ -#ifndef CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H -#define CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H - -#include -#include -#include -#include -#include -#include -#include - -#include "devices/camera/abstract_camera.h" - -namespace cmvr::service { - -// Tracks PTZ commands whose START has not yet been paired with a successful -// STOP. The registry is an operational control boundary; it never invokes a -// camera lifecycle method. -class CameraPtzActivityRegistry final { -public: - enum class DispatchResult { - Success, - RejectedByStopAll, - RejectedByDispatchFence, - DeviceFailure, - }; - - using DispatchFence = std::function; - - DispatchResult control( - const std::string& device_id, - const std::shared_ptr& camera, - device::PtzCommand command, - bool stop, - int speed, - DispatchFence dispatch_fence = {}); - - // Reconciles externally stopped camera PTZ state with this registry. A - // camera backend can call this if it stops PTZ outside CameraService. - void markCameraStopped(const std::string& device_id); - - // StopAll must close the process-wide admission gate before calling this. - // A true result means every tracked START received a successful matching - // STOP. Failed entries are retained so a later StopAll can retry them. - bool stopAllActivities(std::vector* failures = nullptr); - - // StopAll must close the process-wide admission gate first. Stops only PTZ - // commands tracked for device_id. Calls for different physical cameras may - // execute concurrently; calls for one camera remain ordered with control(). - bool stopActivitiesForDevice( - const std::string& device_id, - std::vector* failures = nullptr); - - std::size_t activeCommandCount() const; - // Does not wait for per-device driver I/O. This conservative snapshot - // includes devices with an admitted or historical dispatch state, allowing - // StopAll to cover activity absent from the DeviceManager snapshot. - std::vector trackedDeviceIds() const; - std::vector activeDeviceIds() const; - void clearForTesting(); - -private: - struct PtzCommandHash { - std::size_t operator()(device::PtzCommand command) const noexcept - { - return static_cast(command); - } - }; - - struct ActiveCommand { - std::shared_ptr camera; - int speed{0}; - }; - - using CameraCommands = std::unordered_map< - device::PtzCommand, ActiveCommand, PtzCommandHash>; - - struct DeviceState { - mutable std::mutex mutex; - CameraCommands commands; - std::atomic active_command_count{0U}; - }; - - std::shared_ptr stateForDevice( - const std::string& device_id, - bool create); - - mutable std::mutex states_mutex_; - std::unordered_map> states_; -}; - -CameraPtzActivityRegistry& globalCameraPtzActivityRegistry(); - -} // namespace cmvr::service - -#endif // CMVR_ES_CAMERA_PTZ_ACTIVITY_REGISTRY_H diff --git a/cmvr-es/service/grpc/server/include/grpc_arm_teleop_service.h b/cmvr-es/service/grpc/server/include/grpc_arm_teleop_service.h deleted file mode 100644 index d368428d..00000000 --- a/cmvr-es/service/grpc/server/include/grpc_arm_teleop_service.h +++ /dev/null @@ -1,105 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include - -#include "cmvr/api/arm_teleop_v1.grpc.pb.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" - -namespace cmvr::service { - -class GrpcSecurityGateway; - -} // namespace cmvr::service - -namespace cmvr::safety { -class SafetyManager; -} - -namespace cmvr::service { - -namespace arm_teleop = cmvr::api::armteleop::v1; - -struct ArmTeleopBackendResult { - bool success{false}; - grpc::StatusCode status_code{grpc::StatusCode::INTERNAL}; - std::string detail; - - static ArmTeleopBackendResult ok() - { - return {true, grpc::StatusCode::OK, {}}; - } - - static ArmTeleopBackendResult failure( - const grpc::StatusCode code, - std::string message) - { - return {false, code, std::move(message)}; - } -}; - -struct ArmTeleopBackendSnapshot { - arm_teleop::JointState joint_state; - arm_teleop::RobotSafetyState safety; -}; - -// Execution boundary for ArmTeleopService. The first implementation registers a -// disabled backend in production and injects a fake backend in tests. A future -// RobotArm adapter must live behind this interface so the gRPC reader thread can -// remain a bounded mailbox producer and never touch hardware. Implementations -// must keep every call bounded and non-blocking with respect to hardware I/O; -// snapshot() must return cached state rather than synchronously polling a bus. -class ArmTeleopBackend { -public: - virtual ~ArmTeleopBackend() = default; - - virtual bool available() const noexcept = 0; - virtual std::string unavailableReason() const { return {}; } - virtual arm_teleop::RobotManifest manifest() const = 0; - virtual bool supportsForceFeedback() const noexcept = 0; - virtual ArmTeleopBackendResult open( - const arm_teleop::OpenSession& request) = 0; - // The deadline is computed from the receiver's local monotonic clock. - // Implementations must re-check it immediately before committing a - // hardware command; the protobuf valid_for duration is never interpreted - // as a cross-machine absolute timestamp. - virtual ArmTeleopBackendResult applySetpoint( - const arm_teleop::JointSetpoint& setpoint, - std::chrono::steady_clock::time_point deadline) = 0; - virtual ArmTeleopBackendResult stop( - arm_teleop::StopReason reason, - const std::string& detail) = 0; - virtual ArmTeleopBackendSnapshot snapshot() const = 0; -}; - -std::shared_ptr makeDisabledArmTeleopBackend(); - -class ArmTeleopServiceImpl final - : public arm_teleop::ArmTeleopService::Service { -public: - explicit ArmTeleopServiceImpl( - std::shared_ptr backend = - makeDisabledArmTeleopBackend(), - control::ControlAuthorityManager* authority = nullptr, - std::shared_ptr security_gateway = nullptr, - safety::SafetyManager* safety_manager = nullptr); - ~ArmTeleopServiceImpl() override = default; - - grpc::Status Teleoperate( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream) override; - -private: - std::shared_ptr backend_; - control::ControlAuthorityManager* authority_{nullptr}; - std::shared_ptr security_gateway_; - safety::SafetyManager* safety_manager_{nullptr}; -}; - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_command_transaction.h b/cmvr-es/service/grpc/server/include/grpc_command_transaction.h deleted file mode 100644 index 5008ce21..00000000 --- a/cmvr-es/service/grpc/server/include/grpc_command_transaction.h +++ /dev/null @@ -1,211 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include - -#include "cmvr/api/common.pb.h" -#include "manager/safety_manager/include/safety_manager.h" -#include "service/grpc/server/include/grpc_security.h" - -namespace cmvr::service { - -grpc::Status grpcStatusForSafetyReason( - safety::SafetyReason reason, - const std::string& detail = {}); - -struct GrpcStreamingSafetyOpen { - std::string full_method_name; - std::string device_id; - std::string session_id; - std::string expected_service_instance_id; - std::optional expected_device_generation; - std::uint64_t authority_generation{0}; - safety::SafetyClock::time_point deadline{ - safety::SafetyClock::time_point::max()}; -}; - -// Binds a long-lived control stream to one coordinator permit. Stream -// protocols retain their own sequence, watchdog, and control-lease rules; -// this object owns the safety epoch/device-generation checks shared by all of -// them. It deliberately does not use the unary idempotency ledger. -class GrpcStreamingSafetySession final { -public: - GrpcStreamingSafetySession( - safety::SafetyManager& coordinator, - const GrpcRequestContext& request_context, - GrpcStreamingSafetyOpen open); - - GrpcStreamingSafetySession( - GrpcStreamingSafetySession&&) noexcept = default; - GrpcStreamingSafetySession& operator=( - GrpcStreamingSafetySession&&) noexcept = default; - GrpcStreamingSafetySession( - const GrpcStreamingSafetySession&) = delete; - GrpcStreamingSafetySession& operator=( - const GrpcStreamingSafetySession&) = delete; - - bool admitted() const noexcept { return permit_.has_value(); } - const grpc::Status& status() const noexcept { return status_; } - const safety::AdmissionDecision& admissionDecision() const noexcept - { - return admission_decision_; - } - - bool revalidate(); - safety::DispatchGuard beginDispatch(); - std::uint64_t safetyEpoch() const noexcept; - std::uint64_t deviceGeneration() const noexcept; - std::uint64_t authorityGeneration() const noexcept; - -private: - void reject_(safety::SafetyReason reason, std::string detail); - - safety::SafetyManager* coordinator_{nullptr}; - std::optional permit_; - safety::AdmissionDecision admission_decision_; - grpc::Status status_; -}; - -// Owns one unary command from identity reservation through the final hardware -// dispatch fence. Legacy/Shadow calls without a command ID still use admission, -// but deliberately remain outside the idempotency ledger for wire compatibility. -class GrpcCommandTransaction final { -public: - GrpcCommandTransaction( - safety::SafetyManager& coordinator, - GrpcRequestContext request_context, - GrpcMethodPolicy method_policy, - const google::protobuf::Message& request, - google::protobuf::Message& response); - ~GrpcCommandTransaction() noexcept; - - GrpcCommandTransaction(GrpcCommandTransaction&& other) noexcept; - GrpcCommandTransaction& operator=( - GrpcCommandTransaction&& other) noexcept; - GrpcCommandTransaction(const GrpcCommandTransaction&) = delete; - GrpcCommandTransaction& operator=(const GrpcCommandTransaction&) = delete; - - bool shouldExecute() const noexcept { return should_execute_; } - const grpc::Status& status() const noexcept { return status_; } - const safety::AdmissionDecision& admissionDecision() const noexcept - { - return admission_decision_; - } - - // Must be called immediately before the first driver/SDK mutation. The - // returned guard remains owned by this transaction until finish(). - bool beginDispatch(); - // Long-running unary commands may submit more than one hardware command. - // Revalidate the original permit between submissions, then hold the - // returned guard only around one driver/SDK mutation. - bool revalidate(); - safety::DispatchGuard beginScopedDispatch(); - // Internal mitigation for a command-owned activity. This obtains a fresh - // Stop-lane permit, so an expired/revoked Actuate permit cannot suppress a - // physical stop. - safety::DispatchGuard beginSafetyStopDispatch(); - const grpc::Status& dispatchStatus() const noexcept - { - return dispatch_status_; - } - - grpc::Status finish( - grpc::Status operation_status, - safety::SafetyReason reason = safety::SafetyReason::None, - std::optional lifecycle = std::nullopt); - grpc::Status finishException(std::string detail) noexcept; - - const std::string& deviceId() const noexcept { return device_id_; } - const std::string& commandId() const noexcept { return command_id_; } - std::uint64_t safetyEpoch() const noexcept - { - return admission_decision_.safety_epoch; - } - std::uint64_t deviceGeneration() const noexcept - { - return admission_decision_.device_generation; - } - -private: - void initialize_(const google::protobuf::Message& request); - void rejectBeforeDispatch_( - safety::SafetyReason reason, - std::string detail, - grpc::Status status, - bool complete_reserved_record); - bool restoreOutcome_(const safety::CommandOutcome& outcome); - bool completeLedger_( - safety::CommandLifecycle lifecycle, - safety::SafetyReason reason, - const std::string& detail, - bool hardware_submission_possible) noexcept; - void populateFeedback_( - bool success, - safety::SafetyReason reason, - safety::CommandLifecycle lifecycle, - const std::string& detail); - void abandon_() noexcept; - - safety::SafetyManager* coordinator_{nullptr}; - GrpcRequestContext request_context_; - GrpcMethodPolicy method_policy_; - google::protobuf::Message* response_{nullptr}; - safety::CommandLedger::Ticket ledger_ticket_; - std::optional permit_; - std::optional dispatch_guard_; - safety::AdmissionDecision admission_decision_; - grpc::Status status_; - grpc::Status dispatch_status_; - std::string device_id_; - std::string command_id_; - std::string payload_hash_; - safety::SafetyClock::time_point deadline_{ - safety::SafetyClock::time_point::max()}; - bool should_execute_{false}; - bool owns_ledger_record_{false}; - bool dispatch_started_{false}; - bool completed_{false}; - std::uint64_t safety_stop_sequence_{0}; -}; - -using GrpcUnaryCommandOperation = - std::function; - -grpc::Status executeRegisteredGrpcCommand( - const std::shared_ptr& gateway, - grpc::ServerContext* server_context, - safety::SafetyManager& coordinator, - const std::string& full_method_name, - const google::protobuf::Message* request, - google::protobuf::Message* response, - GrpcUnaryCommandOperation operation); - -// For a legacy RPC whose validated request selects one of several fixed -// server-side intents. Gateway authorization still uses the registered method -// policy; the supplied policy only narrows Coordinator admission after the -// server has parsed the request (for example PTZ START versus STOP). -grpc::Status executeServerDerivedGrpcCommand( - const std::shared_ptr& gateway, - grpc::ServerContext* server_context, - safety::SafetyManager& coordinator, - const std::string& full_method_name, - GrpcMethodPolicy effective_policy, - const google::protobuf::Message* request, - google::protobuf::Message* response, - GrpcUnaryCommandOperation operation); - -// Stable across processes and protobuf map iteration order. Transport identity -// fields and client timestamps are excluded; device generation remains part of -// the semantic payload. -std::string deterministicGrpcPayloadHash( - const std::string& full_method_name, - const google::protobuf::Message& request); - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_error_logging_interceptor.h b/cmvr-es/service/grpc/server/include/grpc_error_logging_interceptor.h deleted file mode 100644 index f780aa5b..00000000 --- a/cmvr-es/service/grpc/server/include/grpc_error_logging_interceptor.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include -#include -#include - -#include - -namespace cmvr::service { - -enum class GrpcFailureKind { - APPLICATION, - GRPC_STATUS, - MALFORMED_RESPONSE, -}; - -enum class GrpcFailureSeverity { - INFO, - WARNING, - ERROR, -}; - -struct GrpcFailureRecord { - GrpcFailureKind kind{GrpcFailureKind::APPLICATION}; - std::string method; - std::string peer; - grpc::StatusCode status_code{grpc::StatusCode::OK}; - GrpcFailureSeverity severity{GrpcFailureSeverity::ERROR}; - std::string detail; -}; - -using GrpcFailureSink = std::function; - -std::unique_ptr -makeGrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink = {}); - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_motor_service.h b/cmvr-es/service/grpc/server/include/grpc_motor_service.h deleted file mode 100644 index 8d7a11d8..00000000 --- a/cmvr-es/service/grpc/server/include/grpc_motor_service.h +++ /dev/null @@ -1,222 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include "cmvr/api/motor_service.grpc.pb.h" -#include "devices/motor/abstract_motor.h" -#include "service/grpc/server/include/motor_activity_coordinator.h" - -namespace cmvr::device { -class DeviceManager; -} - -namespace cmvr::service { - -class gRPCMotorServiceImplTestAccess; -class GrpcCommandTransaction; -class GrpcSecurityGateway; -struct GrpcRequestContext; - -// A deliberately thin synchronous gRPC facade over AbstractMotor. It does not -// schedule trajectories or retain asynchronous operations. The small amount of -// state below only prevents two RPCs from owning one motor at the same time and -// lets emergencyStop invalidate an already-running blocking RPC/stream. -class gRPCMotorServiceImpl final : public api::MotorService::Service { -public: - gRPCMotorServiceImpl(); - explicit gRPCMotorServiceImpl( - std::shared_ptr security_gateway); - ~gRPCMotorServiceImpl() override = default; - - grpc::Status setZero(grpc::ServerContext* context, - const api::SetMotorZeroRequest* request, - api::MotorCommandResponse* response) override; - grpc::Status moveToZero(grpc::ServerContext* context, - const api::MoveMotorToZeroRequest* request, - api::MotorCommandResponse* response) override; - grpc::Status profilePosition(grpc::ServerContext* context, - const api::ProfilePositionRequest* request, - api::MotorCommandResponse* response) override; - grpc::Status profileVelocity(grpc::ServerContext* context, - const api::ProfileVelocityRequest* request, - api::MotorCommandResponse* response) override; - grpc::Status streamCyclicPosition( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream) override; - grpc::Status streamCyclicVelocity( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream) override; - grpc::Status emergencyStop(grpc::ServerContext* context, - const api::EmergencyStopRequest* request, - api::MotorCommandResponse* response) override; - grpc::Status getStatus(grpc::ServerContext* context, - const api::GetMotorStatusRequest* request, - api::GetMotorStatusResponse* response) override; - grpc::Status setEnabled(grpc::ServerContext* context, - const api::SetMotorEnabledRequest* request, - api::MotorCommandResponse* response) override; - -private: - friend class gRPCMotorServiceImplTestAccess; - - struct MotorControlState { - std::mutex mutex; - // Serializes all motion/enable writes with emergency quick-stop. The - // cancel-generation check and the corresponding motor write must occur - // while this mutex is held to prevent stale writes after an E-stop. - std::mutex command_mutex; - // Keeps the two-phase best-effort/final quick-stop sequence exclusive. - // Without this, one concurrent E-stop could clear the shared - // in-progress flag while another E-stop is still dispatching. - std::mutex emergency_mutex; - // Serializes exception cleanup from ownership inspection through the - // final release. A second stale cleanup must re-check ownership only - // after the first cleanup has fully completed. - std::mutex exception_cleanup_mutex; - bool busy{false}; - bool emergency_stopped{false}; - bool emergency_stop_in_progress{false}; - bool exception_cleanup_pending{false}; - std::uint64_t cancel_generation{0}; - api::MotorControlType active_control{api::MOTOR_CONTROL_NONE}; - std::string last_error; - }; - - struct ResolvedMotor { - std::shared_ptr motor; - std::shared_ptr control; - }; - - enum class ResolveAccess { - Control, - Observe, - }; - - struct MotorControlEntry { - std::weak_ptr owner; - std::shared_ptr state; - MotorActivityCoordinator::Registration stop_all_registration; - }; - - class ControlLease { - public: - ControlLease(std::shared_ptr state, - std::uint64_t generation); - ~ControlLease(); - ControlLease(const ControlLease&) = delete; - ControlLease& operator=(const ControlLease&) = delete; - - std::uint64_t generation() const noexcept { return generation_; } - - private: - std::shared_ptr state_; - std::uint64_t generation_{0}; - int uncaught_on_entry_{0}; - }; - - grpc::Status resolveMotor(const api::MotorTarget& target, - ResolvedMotor& resolved, - ResolveAccess access = ResolveAccess::Control) const; - std::shared_ptr stateFor( - const std::shared_ptr& motor) const; - std::shared_ptr existingStateFor( - const std::shared_ptr& motor) const; - std::unique_ptr acquireControl( - const ResolvedMotor& resolved, - api::MotorControlType control, - grpc::Status& failure, - bool allow_emergency_stopped = false) const; - - grpc::Status runProfilePosition(grpc::ServerContext* context, - const ResolvedMotor& resolved, - double target_position_rad, - double max_velocity_rad_s, - double acceleration_rad_s2, - const api::MotorWaitOptions& wait, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command); - grpc::Status waitForPosition(grpc::ServerContext* context, - const ResolvedMotor& resolved, - std::uint64_t generation, - double target_position_rad, - const api::MotorWaitOptions& wait, - api::MotorCommandResponse* response, - std::chrono::steady_clock::time_point started); - grpc::Status waitForVelocity(grpc::ServerContext* context, - const ResolvedMotor& resolved, - std::uint64_t generation, - double target_velocity_rad_s, - const api::MotorWaitOptions& wait, - api::MotorCommandResponse* response, - std::chrono::steady_clock::time_point started); - - grpc::Status setZeroImpl(grpc::ServerContext* context, - const api::SetMotorZeroRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command); - grpc::Status moveToZeroImpl(grpc::ServerContext* context, - const api::MoveMotorToZeroRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command); - grpc::Status profilePositionImpl( - grpc::ServerContext* context, - const api::ProfilePositionRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command); - grpc::Status profileVelocityImpl( - grpc::ServerContext* context, - const api::ProfileVelocityRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command); - grpc::Status emergencyStopImpl( - grpc::ServerContext* context, - const api::EmergencyStopRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command); - grpc::Status getStatusImpl(grpc::ServerContext* context, - const api::GetMotorStatusRequest* request, - api::GetMotorStatusResponse* response); - grpc::Status setEnabledImpl( - grpc::ServerContext* context, - const api::SetMotorEnabledRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command); - grpc::Status streamCyclicPositionImpl( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream, - std::optional& cleanup_target, - const GrpcRequestContext& request_context); - grpc::Status streamCyclicVelocityImpl( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream, - std::optional& cleanup_target, - const GrpcRequestContext& request_context); - - void bestEffortQuickStop(const api::MotorTarget& target, - const std::string& error) noexcept; - void latchUnsafeAfterFailedStop( - const std::shared_ptr& state, - const std::string& error) const; - void fillMotorStatus(const ResolvedMotor& resolved, - api::MotorStatus* status) const; - void setLastError(const std::shared_ptr& state, - const std::string& error) const; - - device::DeviceManager& dmgr_; - std::shared_ptr security_gateway_; - mutable std::mutex states_mutex_; - mutable std::unordered_map states_; -}; - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_recovery_audit.h b/cmvr-es/service/grpc/server/include/grpc_recovery_audit.h deleted file mode 100644 index 9d25d55e..00000000 --- a/cmvr-es/service/grpc/server/include/grpc_recovery_audit.h +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -namespace cmvr::service { - -struct RecoveryAuditRecord { - std::uint64_t occurred_at_unix_ms{0}; - std::string stage; - std::string correlation_id; - std::string principal_id; - std::string peer; - std::string recovery_id; - std::string reason; - std::string mode; - bool all_devices{false}; - std::vector device_ids; - std::uint64_t expected_safety_epoch{0}; - std::uint64_t previous_safety_epoch{0}; - std::uint64_t current_safety_epoch{0}; - std::string result; -}; - -class RecoveryAuditSink { -public: - virtual ~RecoveryAuditSink() = default; - virtual bool append( - const RecoveryAuditRecord& record, - std::string* error = nullptr) noexcept = 0; -}; - -std::shared_ptr makeFileRecoveryAuditSink( - std::string path); - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_robot_arm_teleop_backend.h b/cmvr-es/service/grpc/server/include/grpc_robot_arm_teleop_backend.h deleted file mode 100644 index 39d241ba..00000000 --- a/cmvr-es/service/grpc/server/include/grpc_robot_arm_teleop_backend.h +++ /dev/null @@ -1,18 +0,0 @@ -#pragma once - -#include - -#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" -#include "devices/arm/robot_arm.h" -#include "service/grpc/server/include/grpc_arm_teleop_service.h" - -namespace cmvr::service { - -// Creates a fail-closed adapter from the process RobotArm abstraction to the -// session-based ArmTeleop backend. available() remains false unless the config, -// RobotModel and RobotArm capability all pass static validation. -std::shared_ptr makeRobotArmTeleopBackend( - std::shared_ptr arm, - const config::ArmTeleopBackendConfig& config); - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_safety_participants.h b/cmvr-es/service/grpc/server/include/grpc_safety_participants.h deleted file mode 100644 index bb4252b4..00000000 --- a/cmvr-es/service/grpc/server/include/grpc_safety_participants.h +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include - -namespace cmvr::safety { -class SafetyManager; -} - -namespace cmvr::service { - -class ActionQueueExecutor; -class StopOperationDispatcher; - -class GrpcSafetyParticipantRegistration final { -public: - ~GrpcSafetyParticipantRegistration(); - GrpcSafetyParticipantRegistration( - GrpcSafetyParticipantRegistration&&) noexcept; - GrpcSafetyParticipantRegistration& operator=( - GrpcSafetyParticipantRegistration&&) noexcept; - GrpcSafetyParticipantRegistration( - const GrpcSafetyParticipantRegistration&) = delete; - GrpcSafetyParticipantRegistration& operator=( - const GrpcSafetyParticipantRegistration&) = delete; - -private: - friend std::unique_ptr - registerGrpcSafetyParticipants( - safety::SafetyManager&, - std::shared_ptr, - std::shared_ptr); - struct Impl; - explicit GrpcSafetyParticipantRegistration(std::unique_ptr impl); - std::unique_ptr impl_; -}; - -std::unique_ptr -registerGrpcSafetyParticipants( - safety::SafetyManager& coordinator, - std::shared_ptr action_queue, - std::shared_ptr stop_dispatcher); - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_safety_proto.h b/cmvr-es/service/grpc/server/include/grpc_safety_proto.h deleted file mode 100644 index c5bbae99..00000000 --- a/cmvr-es/service/grpc/server/include/grpc_safety_proto.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include "cmvr/api/safety_command.pb.h" -#include "manager/safety_manager/include/safety_manager.h" - -namespace cmvr::service { - -api::CommandReasonCode toApiSafetyReason( - safety::SafetyReason value) noexcept; -safety::SafetyReason fromApiSafetyReason( - api::CommandReasonCode value) noexcept; -api::SafetyTriState toApiSafetyTriState( - safety::TriState value) noexcept; -api::SafetyCondition toApiSafetyCondition( - safety::SafetyCondition value) noexcept; -api::SystemAdmissionState toApiSystemAdmissionState( - safety::SystemAdmissionState value) noexcept; -api::DeviceAdmissionState toApiDeviceAdmissionState( - safety::DeviceAdmissionState value) noexcept; -api::SafetyBlockerScope toApiSafetyBlockerScope( - safety::BlockerScope value) noexcept; -api::SafetyRecoveryRequirement toApiRecoveryRequirement( - safety::RecoveryRequirement value) noexcept; -api::SafetyOperationResult toApiRecoveryResult( - safety::RecoveryResultCode value) noexcept; - -void populateDeviceSafetyState( - const safety::DeviceSafetyStateView& source, - api::DeviceSafetyStateInfo& destination); -void populateSafetyTargetResult( - const safety::SafetyTargetResult& source, - api::SafetyOperationTargetResult& destination); -void populateSafetyParticipantState( - const safety::ParticipantSafetyStateView& source, - api::SafetyParticipantStateInfo& destination); - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/include/grpc_security.h b/cmvr-es/service/grpc/server/include/grpc_security.h deleted file mode 100644 index 9e8de77f..00000000 --- a/cmvr-es/service/grpc/server/include/grpc_security.h +++ /dev/null @@ -1,268 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" -#include "manager/safety_manager/include/safety_types.h" - -namespace cmvr::service { - -enum class GrpcTransportSecurity { - Insecure, - ServerTls, - MutualTls, -}; - -enum class GrpcAuthenticationMethod { - Disabled, - StaticToken, - Jwt, - TlsClientCertificate, -}; - -enum class GrpcRole { - Anonymous, - Observer, - Operator, - SafetyAdmin, -}; - -enum class GrpcAccessClass { - Read, - Mutate, - Stop, - Recover, -}; - -enum class GrpcRecoveryExposure { - Disabled, - LocalOnly, - Authorized, -}; - -struct GrpcPrincipal { - std::string id{"anonymous"}; - GrpcAuthenticationMethod method{GrpcAuthenticationMethod::Disabled}; - bool authenticated{false}; - std::vector roles{GrpcRole::Anonymous}; -}; - -struct GrpcCallFacts { - std::string correlation_id; - std::string full_method_name; - std::string peer; - std::multimap metadata; - bool transport_encrypted{false}; - bool local_peer{false}; - std::chrono::steady_clock::time_point received_at; - std::chrono::steady_clock::time_point deadline; -}; - -struct GrpcRequestContext { - std::string correlation_id; - std::string full_method_name; - std::string peer; - GrpcPrincipal principal; - bool transport_encrypted{false}; - bool local_peer{false}; - std::chrono::steady_clock::time_point received_at; - std::chrono::steady_clock::time_point deadline; -}; - -struct GrpcMethodPolicy { - std::string full_method_name; - GrpcAccessClass access{GrpcAccessClass::Read}; - GrpcRole minimum_role{GrpcRole::Observer}; - safety::CommandIntent command_intent{safety::CommandIntent::Observe}; - safety::SafetyPolicyFamily policy_family{ - safety::SafetyPolicyFamily::Sensor}; - bool mutating{false}; - bool safety_lane{false}; - - safety::CommandDescriptor commandDescriptor() const - { - return { - full_method_name, - command_intent, - policy_family, - mutating, - safety_lane}; - } -}; - -struct GrpcSecurityRuntimeConfig { - GrpcTransportSecurity transport{GrpcTransportSecurity::Insecure}; - GrpcAuthenticationMethod authentication{ - GrpcAuthenticationMethod::Disabled}; - GrpcRecoveryExposure recovery_exposure{ - GrpcRecoveryExposure::Disabled}; - bool allow_insecure_non_loopback{false}; - bool insecure_non_loopback{false}; - bool legacy_compatibility{false}; - std::string recovery_audit_file; -}; - -struct GrpcSecurityConfigResult { - bool valid{false}; - GrpcSecurityRuntimeConfig config; - std::string error; - std::vector warnings; -}; - -GrpcSecurityConfigResult resolveGrpcSecurityConfig( - const config::GRPCServerConfig& config, - const std::string& effective_host); - -bool isLocalGrpcPeer(const std::string& peer) noexcept; -bool isLoopbackGrpcHost(const std::string& host) noexcept; - -const char* toString(GrpcTransportSecurity value) noexcept; -const char* toString(GrpcAuthenticationMethod value) noexcept; -const char* toString(GrpcRecoveryExposure value) noexcept; -const char* toString(GrpcRole value) noexcept; - -struct GrpcAuthenticationResult { - GrpcPrincipal principal; - grpc::Status status; - - bool ok() const noexcept { return status.ok(); } -}; - -class GrpcAuthenticationProvider { -public: - virtual ~GrpcAuthenticationProvider() = default; - virtual GrpcAuthenticationResult authenticate( - const GrpcCallFacts& facts) const = 0; -}; - -class DisabledGrpcAuthenticationProvider final - : public GrpcAuthenticationProvider { -public: - GrpcAuthenticationResult authenticate( - const GrpcCallFacts& facts) const override; -}; - -struct GrpcAuthorizationDecision { - bool allowed{false}; - grpc::Status status; -}; - -class GrpcAuthorizationPolicy { -public: - virtual ~GrpcAuthorizationPolicy() = default; - virtual GrpcAuthorizationDecision authorize( - const GrpcRequestContext& context, - const GrpcMethodPolicy& method) const = 0; -}; - -class CompatibilityGrpcAuthorizationPolicy final - : public GrpcAuthorizationPolicy { -public: - explicit CompatibilityGrpcAuthorizationPolicy( - GrpcRecoveryExposure recovery_exposure); - - GrpcAuthorizationDecision authorize( - const GrpcRequestContext& context, - const GrpcMethodPolicy& method) const override; - -private: - GrpcRecoveryExposure recovery_exposure_; -}; - -class GrpcMethodPolicyRegistry final { -public: - bool registerPolicy(GrpcMethodPolicy policy); - std::optional find( - const std::string& full_method_name) const; - std::vector snapshot() const; - -private: - std::unordered_map policies_; -}; - -const GrpcMethodPolicyRegistry& defaultGrpcMethodPolicyRegistry(); - -struct GrpcSecurityAuditRecord { - std::string correlation_id; - std::string full_method_name; - std::string principal_id; - std::string peer; - GrpcAuthenticationMethod authentication{ - GrpcAuthenticationMethod::Disabled}; - GrpcAccessClass access{GrpcAccessClass::Read}; - bool authenticated{false}; - bool allowed{false}; - grpc::StatusCode status_code{grpc::StatusCode::OK}; -}; - -using GrpcSecurityAuditSink = - std::function; - -class GrpcCallGuard final { -public: - GrpcCallGuard(GrpcRequestContext context, - GrpcAuthorizationDecision decision); - - bool allowed() const noexcept { return decision_.allowed; } - const grpc::Status& status() const noexcept { return decision_.status; } - const GrpcRequestContext& context() const noexcept { return context_; } - -private: - GrpcRequestContext context_; - GrpcAuthorizationDecision decision_; -}; - -class GrpcSecurityGateway final { -public: - GrpcSecurityGateway( - GrpcSecurityRuntimeConfig config, - std::shared_ptr authentication, - std::shared_ptr authorization, - GrpcSecurityAuditSink audit_sink = {}); - - GrpcCallGuard beginCall( - grpc::ServerContext* server_context, - const GrpcMethodPolicy& method) const; - GrpcCallGuard beginCall( - GrpcCallFacts facts, - const GrpcMethodPolicy& method) const; - - const GrpcSecurityRuntimeConfig& config() const noexcept { return config_; } - -private: - GrpcSecurityRuntimeConfig config_; - std::shared_ptr authentication_; - std::shared_ptr authorization_; - GrpcSecurityAuditSink audit_sink_; -}; - -std::shared_ptr makeGrpcSecurityGateway( - const GrpcSecurityRuntimeConfig& config, - GrpcSecurityAuditSink audit_sink = {}); - -std::shared_ptr makeDefaultGrpcSecurityGateway(); - -GrpcCallGuard beginRegisteredGrpcCall( - const std::shared_ptr& gateway, - grpc::ServerContext* server_context, - const std::string& full_method_name); - -} // namespace cmvr::service - -#define CMVR_GRPC_REQUIRE_REGISTERED_CALL(gateway, server_context, method) \ - const auto cmvr_grpc_call_guard = \ - ::cmvr::service::beginRegisteredGrpcCall( \ - gateway, server_context, method); \ - if (!cmvr_grpc_call_guard.allowed()) { \ - return cmvr_grpc_call_guard.status(); \ - } diff --git a/cmvr-es/service/grpc/server/include/grpc_system_service.h b/cmvr-es/service/grpc/server/include/grpc_system_service.h deleted file mode 100644 index f0ae3deb..00000000 --- a/cmvr-es/service/grpc/server/include/grpc_system_service.h +++ /dev/null @@ -1,67 +0,0 @@ -// -// Created by xtkuang on 2025/6/6. -// - -#ifndef GRPC_SYSTEM_SERVICE_H -#define GRPC_SYSTEM_SERVICE_H - -#include -#include - -#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 ActionQueueExecutor; - class RecoveryAuditSink; - class GrpcSafetyParticipantRegistration; - class GrpcSecurityGateway; - class StopOperationDispatcher; - - class gRPCSystemServiceImpl: public api::SystemService::Service { - public: - gRPCSystemServiceImpl(); - explicit gRPCSystemServiceImpl( - std::chrono::milliseconds stop_timeout); - explicit gRPCSystemServiceImpl( - std::shared_ptr security_gateway); - gRPCSystemServiceImpl( - std::chrono::milliseconds stop_timeout, - std::shared_ptr security_gateway); - gRPCSystemServiceImpl( - std::chrono::milliseconds stop_timeout, - std::shared_ptr security_gateway, - std::shared_ptr recovery_audit_sink); - ~gRPCSystemServiceImpl() override; - // Exposed only to synchronize lifecycle concurrency tests. - static bool waitForStopDispatcherDestructionForTesting( - std::chrono::milliseconds timeout); - // Called by GrpcServerTask before grpc::Server::Shutdown so accepted - // ActionQueue handlers can reach a terminal result and do not hold the - // synchronous server shutdown open indefinitely. - void prepareForShutdown(); - 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 GetDeviceList(grpc::ServerContext* context, const api::GetDeviceListCommand_Request* request, api::GetDeviceListCommand_Feedback* response) override; - grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override; - grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override; - grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override; - grpc::Status GetSafetyState(grpc::ServerContext* context, const cmvr::api::GetSafetyStateCommand_Request* request, cmvr::api::GetSafetyStateCommand_Feedback* response) override; - grpc::Status RecoverSafetyState(grpc::ServerContext* context, const cmvr::api::RecoverSafetyStateCommand_Request* request, cmvr::api::RecoverSafetyStateCommand_Feedback* response) override; - private: - device::DeviceManager& dmgr_; - const std::chrono::milliseconds stop_timeout_; - std::shared_ptr security_gateway_; - std::shared_ptr recovery_audit_sink_; - // Process instances share running jobs through a lifecycle registry. - // The last service owner joins every worker before replacement. - std::shared_ptr stop_dispatcher_; - std::shared_ptr action_queue_; - std::unique_ptr - safety_participant_registration_; - }; -} - -#endif //GRPC_SYSTEM_SERVICE_H diff --git a/cmvr-es/service/grpc/server/include/media_activity_coordinator.h b/cmvr-es/service/grpc/server/include/media_activity_coordinator.h deleted file mode 100644 index b9f0c4d9..00000000 --- a/cmvr-es/service/grpc/server/include/media_activity_coordinator.h +++ /dev/null @@ -1,133 +0,0 @@ -#ifndef CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H -#define CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H - -#include -#include -#include -#include -#include -#include - -#include "service/grpc/stop_all/include/deferred_stop_operation.h" - -namespace cmvr::service { - -// Coordinates in-process media RPC activity with SystemService::StopAll. -// StopAll invalidates the current generation and waits for the affected RPCs -// to release their own device leases; it does not stop device lifecycles. -class MediaActivityCoordinator final { -private: - struct Impl; - struct SessionState; - -public: - using CancelCallback = std::function; - - struct StopAllTicket { - std::uint64_t generation{0}; - std::uint64_t ticket_id{0}; - - bool valid() const noexcept - { - return generation != 0U && ticket_id != 0U; - } - }; - - struct FinishStopAllResult { - bool ticket_consumed{false}; - bool participant_stopped{false}; - bool admission_resumed{false}; - }; - - class Session final { - public: - Session() = default; - ~Session(); - - Session(Session&& other) noexcept; - Session& operator=(Session&& other) noexcept; - - Session(const Session&) = delete; - Session& operator=(const Session&) = delete; - - explicit operator bool() const noexcept; - bool cancelled() const noexcept; - - // Linearizes a short device operation against beginStopAll(). If this - // returns false, StopAll won the race and the operation was not run. - bool runIfCurrent(const std::function& operation) const; - - // Claims an optional process-wide resource for this session. This is - // used by speaker input because one device cannot safely have two RPCs - // feeding and independently stopping the same streaming pipeline. - bool claimExclusiveResource(const std::string& resource_key); - - void reset() noexcept; - - private: - friend class MediaActivityCoordinator; - Session( - std::shared_ptr impl, - std::shared_ptr state); - - std::shared_ptr impl_; - std::shared_ptr state_; - }; - - MediaActivityCoordinator(); - ~MediaActivityCoordinator() = default; - - MediaActivityCoordinator(const MediaActivityCoordinator&) = delete; - MediaActivityCoordinator& operator=(const MediaActivityCoordinator&) = delete; - - // Returns an invalid session while a StopAll round is in progress. - Session beginSession(CancelCallback cancel = {}); - - // Pauses new sessions and invalidates all sessions from the previous - // generation. Cancellation callbacks are normally invoked before this - // returns. SystemService defers them until whole-machine motion stop - // requests have been issued, so a callback cannot delay physical stops. - // Concurrent callers join the same StopAll round. - StopAllTicket beginStopAll(bool defer_cancellation = false); - - // Collects one independently executable, at-most-once cancellation per - // invalidated session. Each operation captures SessionState ownership and - // can safely outlive this coordinator object without capturing `this`. - bool collectCancellationOperations( - const StopAllTicket& ticket, - std::vector& operations, - std::string* error = nullptr) const; - - // Legacy synchronous wrapper which serially executes the operations above. - // Returns false for a stale ticket or when any callback throws. - bool requestCancellation(const StopAllTicket& ticket); - - // Waits until every session invalidated by this ticket has run its cleanup - // and unregistered. A timeout leaves admission paused (fail closed). - bool waitForStopped( - const StopAllTicket& ticket, - std::chrono::milliseconds timeout); - - // Completes one StopAll participant. Admission resumes only after every - // participant succeeds and all invalidated sessions have exited. - bool finishStopAll( - const StopAllTicket& ticket, - bool all_media_stopped); - - FinishStopAllResult finishStopAllDetailed( - const StopAllTicket& ticket, - bool all_media_stopped); - - // Test/process teardown hook. Runtime recovery must use a new verified - // StopAll or Recover transaction instead of bypassing this latch. - void clearForTesting() noexcept; - -private: - std::shared_ptr impl_; -}; - -MediaActivityCoordinator& globalMediaActivityCoordinator(); - -} // namespace cmvr::service - -#endif // CMVR_ES_MEDIA_ACTIVITY_COORDINATOR_H diff --git a/cmvr-es/service/grpc/server/include/motor_activity_coordinator.h b/cmvr-es/service/grpc/server/include/motor_activity_coordinator.h deleted file mode 100644 index b3c14483..00000000 --- a/cmvr-es/service/grpc/server/include/motor_activity_coordinator.h +++ /dev/null @@ -1,159 +0,0 @@ -#ifndef CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H -#define CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H - -#include -#include -#include -#include -#include -#include -#include - -#include "service/grpc/stop_all/include/deferred_stop_operation.h" - -namespace cmvr::service { - -// Coordinates MotorService command dispatch with SystemService::StopAll. -// Registered controls expose only operational cancellation and quick-stop; -// this coordinator never invokes a MotorManager or device lifecycle method. -class MotorActivityCoordinator final { -private: - struct Impl; - -public: - using CancelCallback = std::function; - using QuickStopCallback = std::function; - using IdleCallback = std::function; - - struct StopAllTicket { - std::uint64_t generation{0}; - std::uint64_t ticket_id{0}; - - bool valid() const noexcept - { - return generation != 0U && ticket_id != 0U; - } - }; - - struct FinishStopAllResult { - bool ticket_consumed{false}; - bool participant_stopped{false}; - bool admission_resumed{false}; - }; - - class Registration final { - public: - Registration() = default; - ~Registration(); - - Registration(Registration&& other) noexcept; - Registration& operator=(Registration&& other) noexcept; - - Registration(const Registration&) = delete; - Registration& operator=(const Registration&) = delete; - - explicit operator bool() const noexcept; - void reset() noexcept; - - private: - friend class MotorActivityCoordinator; - Registration(std::shared_ptr impl, std::uint64_t id) noexcept; - - std::shared_ptr impl_; - std::uint64_t id_{0}; - }; - - class AdmissionGuard final { - public: - AdmissionGuard(AdmissionGuard&&) noexcept = default; - AdmissionGuard& operator=(AdmissionGuard&&) noexcept = default; - - AdmissionGuard(const AdmissionGuard&) = delete; - AdmissionGuard& operator=(const AdmissionGuard&) = delete; - - bool accepting() const noexcept { return accepting_; } - - private: - friend class MotorActivityCoordinator; - AdmissionGuard( - std::unique_lock&& lock, - bool accepting) noexcept; - - std::unique_lock lock_; - bool accepting_{false}; - }; - - MotorActivityCoordinator(); - ~MotorActivityCoordinator() = default; - - MotorActivityCoordinator(const MotorActivityCoordinator&) = delete; - MotorActivityCoordinator& operator=(const MotorActivityCoordinator&) = delete; - - Registration registerControl( - CancelCallback cancel, - QuickStopCallback quick_stop, - IdleCallback idle, - std::string description = {}); - - // Hold this guard until the MotorControlState has been marked busy. This - // makes final command admission atomic with beginStopAll(). - AdmissionGuard lockAdmission(); - - // Invalidates admission for every command from the preceding generation. - // With defer_callbacks=false, cancellation callbacks retain their legacy - // synchronous behavior. With true, callers must collect and execute every - // target operation; each operation orders cancellation before quick-stop. - StopAllTicket beginStopAll(bool defer_callbacks = false); - - // Collects one independently executable operation per registration that - // belonged to this StopAll round. Operations capture shared state rather - // than this coordinator and are idempotent, including their failure result. - bool collectStopOperations( - const StopAllTicket& ticket, - std::vector& operations, - std::string* error = nullptr) const; - - // Legacy synchronous wrapper which serially executes the operations above. - bool requestStop( - const StopAllTicket& ticket, - std::string* error = nullptr); - - // Waits for in-flight RPC/stream ownership captured by the round to be - // released after requestStop(). - bool waitForStopped( - const StopAllTicket& ticket, - std::chrono::milliseconds timeout, - std::string* error = nullptr); - - // Convenience operation for callers that do not need split-phase stop. - bool stopAndWait( - const StopAllTicket& ticket, - std::chrono::milliseconds timeout, - std::string* error = nullptr); - - // Admission resumes only when all registered controls confirmed their stop - // and every invalidated RPC/stream has exited. Failure remains fail-closed. - bool finishStopAll( - const StopAllTicket& ticket, - bool all_motors_stopped); - - FinishStopAllResult finishStopAllDetailed( - const StopAllTicket& ticket, - bool all_motors_stopped); - - // Wakes StopAll after a MotorControlState releases or changes ownership. - void notifyStateChanged() noexcept; - - // Test/process teardown hook. Runtime recovery must use another successful - // StopAll round instead of bypassing fail-closed state. - void clearForTesting() noexcept; - -private: - std::shared_ptr impl_; -}; - -MotorActivityCoordinator& globalMotorActivityCoordinator(); - -} // namespace cmvr::service - -#endif // CMVR_ES_MOTOR_ACTIVITY_COORDINATOR_H diff --git a/cmvr-es/service/grpc/server/src/camera_operational_activity_registry.cpp b/cmvr-es/service/grpc/server/src/camera_operational_activity_registry.cpp deleted file mode 100644 index 55607cb7..00000000 --- a/cmvr-es/service/grpc/server/src/camera_operational_activity_registry.cpp +++ /dev/null @@ -1,336 +0,0 @@ -#include "service/grpc/server/include/camera_operational_activity_registry.h" - -#include -#include - -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { -namespace { - -template -CameraOperationalActivityRegistry::DispatchResult dispatchIfAdmitted( - std::mutex& device_mutex, - const CameraOperationalActivityRegistry::DispatchFence& dispatch_fence, - Operation&& operation) -{ - std::uint64_t admitted_generation = 0U; - { - auto admission = globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting()) { - return CameraOperationalActivityRegistry::DispatchResult:: - RejectedByStopAll; - } - admitted_generation = admission.generation(); - } - - std::lock_guard dispatch_lock(device_mutex); - { - auto admission = globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting() || - admission.generation() != admitted_generation) { - return CameraOperationalActivityRegistry::DispatchResult:: - RejectedByStopAll; - } - } - - if (dispatch_fence && !dispatch_fence()) { - return CameraOperationalActivityRegistry::DispatchResult:: - RejectedByDispatchFence; - } - - return operation(); -} - -} // namespace - -std::shared_ptr -CameraOperationalActivityRegistry::stateForDevice( - const std::string& device_id, - const bool create) -{ - std::lock_guard lock(states_mutex_); - const auto existing = states_.find(device_id); - if (existing != states_.end()) { - return existing->second; - } - if (!create) { - return {}; - } - - auto state = std::make_shared(); - states_.emplace(device_id, state); - return state; -} - -CameraOperationalActivityRegistry::DispatchResult -CameraOperationalActivityRegistry::start( - const std::string& device_id, - const std::shared_ptr& camera, - ActivityToken* token, - DispatchFence dispatch_fence) -{ - if (token) { - *token = {}; - } - if (device_id.empty() || !camera) { - return DispatchResult::DeviceFailure; - } - - const auto state = stateForDevice(device_id, true); - return dispatchIfAdmitted(state->mutex, dispatch_fence, [&] { - const auto previous_camera = state->active_camera; - const bool was_active = state->active.load(std::memory_order_acquire); - if (was_active && previous_camera != camera) { - // A device id has one operational owner at a time. Replacing an - // active instance would make a token from either instance unable - // to roll back without risking the other camera. - return DispatchResult::DeviceFailure; - } - if (!previous_camera) { - // Allocate tracking before device I/O so a successful start always - // has a StopAll-visible owner. - state->active_camera = camera; - } - - bool started = false; - try { - started = camera->startOperationalActivity(); - } catch (...) { - state->active_camera = previous_camera; - state->active.store(was_active, std::memory_order_release); - throw; - } - if (!started) { - state->active_camera = previous_camera; - state->active.store(was_active, std::memory_order_release); - return DispatchResult::DeviceFailure; - } - - state->active_camera = camera; - state->active.store(true, std::memory_order_release); - ++state->activity_generation; - if (state->activity_generation == 0U) { - ++state->activity_generation; - } - if (token) { - token->device_id = device_id; - token->activity_generation = state->activity_generation; - token->owns_start = !was_active; - } - return DispatchResult::Success; - }); -} - -bool CameraOperationalActivityRegistry::stopIfCurrent( - const ActivityToken& token) -{ - if (!token.valid() || !token.owns_start) { - return true; - } - - const auto state = stateForDevice(token.device_id, false); - if (!state) { - return true; - } - - std::lock_guard dispatch_lock(state->mutex); - if (!state->active.load(std::memory_order_acquire) || - state->activity_generation != token.activity_generation || - !state->active_camera) { - return true; - } - - bool stopped = false; - try { - stopped = state->active_camera->stopOperationalActivity(); - } catch (...) { - stopped = false; - } - if (!stopped) { - return false; - } - - state->last_stopped_camera = state->active_camera; - state->active_camera.reset(); - state->active.store(false, std::memory_order_release); - return true; -} - -CameraOperationalActivityRegistry::DispatchResult -CameraOperationalActivityRegistry::stopLifecycle( - const std::string& device_id, - const std::shared_ptr& camera, - DispatchFence dispatch_fence) -{ - if (device_id.empty() || !camera) { - return DispatchResult::DeviceFailure; - } - - const auto state = stateForDevice(device_id, true); - return dispatchIfAdmitted(state->mutex, dispatch_fence, [&] { - if (!camera->stop()) { - return DispatchResult::DeviceFailure; - } - state->active_camera.reset(); - state->active.store(false, std::memory_order_release); - return DispatchResult::Success; - }); -} - -void CameraOperationalActivityRegistry::markCameraStopped( - const std::string& device_id) -{ - const auto state = stateForDevice(device_id, false); - if (!state) { - return; - } - std::lock_guard lock(state->mutex); - state->active_camera.reset(); - state->active.store(false, std::memory_order_release); -} - -bool CameraOperationalActivityRegistry::stopActivitiesForDevice( - const std::string& device_id, - std::vector* failures) -{ - return stopActivitiesForDevice(device_id, {}, failures); -} - -bool CameraOperationalActivityRegistry::stopActivitiesForDevice( - const std::string& device_id, - const std::shared_ptr& fallback_camera, - std::vector* failures) -{ - const auto state = stateForDevice( - device_id, static_cast(fallback_camera)); - if (!state) { - return true; - } - - std::lock_guard dispatch_lock(state->mutex); - const auto active_camera = state->active_camera; - const auto camera = active_camera ? active_camera : fallback_camera; - if (!camera) { - return true; - } - - std::uint64_t stop_generation = 0U; - { - const auto admission = globalStopAllAdmissionGate().lockAdmission(); - stop_generation = admission.generation(); - } - if (!active_camera && - state->last_stopped_generation == stop_generation && - state->last_stopped_camera.lock() == camera) { - return true; - } - - bool stopped = false; - std::string detail; - try { - stopped = camera->stopOperationalActivity(); - if (!stopped) { - detail = "operational camera stop was not confirmed"; - } - } catch (const std::exception& error) { - detail = std::string("operational camera stop threw: ") + error.what(); - } catch (...) { - detail = "operational camera stop threw an unknown exception"; - } - - if (stopped) { - if (active_camera) { - state->active_camera.reset(); - } - state->last_stopped_camera = camera; - state->last_stopped_generation = stop_generation; - state->active.store(false, std::memory_order_release); - return true; - } - if (failures) { - failures->push_back(device_id + ": " + detail); - } - return false; -} - -bool CameraOperationalActivityRegistry::stopAllActivities( - std::vector* failures) -{ - std::vector device_ids; - { - std::lock_guard lock(states_mutex_); - device_ids.reserve(states_.size()); - for (const auto& [device_id, state] : states_) { - (void)state; - device_ids.push_back(device_id); - } - } - - bool all_stopped = true; - for (const auto& device_id : device_ids) { - if (!stopActivitiesForDevice(device_id, failures)) { - all_stopped = false; - } - } - return all_stopped; -} - -std::size_t CameraOperationalActivityRegistry::activeCameraCount() const -{ - return activeDeviceIds().size(); -} - -std::vector -CameraOperationalActivityRegistry::trackedDeviceIds() const -{ - std::vector device_ids; - { - std::lock_guard lock(states_mutex_); - device_ids.reserve(states_.size()); - for (const auto& [device_id, state] : states_) { - (void)state; - device_ids.push_back(device_id); - } - } - std::sort(device_ids.begin(), device_ids.end()); - return device_ids; -} - -std::vector -CameraOperationalActivityRegistry::activeDeviceIds() const -{ - std::vector>> states; - { - std::lock_guard lock(states_mutex_); - states.reserve(states_.size()); - for (const auto& entry : states_) { - states.push_back(entry); - } - } - - std::vector device_ids; - device_ids.reserve(states.size()); - for (const auto& [device_id, state] : states) { - if (state->active.load(std::memory_order_acquire)) { - device_ids.push_back(device_id); - } - } - std::sort(device_ids.begin(), device_ids.end()); - return device_ids; -} - -void CameraOperationalActivityRegistry::clearForTesting() -{ - std::lock_guard lock(states_mutex_); - states_.clear(); -} - -CameraOperationalActivityRegistry& -globalCameraOperationalActivityRegistry() -{ - static CameraOperationalActivityRegistry registry; - return registry; -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/camera_ptz_activity_registry.cpp b/cmvr-es/service/grpc/server/src/camera_ptz_activity_registry.cpp deleted file mode 100644 index 609c0807..00000000 --- a/cmvr-es/service/grpc/server/src/camera_ptz_activity_registry.cpp +++ /dev/null @@ -1,233 +0,0 @@ -#include "service/grpc/server/include/camera_ptz_activity_registry.h" - -#include -#include -#include - -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { - -std::shared_ptr -CameraPtzActivityRegistry::stateForDevice( - const std::string& device_id, - const bool create) -{ - std::lock_guard lock(states_mutex_); - const auto existing = states_.find(device_id); - if (existing != states_.end()) { - return existing->second; - } - if (!create) { - return {}; - } - - auto state = std::make_shared(); - states_.emplace(device_id, state); - return state; -} - -CameraPtzActivityRegistry::DispatchResult -CameraPtzActivityRegistry::control( - const std::string& device_id, - const std::shared_ptr& camera, - const device::PtzCommand command, - const bool stop, - const int speed, - DispatchFence dispatch_fence) -{ - if (device_id.empty() || !camera) { - return DispatchResult::DeviceFailure; - } - - // START checks admission on both sides of the per-device dispatch queue. A - // STOP is a safety-lane operation and remains available while StopAll is - // latched; its Coordinator dispatch fence still runs under the same queue. - std::uint64_t admitted_generation = 0U; - if (!stop) { - auto admission = globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting()) { - return DispatchResult::RejectedByStopAll; - } - admitted_generation = admission.generation(); - } - - const auto state = stateForDevice(device_id, true); - std::lock_guard dispatch_lock(state->mutex); - if (!stop) { - auto admission = globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting() || - admission.generation() != admitted_generation) { - return DispatchResult::RejectedByStopAll; - } - } - - // The service-level safety transaction performs its final epoch, - // generation, and hardware-state validation here. Keeping the fence under - // the per-device lock prevents a request that waited in this queue from - // dispatching with a stale admission permit. - if (dispatch_fence && !dispatch_fence()) { - return DispatchResult::RejectedByDispatchFence; - } - - if (!camera->controlPtz(command, stop, speed)) { - return DispatchResult::DeviceFailure; - } - - if (!stop) { - state->commands[command] = {camera, speed}; - } else { - state->commands.erase(command); - } - state->active_command_count.store( - state->commands.size(), std::memory_order_release); - return DispatchResult::Success; -} - -void CameraPtzActivityRegistry::markCameraStopped( - const std::string& device_id) -{ - const auto state = stateForDevice(device_id, false); - if (!state) { - return; - } - std::lock_guard lock(state->mutex); - state->commands.clear(); - state->active_command_count.store(0U, std::memory_order_release); -} - -bool CameraPtzActivityRegistry::stopActivitiesForDevice( - const std::string& device_id, - std::vector* failures) -{ - const auto state = stateForDevice(device_id, false); - if (!state) { - return true; - } - - std::lock_guard dispatch_lock(state->mutex); - bool all_stopped = true; - for (auto command_it = state->commands.begin(); - command_it != state->commands.end();) { - bool stopped = false; - std::string detail; - try { - const auto& activity = command_it->second; - stopped = activity.camera && activity.camera->controlPtz( - command_it->first, true, activity.speed); - if (!stopped) { - detail = "PTZ stop was rejected by the camera"; - } - } catch (const std::exception& error) { - detail = std::string("PTZ stop threw: ") + error.what(); - } catch (...) { - detail = "PTZ stop threw an unknown exception"; - } - - if (stopped) { - command_it = state->commands.erase(command_it); - continue; - } - - all_stopped = false; - if (failures) { - failures->push_back(device_id + ": " + detail); - } - ++command_it; - } - state->active_command_count.store( - state->commands.size(), std::memory_order_release); - return all_stopped; -} - -bool CameraPtzActivityRegistry::stopAllActivities( - std::vector* failures) -{ - std::vector device_ids; - { - std::lock_guard lock(states_mutex_); - device_ids.reserve(states_.size()); - for (const auto& [device_id, state] : states_) { - (void)state; - device_ids.push_back(device_id); - } - } - - bool all_stopped = true; - for (const auto& device_id : device_ids) { - if (!stopActivitiesForDevice(device_id, failures)) { - all_stopped = false; - } - } - return all_stopped; -} - -std::size_t CameraPtzActivityRegistry::activeCommandCount() const -{ - std::vector> states; - { - std::lock_guard lock(states_mutex_); - states.reserve(states_.size()); - for (const auto& [device_id, state] : states_) { - (void)device_id; - states.push_back(state); - } - } - - std::size_t count = 0U; - for (const auto& state : states) { - count += state->active_command_count.load(std::memory_order_acquire); - } - return count; -} - -std::vector CameraPtzActivityRegistry::trackedDeviceIds() const -{ - std::vector device_ids; - { - std::lock_guard lock(states_mutex_); - device_ids.reserve(states_.size()); - for (const auto& [device_id, state] : states_) { - (void)state; - device_ids.push_back(device_id); - } - } - std::sort(device_ids.begin(), device_ids.end()); - return device_ids; -} - -std::vector CameraPtzActivityRegistry::activeDeviceIds() const -{ - std::vector>> states; - { - std::lock_guard lock(states_mutex_); - states.reserve(states_.size()); - for (const auto& entry : states_) { - states.push_back(entry); - } - } - - std::vector device_ids; - device_ids.reserve(states.size()); - for (const auto& [device_id, state] : states) { - if (state->active_command_count.load(std::memory_order_acquire) != 0U) { - device_ids.push_back(device_id); - } - } - std::sort(device_ids.begin(), device_ids.end()); - return device_ids; -} - -void CameraPtzActivityRegistry::clearForTesting() -{ - std::lock_guard lock(states_mutex_); - states_.clear(); -} - -CameraPtzActivityRegistry& globalCameraPtzActivityRegistry() -{ - static CameraPtzActivityRegistry registry; - return registry; -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp deleted file mode 100644 index 319b9455..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_agv_service.cpp +++ /dev/null @@ -1,1321 +0,0 @@ -#include "service/grpc/server/include/grpc_agv_service.h" - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "common/base/logging/logger.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" -#include "service/grpc/server/include/grpc_command_transaction.h" -#include "service/grpc/server/include/grpc_security.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.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::AgvResult& result) -{ - if (result.ok()) { - return grpc::Status::OK; - } - switch (result.code) { - case device::AgvErrorCode::InvalidArgument: - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - result.message); - case device::AgvErrorCode::TaskCanceled: - return grpc::Status( - grpc::StatusCode::CANCELLED, - result.message); - case device::AgvErrorCode::Timeout: - return grpc::Status( - grpc::StatusCode::DEADLINE_EXCEEDED, - result.message); - default: - 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); -} - -grpc::Status setControlLeaseConflict( - api::CommandHeader_Feedback* response, - const std::string& device_id, - const std::string& detail) -{ - const std::string message = - "AGV control is leased by another active control operation: " + - device_id; - if (!detail.empty()) { - CMVR_LOG(WARNING) << "[gRPCAgvServiceImpl] control lease conflict, id=" - << device_id << ", detail=" << detail; - } - fillFeedback(response, false, message); - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, message); -} - -template -grpc::Status setControlLeaseConflict( - Response* response, - const std::string& device_id, - const std::string& detail) -{ - return setControlLeaseConflict( - response->mutable_header(), device_id, detail); -} - -grpc::Status setStopAllRejected( - api::CommandHeader_Feedback* response, - const std::string& device_id) -{ - const std::string message = - "AGV control is temporarily paused by StopAll: " + device_id; - fillFeedback(response, false, message); - return grpc::Status(grpc::StatusCode::UNAVAILABLE, message); -} - -template -grpc::Status setStopAllRejected( - Response* response, - const std::string& device_id) -{ - return setStopAllRejected(response->mutable_header(), device_id); -} - -class ScopedUnaryAgvControlLease final { -public: - ScopedUnaryAgvControlLease( - const std::string& device_id, - const char* operation, - const bool preemptive = false) - : manager_(control::ControlAuthorityManager::instance()) - { - static std::atomic sequence{0}; - const std::string owner = - std::string(preemptive - ? "grpc-agv-safety:" - : "grpc-agv-unary:") + - operation + ":" + - std::to_string( - sequence.fetch_add( - 1U, std::memory_order_relaxed) + - 1U); - const auto ttl = std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>( - std::chrono::hours(24)); - control::ControlAcquireResult acquired; - if (preemptive) { - acquired = manager_.preemptAcquire(device_id, owner, ttl); - } else { - auto admission = - globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting()) { - rejected_by_stop_all_ = true; - detail_ = "System StopAll admission is closed"; - return; - } - admission_generation_ = admission.generation(); - acquired = manager_.tryAcquire(device_id, owner, ttl); - } - acquired_ = acquired.acquired; - token_ = std::move(acquired.token); - detail_ = std::move(acquired.detail); - release_on_destroy_ = !preemptive; - } - - ~ScopedUnaryAgvControlLease() - { - if (release_on_destroy_) { - manager_.release(token_); - } else if (acquired_) { - (void)manager_.retireSafetyHolder(token_); - } - } - - bool acquired() const noexcept { return acquired_; } - bool rejectedByStopAll() const noexcept - { - return rejected_by_stop_all_; - } - const std::string& detail() const noexcept { return detail_; } - - bool waitForPreemptedRelease( - const control::ControlAuthorityManager::Duration timeout) - { - return manager_.waitForPreemptedRelease(token_, timeout); - } - - bool admissionCurrent() const - { - auto admission = globalStopAllAdmissionGate().lockAdmission(); - return admission.accepting() && - admission.generation() == admission_generation_; - } - - bool current() const - { - return manager_.validate(token_) && admissionCurrent(); - } - - std::function cancellationRequested( - grpc::ServerContext* context) const - { - const auto token = token_; - const auto admission_generation = admission_generation_; - return [context, token, admission_generation]() { - try { - if ((context && context->IsCancelled()) || - !control::ControlAuthorityManager::instance() - .validate(token)) { - return true; - } - auto admission = - globalStopAllAdmissionGate().lockAdmission(); - return !admission.accepting() || - admission.generation() != admission_generation; - } catch (...) { - return true; - } - }; - } - - control::ControlDispatchGuard tryBeginDispatch() - { - auto admission = globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting() || - admission.generation() != admission_generation_) { - return {}; - } - return manager_.tryBeginDispatch(token_); - } - - void confirmSafeToRelease() noexcept { release_on_destroy_ = true; } - -private: - control::ControlAuthorityManager& manager_; - control::ControlLeaseToken token_; - std::string detail_; - bool acquired_{false}; - bool release_on_destroy_{true}; - bool rejected_by_stop_all_{false}; - std::uint64_t admission_generation_{0U}; -}; - -template -grpc::Status setControlAdmissionFailure( - Response* response, - const std::string& device_id, - const ScopedUnaryAgvControlLease& lease) -{ - return lease.rejectedByStopAll() - ? setStopAllRejected(response, device_id) - : setControlLeaseConflict(response, device_id, lease.detail()); -} - -template -grpc::Status setControlDispatchFailure( - Response* response, - const std::string& device_id, - const ScopedUnaryAgvControlLease& lease, - const char* operation) -{ - if (!lease.admissionCurrent()) { - return setStopAllRejected(response, device_id); - } - return setControlLeaseConflict( - response, - device_id, - std::string("control lease was preempted before ") + operation + - " dispatch"); -} - -template -grpc::Status executeConfirmedAgvStop( - Response* response, - const std::shared_ptr& agv, - ScopedUnaryAgvControlLease& control_barrier, - const char* operation_name, - Operation&& operation) -{ - try { - const auto initial_stop = operation(); - if (!initial_stop.ok()) { - return setResponseResult(response, initial_stop); - } - - constexpr auto handler_release_timeout = std::chrono::seconds(15); - if (!control_barrier.waitForPreemptedRelease( - std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>( - handler_release_timeout))) { - return setResponseResult( - response, - device::AgvResult::failure( - device::AgvErrorCode::Timeout, - std::string(operation_name) + - " timed out waiting for the preempted control handler to exit")); - } - - const auto final_stop = operation(); - if (!final_stop.ok()) { - return setResponseResult(response, final_stop); - } - - const auto stopped = agv->confirmMotionStopped(); - if (!stopped.ok()) { - std::string message = std::string(operation_name) + - " did not reach a confirmed stopped state: " + - stopped.message; - return setResponseResult( - response, - device::AgvResult::failure(stopped.code, message)); - } - control_barrier.confirmSafeToRelease(); - return setResponseResult(response, final_stop); - } catch (...) { - throw; - } -} - -template -grpc::Status setNavigationRequestCanceled(Response* response) -{ - constexpr char message[] = - "AGV navigation request was canceled before command dispatch"; - fillFeedback(response->mutable_header(), false, message); - return grpc::Status(grpc::StatusCode::CANCELLED, 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, - std::function cancellation_requested = {}) -{ - 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(); - dst.wait_timeout_ms = src.wait_timeout_ms(); - dst.poll_interval_ms = src.poll_interval_ms(); - dst.cancellation_requested = std::move(cancellation_requested); - return dst; -} - -device::AgvVelocity toVelocity(const msgs::AgvVelocity& src) -{ - return {src.vx(), src.vy(), src.wz()}; -} - -device::AgvTranslation toTranslation(const msgs::AgvTranslation& src) -{ - return { - src.distance(), - src.vx(), - src.vy(), - static_cast(src.mode()) - }; -} - -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() - : gRPCAgvServiceImpl(makeDefaultGrpcSecurityGateway()) -{ -} - -gRPCAgvServiceImpl::gRPCAgvServiceImpl( - std::shared_ptr security_gateway) - : dmgr_(device::DeviceManager::getInstance()), - security_gateway_(security_gateway - ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()) -{ -} - -grpc::Status gRPCAgvServiceImpl::getRuntimeState(grpc::ServerContext* context, - const api::AgvRuntimeStateCommand_Request* request, - api::AgvRuntimeStateCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.AgvService/getRuntimeState"); - 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* context, - const api::AgvNavigationStatusCommand_Request* request, - api::AgvNavigationStatusCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.AgvService/getNavigationStatus"); - 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* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/emergencyStop", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_barrier( - device_id, "emergencyStop", true); - if (!control_barrier.acquired()) { - return setControlLeaseConflict( - response, device_id, control_barrier.detail()); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return executeConfirmedAgvStop( - response, agv, control_barrier, "emergencyStop", - [&agv]() { return agv->emergencyStop(); }); - }); -} - -grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/clearFault", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "clearFault"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "clearFault"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult(response, agv->clearFault()); - }); -} - -grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context, - const api::AgvNavigateToPoseCommand_Request* request, - api::AgvNavigateToPoseCommand_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/navigateToPose", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - if (context && context->IsCancelled()) { - return setNavigationRequestCanceled(response); - } - const std::string device_id = request->header().device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "navigateToPose"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "navigateToPose"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult(response, agv->navigateToPose( - toPose2d(request->pose()), - toMotionOptions( - request->options(), - control_lease.cancellationRequested(context)), - toAdapterParams(request->adapter_params()))); - }); -} - -grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context, - const api::AgvNavigateToStationCommand_Request* request, - api::AgvNavigateToStationCommand_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/navigateToStation", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - if (context && context->IsCancelled()) { - return setNavigationRequestCanceled(response); - } - const std::string device_id = request->header().device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "navigateToStation"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, - "navigateToStation"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult(response, agv->navigateToStation( - request->station_id(), - toMotionOptions( - request->options(), - control_lease.cancellationRequested(context)), - toAdapterParams(request->adapter_params()))); - }); -} - -grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, - const api::AgvFollowPathCommand_Request* request, - api::AgvFollowPathCommand_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/followPath", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - if (context && context->IsCancelled()) { - return setNavigationRequestCanceled(response); - } - const std::string device_id = request->header().device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "followPath"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "followPath"); - } - std::vector path; - path.reserve(static_cast(request->path_size())); - for (const auto& segment : request->path()) { - path.push_back(toPathSegment(segment)); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult( - response, - agv->followPath( - path, - toMotionOptions( - request->options(), - control_lease.cancellationRequested(context)))); - }); -} - - - -grpc::Status gRPCAgvServiceImpl::translate( - grpc::ServerContext* context, - const api::AgvTranslateCommand_Request* request, - api::AgvTranslateCommand_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/translate", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - if (context && context->IsCancelled()) { - return setNavigationRequestCanceled(response); - } - const std::string device_id = request->header().device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "translate"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "translate"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult( - response, - agv->translate(toTranslation(request->translation()))); - }); -} - - - -grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/pauseNavigation", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "pauseNavigation"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, - "pauseNavigation"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult(response, agv->pauseNavigation()); - }); -} - -grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/resumeNavigation", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "resumeNavigation"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, - "resumeNavigation"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult(response, agv->resumeNavigation()); - }); -} - -grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/cancelNavigation", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_barrier( - device_id, "cancelNavigation", true); - if (!control_barrier.acquired()) { - return setControlLeaseConflict( - response, device_id, control_barrier.detail()); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return executeConfirmedAgvStop( - response, agv, control_barrier, "cancelNavigation", - [&agv]() { return agv->cancelNavigation(); }); - }); -} - -grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext* context, - const api::AgvSetVelocityCommand_Request* request, - api::AgvSetVelocityCommand_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/setVelocity", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "setVelocity"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "setVelocity"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult(response, agv->setVelocity(toVelocity(request->velocity()))); - }); -} - -grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/stopVelocityControl", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_barrier( - device_id, "stopVelocityControl", true); - if (!control_barrier.acquired()) { - return setControlLeaseConflict( - response, device_id, control_barrier.detail()); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return executeConfirmedAgvStop( - response, agv, control_barrier, "stopVelocityControl", - [&agv]() { return agv->stopVelocityControl(); }); - }); -} - -grpc::Status gRPCAgvServiceImpl::listMaps(grpc::ServerContext* context, - const api::AgvListMapsCommand_Request* request, - api::AgvListMapsCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.AgvService/listMaps"); - 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* context, - const api::AgvListStationsCommand_Request* request, - api::AgvListStationsCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.AgvService/listStations"); - 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* context, - const api::AgvMapCommand_Request* request, - api::AgvMapCommand_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/switchMap", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "switchMap"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "switchMap"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult(response, agv->switchMap(request->map_name())); - }); -} - -grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext* context, - const api::AgvMapCommand_Request* request, - api::AgvMapCommand_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/uploadMap", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "uploadMap"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "uploadMap"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult(response, agv->uploadMap(request->map_name(), request->content())); - }); -} - -grpc::Status gRPCAgvServiceImpl::downloadMap(grpc::ServerContext* context, - const api::AgvMapCommand_Request* request, - api::AgvMapCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.AgvService/downloadMap"); - 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* context, - const api::AgvStartMappingCommand_Request* request, - api::AgvStartMappingCommand_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/startMapping", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "startMapping"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "startMapping"); - } - device::AgvMappingOptions options; - options.dimension = toMapDimension(request->dimension()); - options.map_name = request->map_name(); - options.real_time = request->real_time(); - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - const auto result = agv->startMapping(options); - if (result.ok()) { - response->set_session_id(device_id + "_mapping"); - } - return setResponseResult(response, result); - }); -} - -grpc::Status gRPCAgvServiceImpl::streamMap(grpc::ServerContext* context, - const api::AgvMapStreamCommand_Request* request, - grpc::ServerWriter* writer) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.AgvService/streamMap"); - 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* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.AgvService/stopMapping", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto agv = dmgr_.getDevice(device_id); - if (!agv) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryAgvControlLease control_lease( - device_id, "stopMapping"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "stopMapping"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - return setResponseResult(response, agv->stopMapping()); - }); -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/server/src/grpc_arm_service.cpp deleted file mode 100644 index 67250253..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_arm_service.cpp +++ /dev/null @@ -1,1046 +0,0 @@ -#include "service/grpc/server/include/grpc_arm_service.h" - -#include -#include -#include - -#include - -#include "common/base/logging/logger.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" -#include "service/grpc/server/include/grpc_command_transaction.h" -#include "service/grpc/server/include/grpc_security.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.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, - std::function cancellation_requested = {}) -{ - 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(); - dst.cancellation_requested = std::move(cancellation_requested); - 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); -} - -grpc::Status setStopAllRejected( - api::CommandHeader_Feedback* response, - const std::string& device_id) -{ - const std::string message = - "RobotArm control is temporarily paused by StopAll: " + device_id; - fillFeedback(response, false, message); - return grpc::Status(grpc::StatusCode::UNAVAILABLE, message); -} - -template -grpc::Status setStopAllRejected( - Response* response, - const std::string& device_id) -{ - return setStopAllRejected(response->mutable_header(), device_id); -} - -grpc::Status setControlLeaseConflict( - api::CommandHeader_Feedback* response, - const std::string& device_id, - const std::string& detail) -{ - std::string message = - "RobotArm control is leased by another active control operation: " + - device_id; - if (!detail.empty()) { - CMVR_LOG(WARNING) << "[gRPCArmServiceImpl] control lease conflict, id=" - << device_id << ", detail=" << detail; - } - fillFeedback(response, false, message); - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, message); -} - -template -grpc::Status setControlLeaseConflict( - Response* response, - const std::string& device_id, - const std::string& detail) -{ - return setControlLeaseConflict( - response->mutable_header(), device_id, detail); -} - -grpc::Status setControlCancelled( - api::CommandHeader_Feedback* response, - const std::string& device_id, - const std::string& reason) -{ - std::string message = "RobotArm control was cancelled: " + device_id; - if (!reason.empty()) { - message += ", " + reason; - } - fillFeedback(response, false, message); - return grpc::Status(grpc::StatusCode::CANCELLED, message); -} - -class ScopedUnaryControlLease final { -public: - ScopedUnaryControlLease( - const std::string& device_id, - const char* operation, - const bool preemptive = false) - : manager_(control::ControlAuthorityManager::instance()) - { - static std::atomic sequence{0}; - const std::string owner = - std::string("grpc-arm-unary:") + operation + ":" + - std::to_string( - sequence.fetch_add( - 1U, std::memory_order_relaxed) + - 1U); - const auto ttl = std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>( - std::chrono::hours(24)); - control::ControlAcquireResult acquired; - if (preemptive) { - acquired = manager_.preemptAcquire(device_id, owner, ttl); - } else { - auto admission = - globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting()) { - rejected_by_stop_all_ = true; - detail_ = "System StopAll admission is closed"; - return; - } - admission_generation_ = admission.generation(); - acquired = manager_.tryAcquire(device_id, owner, ttl); - } - acquired_ = acquired.acquired; - token_ = std::move(acquired.token); - detail_ = std::move(acquired.detail); - release_on_destroy_ = !preemptive; - } - - ~ScopedUnaryControlLease() - { - if (release_on_destroy_) { - manager_.release(token_); - } else if (acquired_) { - (void)manager_.retireSafetyHolder(token_); - } - } - - bool acquired() const noexcept { return acquired_; } - bool rejectedByStopAll() const noexcept - { - return rejected_by_stop_all_; - } - const std::string& detail() const noexcept { return detail_; } - void confirmSafeToRelease() noexcept { release_on_destroy_ = true; } - - bool waitForPreemptedRelease( - const control::ControlAuthorityManager::Duration timeout) - { - return manager_.waitForPreemptedRelease(token_, timeout); - } - - bool admissionCurrent() const - { - auto admission = globalStopAllAdmissionGate().lockAdmission(); - return admission.accepting() && - admission.generation() == admission_generation_; - } - - bool current() const - { - return manager_.validate(token_) && admissionCurrent(); - } - - std::function cancellationRequested( - grpc::ServerContext* context) const - { - const auto token = token_; - const auto admission_generation = admission_generation_; - return [context, token, admission_generation]() { - try { - if ((context && context->IsCancelled()) || - !control::ControlAuthorityManager::instance() - .validate(token)) { - return true; - } - auto admission = - globalStopAllAdmissionGate().lockAdmission(); - return !admission.accepting() || - admission.generation() != admission_generation; - } catch (...) { - return true; - } - }; - } - - control::ControlDispatchGuard tryBeginDispatch() - { - auto admission = globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting() || - admission.generation() != admission_generation_) { - return {}; - } - return manager_.tryBeginDispatch(token_); - } - -private: - control::ControlAuthorityManager& manager_; - control::ControlLeaseToken token_; - std::string detail_; - bool acquired_{false}; - bool release_on_destroy_{true}; - bool rejected_by_stop_all_{false}; - std::uint64_t admission_generation_{0U}; -}; - -template -device::Result executeConfirmedArmStop( - ScopedUnaryControlLease& control_barrier, - const char* operation_name, - Operation&& operation) -{ - const auto initial_stop = operation(); - if (!initial_stop.ok()) { - return initial_stop; - } - - constexpr auto handler_release_timeout = std::chrono::seconds(15); - if (!control_barrier.waitForPreemptedRelease( - std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>( - handler_release_timeout))) { - return device::Result::failure( - device::ArmErrorCode::Timeout, - std::string(operation_name) + - " timed out waiting for the preempted control handler to exit"); - } - - const auto final_stop = operation(); - if (final_stop.ok()) { - control_barrier.confirmSafeToRelease(); - } - return final_stop; -} - -template -grpc::Status setControlAdmissionFailure( - Response* response, - const std::string& device_id, - const ScopedUnaryControlLease& lease) -{ - return lease.rejectedByStopAll() - ? setStopAllRejected(response, device_id) - : setControlLeaseConflict(response, device_id, lease.detail()); -} - -template -grpc::Status setControlDispatchFailure( - Response* response, - const std::string& device_id, - const ScopedUnaryControlLease& lease, - const char* operation) -{ - if (!lease.admissionCurrent()) { - return setStopAllRejected(response, device_id); - } - return setControlLeaseConflict( - response, - device_id, - std::string("control lease was preempted before ") + operation + - " dispatch"); -} - -} // namespace - -gRPCArmServiceImpl::gRPCArmServiceImpl() - : gRPCArmServiceImpl(makeDefaultGrpcSecurityGateway()) -{ -} - -gRPCArmServiceImpl::gRPCArmServiceImpl( - std::shared_ptr security_gateway) - : dmgr_(device::DeviceManager::getInstance()), - security_gateway_(security_gateway - ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()) -{ -} - -grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/torqueOff", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryControlLease control_barrier( - device_id, "torqueOff", true); - if (!control_barrier.acquired()) { - return setControlLeaseConflict( - response, device_id, control_barrier.detail()); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - const auto result = executeConfirmedArmStop( - control_barrier, - "torqueOff", - [&arm]() { return arm->torqueOff(); }); - fillFeedback(response, result.ok(), result.ok() ? "" : result.message); - if (result.ok()) { - logRpcSuccess("torqueOff", device_id); - } - return resultToStatus(result); - }); -} - -grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/torqueOn", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryControlLease control_lease( - device_id, "torqueOn"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "torqueOn"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - const auto cancellation_requested = - control_lease.cancellationRequested(context); - const auto result = arm->torqueOn(cancellation_requested); - const bool control_current = control_lease.current(); - const bool cancellation_result = - result.ok() || - result.code == device::ArmErrorCode::CommandRejected; - if (cancellation_result && - !control_lease.admissionCurrent()) { - return setStopAllRejected(response, device_id); - } - const bool rpc_cancelled = context && context->IsCancelled(); - if (cancellation_result && - (rpc_cancelled || !control_current)) { - return setControlCancelled( - response, - device_id, - rpc_cancelled - ? "the RPC was cancelled" - : "control ownership was revoked"); - } - fillFeedback(response, result.ok(), result.ok() ? "" : result.message); - if (result.ok()) { - logRpcSuccess("torqueOn", device_id); - } - return resultToStatus(result); - }); -} - -grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext* context, - const api::MoveJ_Request* request, - api::MoveJ_Response* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/moveJ", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryControlLease control_lease( - device_id, "moveJ"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "moveJ"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - auto options = toMotionOptions( - request->options(), - control_lease.cancellationRequested(context)); - const auto result = arm->moveJ( - toJointPositionCommand(request->target()), options); - if (result.ok()) { - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id - << ", positions=" << request->target().position_size(); - } - return setResponseResult(response, result); - }); -} - -grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context, - const api::MoveL_Request* request, - api::MoveL_Response* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/moveL", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryControlLease control_lease( - device_id, "moveL"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "moveL"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - auto options = toMotionOptions( - request->options(), - control_lease.cancellationRequested(context)); - const auto result = arm->moveL( - toCartesianPose(request->target()), - options, - toFrameType(request->frame())); - if (result.ok()) { - CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id - << ", frame=" << request->frame(); - } - return setResponseResult(response, result); - }); -} - -grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext* context, - const api::SpeedJ_Request* request, - api::SpeedJ_Response* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/speedJ", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryControlLease control_lease( - device_id, "speedJ"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "speedJ"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - 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); - }); -} - -grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext* context, - const api::SpeedL_Request* request, - api::SpeedL_Response* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/speedL", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryControlLease control_lease( - device_id, "speedL"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "speedL"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - 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); - }); -} - -grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext* context, - const api::ServoJ_Request* request, - api::ServoJ_Response* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/servoJ", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryControlLease control_lease( - device_id, "servoJ"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "servoJ"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - 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); - }); -} - -grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext* context, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/stopMotion", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryControlLease control_barrier( - device_id, "stopMotion", true); - if (!control_barrier.acquired()) { - return setControlLeaseConflict( - response, device_id, control_barrier.detail()); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - const auto result = executeConfirmedArmStop( - control_barrier, - "stopMotion", - [&arm]() { return arm->stopMotion(); }); - fillFeedback(response, result.ok(), result.ok() ? "" : result.message); - if (result.ok()) { - logRpcSuccess("stopMotion", device_id); - } - return resultToStatus(result); - }); -} - -grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext* context, - const api::JointRequest* request, - api::JointResponse* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.ArmService/getJointState"); - 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* context, - const api::GetRobotState_Request* request, - api::GetRobotState_Response* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.ArmService/getRobotState"); - 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* context, - const api::GetPose_Request* request, - api::GetPose_Response* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.ArmService/getPose"); - 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* context, - const api::CalibrateZeroQ_Request* request, - api::CalibrateZeroQ_Response* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/calibrateZeroQ", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryControlLease control_lease( - device_id, "calibrateZeroQ"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "calibrateZeroQ"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - 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); - }); -} - -grpc::Status gRPCArmServiceImpl::getPoseMatrix(grpc::ServerContext* context, - const api::GetPoseMatrix_Request*, - api::GetPoseMatrix_Response* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.ArmService/getPoseMatrix"); - 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* context, - const api::ComputeForwardKinematics_Request*, - api::ComputeForwardKinematics_Response* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.ArmService/computeForwardKinematics"); - fillFeedback(response->mutable_header(), false, "computeForwardKinematics is not implemented"); - return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "computeForwardKinematics is not implemented"); -} - -grpc::Status gRPCArmServiceImpl::ExecuteJsonCommand( - grpc::ServerContext* context, - const api::JsonDeviceCommand_Request* request, - api::JsonDeviceCommand_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/ExecuteJsonCommand", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->header().device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - fillFeedback( - response->mutable_header(), - false, - "Device not found: " + device_id); - return grpc::Status::OK; - } - - ScopedUnaryControlLease control_lease( - device_id, "ExecuteJsonCommand"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, - "ExecuteJsonCommand"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - - std::string response_json; - const bool success = arm->executeJsonCommand( - request->request_json(), response_json); - fillFeedback( - response->mutable_header(), - success, - success ? "" : response_json); - response->set_response_json(response_json); - if (success) { - logRpcSuccess("ExecuteJsonCommand", device_id); - } - return grpc::Status::OK; - }); -} - -grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, - const cmvr::api::CommandHeader_Request *request, - cmvr::api::CommandHeader_Feedback *response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.ArmService/clearFault", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const std::string device_id = request->device_id(); - auto arm = dmgr_.getDevice(device_id); - if (!arm) { - return setDeviceNotFound(response, device_id); - } - ScopedUnaryControlLease control_lease( - device_id, "clearFault"); - if (!control_lease.acquired()) { - return setControlAdmissionFailure( - response, device_id, control_lease); - } - auto dispatch = control_lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - return setControlDispatchFailure( - response, device_id, control_lease, "clearFault"); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - const auto result = arm->clearFault(); - fillFeedback(response, result.ok(), result.ok() ? "" : result.message); - return resultToStatus(result); - }); -} -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_arm_teleop_service.cpp b/cmvr-es/service/grpc/server/src/grpc_arm_teleop_service.cpp deleted file mode 100644 index b2f5d809..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_arm_teleop_service.cpp +++ /dev/null @@ -1,1227 +0,0 @@ -#include "service/grpc/server/include/grpc_arm_teleop_service.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" -#include "service/grpc/server/include/grpc_command_transaction.h" -#include "service/grpc/server/include/grpc_security.h" - -namespace cmvr::service { - -namespace { - -using Clock = std::chrono::steady_clock; - -constexpr std::uint32_t kProtocolMajor = 1; -constexpr std::uint32_t kProtocolMinor = 0; -constexpr std::uint32_t kDefaultWatchdogMs = 250; -constexpr std::uint32_t kMinimumWatchdogMs = 20; -constexpr std::uint32_t kMaximumWatchdogMs = 60000; -constexpr std::uint32_t kDefaultLeaseMs = 10000; -constexpr std::uint32_t kMaximumLeaseMs = 600000; -constexpr std::uint32_t kMaximumRateHz = 1000; -constexpr std::size_t kMaximumJoints = 64; -constexpr std::size_t kMaximumIdentifierLength = 128; -constexpr auto kLoopSlice = std::chrono::milliseconds(5); -constexpr auto kReaderJoinGrace = std::chrono::milliseconds(50); - -std::atomic g_session_sequence{0}; - -class ScopeExit final { -public: - explicit ScopeExit(std::function callback) - : callback_(std::move(callback)) - { - } - - ~ScopeExit() noexcept - { - if (!callback_) { - return; - } - try { - callback_(); - } catch (...) { - } - } - - void release() noexcept { callback_ = {}; } - -private: - std::function callback_; -}; - -class DisabledArmTeleopBackend final : public ArmTeleopBackend { -public: - bool available() const noexcept override { return false; } - - arm_teleop::RobotManifest manifest() const override { return {}; } - - bool supportsForceFeedback() const noexcept override { return false; } - - ArmTeleopBackendResult open( - const arm_teleop::OpenSession&) override - { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - "arm teleoperation backend is disabled"); - } - - ArmTeleopBackendResult applySetpoint( - const arm_teleop::JointSetpoint&, - std::chrono::steady_clock::time_point) override - { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - "arm teleoperation backend is disabled"); - } - - ArmTeleopBackendResult stop( - const arm_teleop::StopReason, - const std::string&) override - { - return ArmTeleopBackendResult::ok(); - } - - ArmTeleopBackendSnapshot snapshot() const override - { - ArmTeleopBackendSnapshot result; - result.safety.set_connected(false); - result.safety.set_powered_on(false); - result.safety.set_fault(true); - result.safety.set_fault_detail( - "arm teleoperation backend is disabled"); - return result; - } -}; - -struct NegotiatedOpen { - std::uint32_t watchdog_ms{kDefaultWatchdogMs}; - std::uint32_t lease_ms{kDefaultLeaseMs}; -}; - -struct SessionRuntime { - std::string session_id; - std::uint64_t received_sequence{0}; - std::uint64_t applied_sequence{0}; - std::uint64_t dropped_setpoints{0}; - std::uint64_t rejected_setpoints{0}; - std::uint32_t watchdog_ms{kDefaultWatchdogMs}; - std::uint32_t lease_ms{kDefaultLeaseMs}; - Clock::time_point lease_deadline{}; -}; - -struct PendingFrame { - arm_teleop::ClientFrame frame; - Clock::time_point arrived{}; - std::uint64_t ordinal{0}; -}; - -bool isHexDigest(const std::string& value) -{ - return value.size() == 64 && - std::all_of(value.begin(), value.end(), [](const unsigned char ch) { - return std::isxdigit(ch) != 0; - }); -} - -grpc::Status validateManifestSyntax( - const arm_teleop::RobotManifest& manifest) -{ - if (manifest.robot_id().empty() || - manifest.robot_id().size() > kMaximumIdentifierLength) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "expected_robot.robot_id is required and must not exceed 128 bytes"); - } - if (!isHexDigest(manifest.model_sha256())) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "expected_robot.model_sha256 must contain 64 hexadecimal characters"); - } - if (!isHexDigest(manifest.calibration_sha256())) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "expected_robot.calibration_sha256 must contain 64 hexadecimal characters"); - } - if (manifest.joint_names_size() == 0 || - manifest.joint_names_size() > - static_cast(kMaximumJoints)) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "expected_robot.joint_names must contain between 1 and 64 joints"); - } - - std::unordered_set joint_names; - joint_names.reserve( - static_cast(manifest.joint_names_size())); - for (const auto& joint_name : manifest.joint_names()) { - if (joint_name.empty() || - joint_name.size() > kMaximumIdentifierLength || - !joint_names.insert(joint_name).second) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "expected_robot.joint_names must be non-empty and unique"); - } - } - if (manifest.position_unit().empty() || - manifest.velocity_unit().empty() || - manifest.effort_unit().empty()) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "expected_robot position, velocity, and effort units are required"); - } - if (manifest.base_frame().empty() || manifest.tool_frame().empty()) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "expected_robot base_frame and tool_frame are required"); - } - return grpc::Status::OK; -} - -grpc::Status validateOpen( - const arm_teleop::OpenSession& open, - NegotiatedOpen& negotiated) -{ - if (open.protocol_major() != kProtocolMajor) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "unsupported arm teleoperation protocol major"); - } - if (open.protocol_minor() > kProtocolMinor) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "unsupported arm teleoperation protocol minor"); - } - if (open.client_instance_id().empty() || - open.client_instance_id().size() > kMaximumIdentifierLength) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "client_instance_id is required and must not exceed 128 bytes"); - } - - const auto manifest_status = - validateManifestSyntax(open.expected_robot()); - if (!manifest_status.ok()) { - return manifest_status; - } - - if (open.requested_command_rate_hz() == 0 || - open.requested_command_rate_hz() > kMaximumRateHz) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "requested_command_rate_hz must be in [1, 1000]"); - } - if (open.requested_state_rate_hz() == 0 || - open.requested_state_rate_hz() > kMaximumRateHz) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "requested_state_rate_hz must be in [1, 1000]"); - } - - negotiated.watchdog_ms = - open.watchdog_timeout_ms() == 0 - ? kDefaultWatchdogMs - : open.watchdog_timeout_ms(); - if (negotiated.watchdog_ms < kMinimumWatchdogMs || - negotiated.watchdog_ms > kMaximumWatchdogMs) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "watchdog_timeout_ms must be zero or in [20, 60000]"); - } - - negotiated.lease_ms = - open.requested_lease_ms() == 0 - ? kDefaultLeaseMs - : open.requested_lease_ms(); - if (negotiated.lease_ms < negotiated.watchdog_ms || - negotiated.lease_ms > kMaximumLeaseMs) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "requested_lease_ms must be zero or between watchdog_timeout_ms and 600000"); - } - return grpc::Status::OK; -} - -grpc::Status compareManifests( - const arm_teleop::RobotManifest& expected, - const arm_teleop::RobotManifest& actual) -{ - if (expected.robot_id() != actual.robot_id()) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "robot_id does not match the teleoperation backend"); - } - if (expected.model_sha256() != actual.model_sha256()) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "model_sha256 does not match the teleoperation backend"); - } - if (expected.calibration_sha256() != - actual.calibration_sha256()) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "calibration_sha256 does not match the teleoperation backend"); - } - if (expected.joint_names_size() != actual.joint_names_size()) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "joint count does not match the teleoperation backend"); - } - for (int index = 0; index < expected.joint_names_size(); ++index) { - if (expected.joint_names(index) != actual.joint_names(index)) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "joint order does not match the teleoperation backend"); - } - } - if (expected.position_unit() != actual.position_unit() || - expected.velocity_unit() != actual.velocity_unit() || - expected.effort_unit() != actual.effort_unit()) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "joint units do not match the teleoperation backend"); - } - if (expected.base_frame() != actual.base_frame() || - expected.tool_frame() != actual.tool_frame()) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "base or tool frame does not match the teleoperation backend"); - } - return grpc::Status::OK; -} - -grpc::Status validateSetpoint( - const arm_teleop::JointSetpoint& setpoint, - const std::size_t joint_count, - const std::uint32_t watchdog_ms) -{ - if (setpoint.valid_for_us() == 0) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "setpoint.valid_for_us must be non-zero"); - } - const std::uint64_t maximum_validity_us = - static_cast(watchdog_ms) * 1000U; - if (setpoint.valid_for_us() > maximum_validity_us) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "setpoint.valid_for_us must not exceed the negotiated watchdog"); - } - if (setpoint.position_rad_size() != - static_cast(joint_count) || - setpoint.velocity_rad_s_size() != - static_cast(joint_count)) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "setpoint position and velocity dimensions must match the robot manifest"); - } - for (const double value : setpoint.position_rad()) { - if (!std::isfinite(value)) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "setpoint positions must be finite"); - } - } - for (const double value : setpoint.velocity_rad_s()) { - if (!std::isfinite(value)) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "setpoint velocities must be finite"); - } - } - return grpc::Status::OK; -} - -std::string nextSessionId() -{ - const auto sequence = - g_session_sequence.fetch_add(1, std::memory_order_relaxed) + 1; - const auto timestamp = std::chrono::duration_cast( - Clock::now().time_since_epoch()) - .count(); - std::ostringstream output; - output << "arm-teleop-" << timestamp << "-" << sequence; - return output.str(); -} - -std::uint32_t leaseRemainingMs( - const Clock::time_point now, - const Clock::time_point deadline) -{ - if (now >= deadline) { - return 0; - } - const auto remaining = - std::chrono::duration_cast( - deadline - now); - const auto value = remaining.count(); - if (value <= 0) { - return 1; - } - return static_cast( - std::min(value, kMaximumLeaseMs)); -} - -void fillServerFrame( - const SessionRuntime& session, - const arm_teleop::SessionPhase phase, - const arm_teleop::StopReason stop_reason, - const std::string& detail, - const ArmTeleopBackendSnapshot& snapshot, - arm_teleop::ServerFrame& frame) -{ - auto* status = frame.mutable_status(); - status->set_session_id(session.session_id); - status->set_phase(phase); - status->set_received_sequence(session.received_sequence); - status->set_applied_sequence(session.applied_sequence); - status->set_dropped_setpoints(session.dropped_setpoints); - status->set_rejected_setpoints(session.rejected_setpoints); - status->set_negotiated_watchdog_ms(session.watchdog_ms); - status->set_lease_remaining_ms( - leaseRemainingMs(Clock::now(), session.lease_deadline)); - status->set_stop_reason(stop_reason); - status->set_detail(detail); - *frame.mutable_joint_state() = snapshot.joint_state; - *frame.mutable_safety() = snapshot.safety; -} - -bool writeBareRejection( - grpc::ServerReaderWriter* stream, - const grpc::Status& status) -{ - arm_teleop::ServerFrame frame; - frame.mutable_status()->set_phase( - arm_teleop::SESSION_PHASE_REJECTED); - frame.mutable_status()->set_stop_reason( - arm_teleop::STOP_REASON_PROTOCOL_ERROR); - frame.mutable_status()->set_detail(status.error_message()); - frame.mutable_safety()->set_connected(false); - frame.mutable_safety()->set_powered_on(false); - frame.mutable_safety()->set_fault(true); - frame.mutable_safety()->set_fault_detail(status.error_message()); - return stream->Write(frame); -} - -grpc::Status cancelledStatus(grpc::ServerContext* context) -{ - if (std::chrono::system_clock::now() >= context->deadline()) { - return grpc::Status( - grpc::StatusCode::DEADLINE_EXCEEDED, - "arm teleoperation RPC deadline exceeded"); - } - return grpc::Status( - grpc::StatusCode::CANCELLED, - "arm teleoperation RPC cancelled"); -} - -} // namespace - -std::shared_ptr makeDisabledArmTeleopBackend() -{ - return std::make_shared(); -} - -ArmTeleopServiceImpl::ArmTeleopServiceImpl( - std::shared_ptr backend, - control::ControlAuthorityManager* authority, - std::shared_ptr security_gateway, - safety::SafetyManager* safety_manager) - : backend_(std::move(backend)), - authority_( - authority ? authority - : &control::ControlAuthorityManager::instance()), - security_gateway_(security_gateway - ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()), - safety_manager_(safety_manager) -{ - if (!backend_) { - backend_ = makeDisabledArmTeleopBackend(); - } -} - -grpc::Status ArmTeleopServiceImpl::Teleoperate( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.armteleop.v1.ArmTeleopService/Teleoperate"); - if (context == nullptr || stream == nullptr) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - "arm teleoperation server received a null stream"); - } - - arm_teleop::ClientFrame first_frame; - if (!stream->Read(&first_frame)) { - return context->IsCancelled() - ? cancelledStatus(context) - : grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "the first client frame must be OpenSession"); - } - if (!first_frame.has_open()) { - const grpc::Status status( - grpc::StatusCode::INVALID_ARGUMENT, - "the first client frame must be OpenSession"); - writeBareRejection(stream, status); - return status; - } - - NegotiatedOpen negotiated; - const auto open_status = - validateOpen(first_frame.open(), negotiated); - if (!open_status.ok()) { - writeBareRejection(stream, open_status); - return open_status; - } - if (!backend_->available()) { - const grpc::Status status( - grpc::StatusCode::FAILED_PRECONDITION, - "arm teleoperation backend is disabled"); - writeBareRejection(stream, status); - return status; - } - - try { - const auto backend_manifest = backend_->manifest(); - const auto manifest_status = compareManifests( - first_frame.open().expected_robot(), backend_manifest); - if (!manifest_status.ok()) { - writeBareRejection(stream, manifest_status); - return manifest_status; - } - - const std::string session_id = nextSessionId(); - control::ControlAcquireResult acquired; - { - auto admission = - globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting()) { - const grpc::Status status( - grpc::StatusCode::UNAVAILABLE, - "arm teleoperation is temporarily paused by StopAll"); - writeBareRejection(stream, status); - return status; - } - acquired = authority_->tryAcquire( - backend_manifest.robot_id(), - session_id, - std::chrono::milliseconds(negotiated.lease_ms)); - } - if (!acquired.acquired) { - const grpc::Status status( - grpc::StatusCode::RESOURCE_EXHAUSTED, - acquired.detail.empty() - ? "another controller owns the arm control lease" - : acquired.detail); - writeBareRejection(stream, status); - return status; - } - const auto control_lease = acquired.token; - ScopeExit lease_guard([this, control_lease]() { - authority_->release(control_lease); - }); - - std::optional safety_session; - if (safety_manager_) { - GrpcStreamingSafetyOpen safety_open; - safety_open.full_method_name = - "/cmvr.api.armteleop.v1.ArmTeleopService/Teleoperate"; - safety_open.device_id = backend_manifest.robot_id(); - safety_open.session_id = session_id; - safety_open.authority_generation = control_lease.generation; - safety_open.deadline = cmvr_grpc_call_guard.context().deadline; - safety_session.emplace( - *safety_manager_, - cmvr_grpc_call_guard.context(), - std::move(safety_open)); - if (!safety_session->admitted()) { - writeBareRejection(stream, safety_session->status()); - return safety_session->status(); - } - } - - if (first_frame.open().request_force_feedback() && - !backend_->supportsForceFeedback()) { - const grpc::Status status( - grpc::StatusCode::FAILED_PRECONDITION, - "force feedback was requested but is unavailable"); - writeBareRejection(stream, status); - return status; - } - - bool backend_open_attempted = false; - bool backend_stopped = false; - const auto safeStop = - [&](const arm_teleop::StopReason reason, - const std::string& detail) noexcept { - if (!backend_open_attempted || backend_stopped) { - return ArmTeleopBackendResult::ok(); - } - backend_stopped = true; - try { - return backend_->stop(reason, detail); - } catch (const std::exception& error) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::INTERNAL, - std::string("teleoperation backend stop exception: ") + - error.what()); - } catch (...) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::INTERNAL, - "teleoperation backend stop exception"); - } - }; - ScopeExit backend_guard([&]() { - safeStop( - arm_teleop::STOP_REASON_PROTOCOL_ERROR, - "arm teleoperation handler terminated unexpectedly"); - }); - - ArmTeleopBackendResult backend_open; - { - auto dispatch = - authority_->tryBeginDispatch(control_lease); - if (!dispatch.acquired()) { - const grpc::Status status( - grpc::StatusCode::ABORTED, - "arm teleoperation control authority was revoked before backend open"); - writeBareRejection(stream, status); - return status; - } - auto safety_dispatch = safety_session - ? safety_session->beginDispatch() - : safety::DispatchGuard{}; - if (safety_session && !safety_dispatch.acquired()) { - writeBareRejection(stream, safety_session->status()); - return safety_session->status(); - } - backend_open_attempted = true; - backend_open = backend_->open(first_frame.open()); - } - if (!backend_open.success) { - const auto stopped = safeStop( - arm_teleop::STOP_REASON_PROTOCOL_ERROR, - backend_open.detail); - const std::string detail = - stopped.success - ? backend_open.detail - : backend_open.detail + "; " + stopped.detail; - const grpc::Status status( - stopped.success ? backend_open.status_code - : grpc::StatusCode::INTERNAL, - detail); - writeBareRejection(stream, status); - return status; - } - - SessionRuntime session; - session.session_id = session_id; - session.watchdog_ms = negotiated.watchdog_ms; - session.lease_ms = negotiated.lease_ms; - session.lease_deadline = - Clock::now() + std::chrono::milliseconds(session.lease_ms); - const std::size_t joint_count = static_cast( - backend_manifest.joint_names_size()); - - const auto backendSnapshot = - [&]() noexcept { - ArmTeleopBackendSnapshot snapshot; - try { - snapshot = backend_->snapshot(); - } catch (const std::exception& error) { - snapshot.safety.set_fault(true); - snapshot.safety.set_fault_detail( - std::string("teleoperation backend snapshot exception: ") + - error.what()); - } catch (...) { - snapshot.safety.set_fault(true); - snapshot.safety.set_fault_detail( - "teleoperation backend snapshot exception"); - } - return snapshot; - }; - const auto writeStatus = - [&](const arm_teleop::SessionPhase phase, - const arm_teleop::StopReason reason, - const std::string& detail) { - arm_teleop::ServerFrame frame; - fillServerFrame( - session, phase, reason, detail, - backendSnapshot(), frame); - return stream->Write(frame); - }; - - if (!writeStatus( - arm_teleop::SESSION_PHASE_OPENED, - arm_teleop::STOP_REASON_UNSPECIFIED, {})) { - safeStop( - arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, - "client stopped reading while opening"); - return grpc::Status( - grpc::StatusCode::CANCELLED, - "client stopped reading while opening"); - } - if (!writeStatus( - arm_teleop::SESSION_PHASE_READY, - arm_teleop::STOP_REASON_UNSPECIFIED, {})) { - safeStop( - arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, - "client stopped reading while entering ready state"); - return grpc::Status( - grpc::StatusCode::CANCELLED, - "client stopped reading while entering ready state"); - } - - struct InputSlot { - std::mutex mutex; - std::condition_variable cv; - std::optional latest_setpoint; - std::optional latest_heartbeat; - std::optional terminal; - bool ended{false}; - bool reader_failed{false}; - std::string reader_error; - std::uint64_t dropped_setpoints{0}; - std::uint64_t next_ordinal{0}; - } input; - - std::thread reader; - bool reader_joined = false; - const auto joinReader = - [&](const bool cancel_context) noexcept { - if (cancel_context) { - bool ended = false; - try { - std::unique_lock lock(input.mutex); - input.cv.wait_for( - lock, kReaderJoinGrace, - [&]() { return input.ended; }); - ended = input.ended; - } catch (...) { - } - if (!ended) { - context->TryCancel(); - } - } - input.cv.notify_all(); - if (reader.joinable()) { - try { - reader.join(); - } catch (...) { - } - } - reader_joined = true; - }; - ScopeExit stream_guard([&]() { - safeStop( - arm_teleop::STOP_REASON_PROTOCOL_ERROR, - "arm teleoperation stream terminated unexpectedly"); - if (!reader_joined) { - joinReader(true); - } - }); - - try { - reader = std::thread([&]() { - try { - arm_teleop::ClientFrame incoming; - while (stream->Read(&incoming)) { - PendingFrame pending; - pending.arrived = Clock::now(); - pending.frame = std::move(incoming); - bool terminal = false; - { - std::lock_guard lock(input.mutex); - pending.ordinal = ++input.next_ordinal; - if (pending.frame.has_setpoint()) { - if (input.latest_setpoint.has_value()) { - ++input.dropped_setpoints; - } - input.latest_setpoint = std::move(pending); - } else if (pending.frame.has_heartbeat()) { - input.latest_heartbeat = std::move(pending); - } else { - if (input.latest_setpoint.has_value()) { - ++input.dropped_setpoints; - } - input.latest_setpoint.reset(); - input.latest_heartbeat.reset(); - input.terminal = std::move(pending); - terminal = true; - } - } - input.cv.notify_one(); - incoming.Clear(); - if (terminal) { - break; - } - } - } catch (const std::exception& error) { - std::lock_guard lock(input.mutex); - input.reader_failed = true; - input.reader_error = error.what(); - } catch (...) { - std::lock_guard lock(input.mutex); - input.reader_failed = true; - input.reader_error = "unknown reader exception"; - } - { - std::lock_guard lock(input.mutex); - input.ended = true; - } - input.cv.notify_one(); - }); - } catch (const std::exception& error) { - const std::string detail = - std::string("failed to start teleoperation reader: ") + - error.what(); - safeStop( - arm_teleop::STOP_REASON_PROTOCOL_ERROR, detail); - return grpc::Status( - grpc::StatusCode::INTERNAL, detail); - } - - Clock::time_point last_valid_activity = Clock::now(); - - const auto finish = - [&](const arm_teleop::SessionPhase requested_phase, - const arm_teleop::StopReason reason, - const std::string& requested_detail, - const grpc::Status& requested_status, - const bool cancel_reader) { - const auto stopped = safeStop(reason, requested_detail); - const auto phase = - stopped.success - ? requested_phase - : arm_teleop::SESSION_PHASE_FAILED; - const std::string detail = - stopped.success - ? requested_detail - : requested_detail + "; " + stopped.detail; - const bool wrote = - writeStatus(phase, reason, detail); - bool reader_has_ended = false; - { - std::lock_guard lock(input.mutex); - reader_has_ended = input.ended; - } - // Protocol errors are terminal even if a misbehaving client - // keeps its write half open. Never wait indefinitely for the - // reader's blocking Read in that case. - joinReader(cancel_reader || !reader_has_ended); - stream_guard.release(); - if (!stopped.success) { - return grpc::Status( - grpc::StatusCode::INTERNAL, detail); - } - if (!wrote) { - return grpc::Status( - grpc::StatusCode::CANCELLED, - "client stopped reading the terminal teleoperation status"); - } - return requested_status; - }; - - for (;;) { - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - const auto status = cancelledStatus(context); - const auto stopped = safeStop( - arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, - status.error_message()); - joinReader(true); - stream_guard.release(); - return stopped.success - ? status - : grpc::Status( - grpc::StatusCode::INTERNAL, - status.error_message() + "; " + - stopped.detail); - } - if (!authority_->validate(control_lease)) { - const std::string detail = - "arm teleoperation control authority was revoked"; - return finish( - arm_teleop::SESSION_PHASE_LEASE_LOST, - arm_teleop::STOP_REASON_LEASE_REVOKED, - detail, - grpc::Status( - grpc::StatusCode::ABORTED, detail), - true); - } - if (safety_session && !safety_session->revalidate()) { - const std::string detail = - "arm teleoperation safety session was invalidated: " + - safety_session->status().error_message(); - return finish( - arm_teleop::SESSION_PHASE_LEASE_LOST, - arm_teleop::STOP_REASON_EMERGENCY_STOP, - detail, - safety_session->status(), - true); - } - - std::optional pending; - bool ended = false; - bool reader_failed = false; - std::string reader_error; - { - std::unique_lock lock(input.mutex); - input.cv.wait_for(lock, kLoopSlice, [&]() { - return input.latest_setpoint.has_value() || - input.latest_heartbeat.has_value() || - input.terminal.has_value() || - input.ended; - }); - - if (input.terminal.has_value()) { - pending = std::move(input.terminal); - input.terminal.reset(); - } else if (input.latest_setpoint.has_value() && - input.latest_heartbeat.has_value()) { - if (input.latest_setpoint->ordinal < - input.latest_heartbeat->ordinal) { - pending = std::move(input.latest_setpoint); - input.latest_setpoint.reset(); - } else { - pending = std::move(input.latest_heartbeat); - input.latest_heartbeat.reset(); - } - } else if (input.latest_setpoint.has_value()) { - pending = std::move(input.latest_setpoint); - input.latest_setpoint.reset(); - } else if (input.latest_heartbeat.has_value()) { - pending = std::move(input.latest_heartbeat); - input.latest_heartbeat.reset(); - } - session.dropped_setpoints = - input.dropped_setpoints; - ended = input.ended; - reader_failed = input.reader_failed; - reader_error = input.reader_error; - } - - const auto now = Clock::now(); - if (pending.has_value() && pending->frame.has_stop()) { - const auto requested_reason = - pending->frame.stop().reason(); - if (requested_reason == - arm_teleop::STOP_REASON_UNSPECIFIED) { - const std::string detail = - "StopSession.reason must be specified"; - ++session.rejected_setpoints; - return finish( - arm_teleop::SESSION_PHASE_REJECTED, - arm_teleop::STOP_REASON_PROTOCOL_ERROR, - detail, - grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - detail), - false); - } - return finish( - arm_teleop::SESSION_PHASE_STOPPED, - requested_reason, - pending->frame.stop().detail(), - grpc::Status::OK, false); - } - if (now >= session.lease_deadline) { - const std::string detail = - "arm teleoperation control lease expired"; - return finish( - arm_teleop::SESSION_PHASE_LEASE_LOST, - arm_teleop::STOP_REASON_LEASE_REVOKED, - detail, - grpc::Status( - grpc::StatusCode::ABORTED, detail), - true); - } - if (!pending.has_value()) { - if (reader_failed) { - const std::string detail = - "teleoperation reader failed: " + reader_error; - return finish( - arm_teleop::SESSION_PHASE_FAILED, - arm_teleop::STOP_REASON_PROTOCOL_ERROR, - detail, - grpc::Status( - grpc::StatusCode::INTERNAL, detail), - false); - } - if (ended) { - return finish( - arm_teleop::SESSION_PHASE_STOPPED, - arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, - "client closed the teleoperation input stream", - grpc::Status::OK, false); - } - if (now - last_valid_activity >= - std::chrono::milliseconds(session.watchdog_ms)) { - const std::string detail = - "arm teleoperation watchdog expired"; - return finish( - arm_teleop::SESSION_PHASE_WATCHDOG_EXPIRED, - arm_teleop::STOP_REASON_WATCHDOG, - detail, - grpc::Status( - grpc::StatusCode::DEADLINE_EXCEEDED, - detail), - true); - } - continue; - } - - if (pending->frame.has_open() || - pending->frame.payload_case() == - arm_teleop::ClientFrame::PAYLOAD_NOT_SET) { - const std::string detail = - "OpenSession is only valid as the first client frame"; - return finish( - arm_teleop::SESSION_PHASE_REJECTED, - arm_teleop::STOP_REASON_PROTOCOL_ERROR, - detail, - grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, detail), - false); - } - - const std::uint64_t sequence = - pending->frame.has_setpoint() - ? pending->frame.setpoint().sequence() - : pending->frame.heartbeat().sequence(); - if (sequence == 0 || - sequence <= session.received_sequence) { - const std::string detail = - "client sequence must be strictly increasing and non-zero"; - if (pending->frame.has_setpoint()) { - ++session.rejected_setpoints; - } - return finish( - arm_teleop::SESSION_PHASE_REJECTED, - arm_teleop::STOP_REASON_PROTOCOL_ERROR, - detail, - grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, detail), - false); - } - if (pending->arrived - last_valid_activity >= - std::chrono::milliseconds(session.watchdog_ms)) { - const std::string detail = - "arm teleoperation watchdog expired before the next valid client frame"; - return finish( - arm_teleop::SESSION_PHASE_WATCHDOG_EXPIRED, - arm_teleop::STOP_REASON_WATCHDOG, - detail, - grpc::Status( - grpc::StatusCode::DEADLINE_EXCEEDED, - detail), - true); - } - - if (pending->frame.has_heartbeat()) { - if (!authority_->renew( - control_lease, - std::chrono::milliseconds( - session.lease_ms))) { - const std::string detail = - "arm teleoperation control authority was revoked"; - return finish( - arm_teleop::SESSION_PHASE_LEASE_LOST, - arm_teleop::STOP_REASON_LEASE_REVOKED, - detail, - grpc::Status( - grpc::StatusCode::ABORTED, detail), - true); - } - session.received_sequence = sequence; - last_valid_activity = pending->arrived; - session.lease_deadline = - pending->arrived + - std::chrono::milliseconds(session.lease_ms); - if (!writeStatus( - session.applied_sequence == 0 - ? arm_teleop::SESSION_PHASE_READY - : arm_teleop::SESSION_PHASE_ACTIVE, - arm_teleop::STOP_REASON_UNSPECIFIED, {})) { - const std::string detail = - "client stopped reading heartbeat status"; - const auto stopped = safeStop( - arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, - detail); - joinReader(true); - stream_guard.release(); - return stopped.success - ? grpc::Status( - grpc::StatusCode::CANCELLED, - detail) - : grpc::Status( - grpc::StatusCode::INTERNAL, - detail + "; " + stopped.detail); - } - continue; - } - - const auto& setpoint = pending->frame.setpoint(); - const auto setpoint_status = validateSetpoint( - setpoint, joint_count, session.watchdog_ms); - if (!setpoint_status.ok()) { - ++session.rejected_setpoints; - return finish( - arm_teleop::SESSION_PHASE_REJECTED, - arm_teleop::STOP_REASON_PROTOCOL_ERROR, - setpoint_status.error_message(), - setpoint_status, false); - } - session.received_sequence = sequence; - - const auto command_deadline = - pending->arrived + - std::chrono::microseconds(setpoint.valid_for_us()); - if (Clock::now() >= command_deadline) { - ++session.rejected_setpoints; - if (Clock::now() - last_valid_activity >= - std::chrono::milliseconds(session.watchdog_ms)) { - const std::string detail = - "arm teleoperation watchdog expired while rejecting stale setpoints"; - return finish( - arm_teleop::SESSION_PHASE_WATCHDOG_EXPIRED, - arm_teleop::STOP_REASON_WATCHDOG, - detail, - grpc::Status( - grpc::StatusCode::DEADLINE_EXCEEDED, - detail), - true); - } - if (!writeStatus( - arm_teleop::SESSION_PHASE_HOLDING, - arm_teleop::STOP_REASON_UNSPECIFIED, - "setpoint expired before backend dispatch")) { - const std::string detail = - "client stopped reading expired-setpoint status"; - const auto stopped = safeStop( - arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, - detail); - joinReader(true); - stream_guard.release(); - return stopped.success - ? grpc::Status( - grpc::StatusCode::CANCELLED, - detail) - : grpc::Status( - grpc::StatusCode::INTERNAL, - detail + "; " + stopped.detail); - } - continue; - } - - if (!authority_->renew( - control_lease, - std::chrono::milliseconds( - session.lease_ms))) { - const std::string detail = - "arm teleoperation control authority was revoked"; - return finish( - arm_teleop::SESSION_PHASE_LEASE_LOST, - arm_teleop::STOP_REASON_LEASE_REVOKED, - detail, - grpc::Status( - grpc::StatusCode::ABORTED, detail), - true); - } - session.lease_deadline = - pending->arrived + - std::chrono::milliseconds(session.lease_ms); - ArmTeleopBackendResult applied; - { - auto dispatch = - authority_->tryBeginDispatch(control_lease); - if (!dispatch.acquired()) { - const std::string detail = - "arm teleoperation control authority was revoked"; - return finish( - arm_teleop::SESSION_PHASE_LEASE_LOST, - arm_teleop::STOP_REASON_LEASE_REVOKED, - detail, - grpc::Status( - grpc::StatusCode::ABORTED, detail), - true); - } - auto safety_dispatch = safety_session - ? safety_session->beginDispatch() - : safety::DispatchGuard{}; - if (safety_session && !safety_dispatch.acquired()) { - const std::string detail = - "arm teleoperation setpoint rejected by safety: " + - safety_session->status().error_message(); - return finish( - arm_teleop::SESSION_PHASE_LEASE_LOST, - arm_teleop::STOP_REASON_EMERGENCY_STOP, - detail, - safety_session->status(), - true); - } - applied = backend_->applySetpoint( - setpoint, command_deadline); - } - if (!applied.success) { - ++session.rejected_setpoints; - return finish( - arm_teleop::SESSION_PHASE_FAILED, - arm_teleop::STOP_REASON_ROBOT_FAULT, - applied.detail, - grpc::Status(applied.status_code, applied.detail), - true); - } - session.applied_sequence = sequence; - last_valid_activity = pending->arrived; - if (!writeStatus( - arm_teleop::SESSION_PHASE_ACTIVE, - arm_teleop::STOP_REASON_UNSPECIFIED, {})) { - const std::string detail = - "client stopped reading active teleoperation status"; - const auto stopped = safeStop( - arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, - detail); - joinReader(true); - stream_guard.release(); - return stopped.success - ? grpc::Status( - grpc::StatusCode::CANCELLED, - detail) - : grpc::Status( - grpc::StatusCode::INTERNAL, - detail + "; " + stopped.detail); - } - } - } catch (const std::exception& error) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - std::string("arm teleoperation service exception: ") + - error.what()); - } catch (...) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - "arm teleoperation service exception"); - } -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp b/cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp deleted file mode 100644 index bba5b75e..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp +++ /dev/null @@ -1,1269 +0,0 @@ -#include "service/grpc/server/include/grpc_command_transaction.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "service/grpc/server/include/grpc_safety_proto.h" - -namespace cmvr::service { - -namespace { - -constexpr auto kDefaultCommandValidity = std::chrono::seconds(30); -constexpr auto kMaximumCommandValidity = std::chrono::minutes(5); -constexpr std::size_t kMaximumDeviceIdLength = 256; - -const api::CommandHeader_Request* findRequestHeader( - const google::protobuf::Message& message) -{ - if (const auto* header = - dynamic_cast(&message)) { - return header; - } - const auto* reflection = message.GetReflection(); - std::vector fields; - reflection->ListFields(message, &fields); - for (const auto* field : fields) { - if (field->cpp_type() != - google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE) { - continue; - } - if (field->is_repeated()) { - for (int index = 0; - index < reflection->FieldSize(message, field); ++index) { - if (const auto* header = findRequestHeader( - reflection->GetRepeatedMessage( - message, field, index))) { - return header; - } - } - } else if (const auto* header = findRequestHeader( - reflection->GetMessage(message, field))) { - return header; - } - } - return nullptr; -} - -api::CommandHeader_Feedback* findFeedbackHeader( - google::protobuf::Message& message) -{ - if (auto* header = dynamic_cast(&message)) { - return header; - } - const auto* reflection = message.GetReflection(); - const auto* descriptor = message.GetDescriptor(); - for (int index = 0; index < descriptor->field_count(); ++index) { - const auto* field = descriptor->field(index); - if (field->cpp_type() != - google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE || - field->is_repeated()) { - continue; - } - auto* nested = reflection->MutableMessage(&message, field); - if (auto* header = findFeedbackHeader(*nested)) { - return header; - } - } - return nullptr; -} - -void canonicalizeMessage(google::protobuf::Message& message) -{ - if (auto* header = dynamic_cast(&message)) { - header->clear_timestamp(); - header->clear_command_id(); - header->clear_expected_service_instance_id(); - header->clear_valid_for_ms(); - } - - const auto* reflection = message.GetReflection(); - std::vector fields; - reflection->ListFields(message, &fields); - for (const auto* field : fields) { - if (field->cpp_type() != - google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE) { - continue; - } - if (field->is_repeated()) { - const int size = reflection->FieldSize(message, field); - for (int index = 0; index < size; ++index) { - canonicalizeMessage( - *reflection->MutableRepeatedMessage( - &message, field, index)); - } - } else { - canonicalizeMessage( - *reflection->MutableMessage(&message, field)); - } - } -} - -bool deterministicSerialize( - const google::protobuf::Message& message, - std::string& serialized) -{ - serialized.clear(); - google::protobuf::io::StringOutputStream stream(&serialized); - google::protobuf::io::CodedOutputStream coded_stream(&stream); - coded_stream.SetSerializationDeterministic(true); - if (!message.SerializeToCodedStream(&coded_stream)) { - serialized.clear(); - return false; - } - coded_stream.Trim(); - return !coded_stream.HadError(); -} - -std::uint64_t stableHash( - const std::string& value, - const std::uint64_t seed) noexcept -{ - std::uint64_t hash = 1469598103934665603ULL ^ seed; - for (const unsigned char byte : value) { - hash ^= static_cast(byte); - hash *= 1099511628211ULL; - } - hash ^= hash >> 33U; - hash *= 0xff51afd7ed558ccdULL; - hash ^= hash >> 33U; - hash *= 0xc4ceb9fe1a85ec53ULL; - hash ^= hash >> 33U; - return hash; -} - -bool isEnforced( - const safety::SafetyManagerConfig& config, - const std::string& device_id) -{ - switch (config.enforcement_mode) { - case safety::EnforcementMode::Legacy: - case safety::EnforcementMode::Shadow: - return false; - case safety::EnforcementMode::EnforceSelected: - return config.enforced_device_ids.count(device_id) != 0; - case safety::EnforcementMode::EnforceAll: - return true; - } - return false; -} - -bool commandIdentityRequired( - const GrpcMethodPolicy& policy, - const bool enforced) noexcept -{ - return enforced && policy.mutating && - policy.command_intent != safety::CommandIntent::Stop && - policy.command_intent != safety::CommandIntent::RecoverAdmission; -} - -api::CommandExecutionState toApiLifecycle( - const safety::CommandLifecycle lifecycle) noexcept -{ - using safety::CommandLifecycle; - switch (lifecycle) { - case CommandLifecycle::Received: - return api::COMMAND_EXECUTION_STATE_RECEIVED; - case CommandLifecycle::Reserved: - return api::COMMAND_EXECUTION_STATE_RESERVED; - case CommandLifecycle::RejectedBeforeDispatch: - return api::COMMAND_EXECUTION_STATE_REJECTED_BEFORE_DISPATCH; - case CommandLifecycle::Admitted: - return api::COMMAND_EXECUTION_STATE_ADMITTED; - case CommandLifecycle::Dispatching: - return api::COMMAND_EXECUTION_STATE_DISPATCHING; - case CommandLifecycle::AcceptedByHardware: - return api::COMMAND_EXECUTION_STATE_ACCEPTED_BY_HARDWARE; - case CommandLifecycle::Completed: - return api::COMMAND_EXECUTION_STATE_COMPLETED; - case CommandLifecycle::Failed: - return api::COMMAND_EXECUTION_STATE_FAILED; - case CommandLifecycle::CanceledBeforeDispatch: - return api::COMMAND_EXECUTION_STATE_CANCELED_BEFORE_DISPATCH; - case CommandLifecycle::OutcomeUnknown: - return api::COMMAND_EXECUTION_STATE_OUTCOME_UNKNOWN; - } - return api::COMMAND_EXECUTION_STATE_UNSPECIFIED; -} - -grpc::Status statusForReason( - const safety::SafetyReason reason, - const std::string& detail) -{ - using safety::SafetyReason; - grpc::StatusCode code = grpc::StatusCode::FAILED_PRECONDITION; - switch (reason) { - case SafetyReason::None: return grpc::Status::OK; - case SafetyReason::InvalidArgument: - case SafetyReason::CommandIdRequired: - case SafetyReason::RecoveryReasonRequired: - code = grpc::StatusCode::INVALID_ARGUMENT; - break; - case SafetyReason::Unauthenticated: - code = grpc::StatusCode::UNAUTHENTICATED; - break; - case SafetyReason::PermissionDenied: - code = grpc::StatusCode::PERMISSION_DENIED; - break; - case SafetyReason::DeviceNotFound: - code = grpc::StatusCode::NOT_FOUND; - break; - case SafetyReason::UnsupportedCommand: - code = grpc::StatusCode::UNIMPLEMENTED; - break; - case SafetyReason::DeviceUnavailable: - case SafetyReason::DeviceDisconnected: - case SafetyReason::SafetyStateMissing: - case SafetyReason::SafetyStateStale: - case SafetyReason::SystemStarting: - code = grpc::StatusCode::UNAVAILABLE; - break; - case SafetyReason::SystemStopping: - case SafetyReason::GenerationMismatch: - case SafetyReason::OutcomeUnknown: - code = grpc::StatusCode::ABORTED; - break; - case SafetyReason::CommandIdConflict: - code = grpc::StatusCode::ALREADY_EXISTS; - break; - case SafetyReason::ResultEvicted: - code = grpc::StatusCode::FAILED_PRECONDITION; - break; - case SafetyReason::LedgerExhausted: - case SafetyReason::Backpressure: - case SafetyReason::ControlBusy: - code = grpc::StatusCode::RESOURCE_EXHAUSTED; - break; - case SafetyReason::DeadlineExceededBeforeDispatch: - case SafetyReason::ParticipantTimeout: - code = grpc::StatusCode::DEADLINE_EXCEEDED; - break; - case SafetyReason::InternalError: - code = grpc::StatusCode::INTERNAL; - break; - case SafetyReason::RecoveryRpcDisabled: - case SafetyReason::SafetyLatched: - case SafetyReason::HardwareUnsafe: - case SafetyReason::EmergencyStopActive: - case SafetyReason::ProtectiveStopActive: - case SafetyReason::DeviceFault: - case SafetyReason::DeviceNotReady: - case SafetyReason::DeviceStillMoving: - case SafetyReason::StopUnconfirmed: - case SafetyReason::RecoveryEpochMismatch: - case SafetyReason::RecoveryAuditFailed: - break; - } - return grpc::Status( - code, - detail.empty() ? safety::toString(reason) : detail); -} - -safety::SafetyReason reasonForStatus(const grpc::Status& status) noexcept -{ - if (status.ok()) { - return safety::SafetyReason::InternalError; - } - switch (status.error_code()) { - case grpc::StatusCode::INVALID_ARGUMENT: - case grpc::StatusCode::OUT_OF_RANGE: - return safety::SafetyReason::InvalidArgument; - case grpc::StatusCode::UNAUTHENTICATED: - return safety::SafetyReason::Unauthenticated; - case grpc::StatusCode::PERMISSION_DENIED: - return safety::SafetyReason::PermissionDenied; - case grpc::StatusCode::NOT_FOUND: - return safety::SafetyReason::DeviceNotFound; - case grpc::StatusCode::UNIMPLEMENTED: - return safety::SafetyReason::UnsupportedCommand; - case grpc::StatusCode::UNAVAILABLE: - return safety::SafetyReason::DeviceUnavailable; - case grpc::StatusCode::RESOURCE_EXHAUSTED: - return safety::SafetyReason::ControlBusy; - case grpc::StatusCode::DEADLINE_EXCEEDED: - return safety::SafetyReason::DeadlineExceededBeforeDispatch; - case grpc::StatusCode::ABORTED: - return safety::SafetyReason::GenerationMismatch; - default: - return safety::SafetyReason::InternalError; - } -} - -bool validDeviceId(const std::string& device_id) noexcept -{ - return !device_id.empty() && device_id.size() <= kMaximumDeviceIdLength; -} - -} // namespace - -grpc::Status grpcStatusForSafetyReason( - const safety::SafetyReason reason, - const std::string& detail) -{ - return statusForReason(reason, detail); -} - -GrpcStreamingSafetySession::GrpcStreamingSafetySession( - safety::SafetyManager& coordinator, - const GrpcRequestContext& request_context, - GrpcStreamingSafetyOpen open) - : coordinator_(&coordinator), - status_(grpc::Status::OK) -{ - if (!validDeviceId(open.device_id) || open.session_id.empty()) { - reject_( - safety::SafetyReason::InvalidArgument, - "stream device_id and session_id are required"); - return; - } - const auto policy = - defaultGrpcMethodPolicyRegistry().find(open.full_method_name); - if (!policy.has_value() || !policy->mutating) { - reject_( - safety::SafetyReason::InternalError, - "stream requires a registered mutating method policy"); - return; - } - if (!open.expected_service_instance_id.empty() && - open.expected_service_instance_id != coordinator.serviceInstanceId()) { - reject_( - safety::SafetyReason::GenerationMismatch, - "expected_service_instance_id does not match the active control service instance"); - return; - } - - safety::AdmissionRequest request; - request.command = policy->commandDescriptor(); - request.actor.principal_id = request_context.principal.id; - request.actor.authenticated = request_context.principal.authenticated; - for (const auto role : request_context.principal.roles) { - request.actor.roles.emplace_back(toString(role)); - } - request.command_id = std::move(open.session_id); - request.device_id = std::move(open.device_id); - request.expected_device_generation = - open.expected_device_generation; - request.authority_generation = open.authority_generation; - request.deadline = std::min(open.deadline, request_context.deadline); - - auto admitted = coordinator.admit(request); - admission_decision_ = admitted.decision; - if (!admitted.permit.has_value()) { - reject_( - admitted.decision.reason == safety::SafetyReason::None - ? safety::SafetyReason::InternalError - : admitted.decision.reason, - admitted.decision.detail); - return; - } - permit_ = std::move(admitted.permit); -} - -bool GrpcStreamingSafetySession::revalidate() -{ - if (!coordinator_ || !permit_.has_value()) { - reject_( - safety::SafetyReason::InternalError, - "stream safety session is not admitted"); - return false; - } - const auto check = coordinator_->revalidatePermit(*permit_); - if (!check.safe) { - reject_( - check.reason == safety::SafetyReason::None - ? safety::SafetyReason::SafetyLatched - : check.reason, - check.detail); - return false; - } - status_ = grpc::Status::OK; - return true; -} - -safety::DispatchGuard GrpcStreamingSafetySession::beginDispatch() -{ - if (!coordinator_ || !permit_.has_value()) { - reject_( - safety::SafetyReason::InternalError, - "stream safety session is not dispatchable"); - return {}; - } - auto guard = coordinator_->beginDispatch(*permit_); - if (!guard.acquired()) { - const auto& check = guard.hardwareCheck(); - reject_( - check.reason == safety::SafetyReason::None - ? safety::SafetyReason::SafetyLatched - : check.reason, - check.detail); - return guard; - } - status_ = grpc::Status::OK; - return guard; -} - -std::uint64_t GrpcStreamingSafetySession::safetyEpoch() const noexcept -{ - return permit_ ? permit_->safety_epoch : 0U; -} - -std::uint64_t GrpcStreamingSafetySession::deviceGeneration() const noexcept -{ - return permit_ ? permit_->device_generation : 0U; -} - -std::uint64_t GrpcStreamingSafetySession::authorityGeneration() const noexcept -{ - return permit_ ? permit_->authority_generation : 0U; -} - -void GrpcStreamingSafetySession::reject_( - const safety::SafetyReason reason, - std::string detail) -{ - status_ = grpcStatusForSafetyReason(reason, detail); - permit_.reset(); -} - -std::string deterministicGrpcPayloadHash( - const std::string& full_method_name, - const google::protobuf::Message& request) -{ - std::unique_ptr normalized(request.New()); - if (!normalized) { - return {}; - } - normalized->CopyFrom(request); - normalized->DiscardUnknownFields(); - canonicalizeMessage(*normalized); - - std::string serialized; - if (!deterministicSerialize(*normalized, serialized)) { - return {}; - } - serialized.insert(0, full_method_name + '\0'); - constexpr std::array seeds{ - 0xa4093822299f31d0ULL, - 0x082efa98ec4e6c89ULL, - 0x452821e638d01377ULL, - 0xbe5466cf34e90c6cULL}; - std::ostringstream output; - output << std::hex << std::setfill('0'); - for (const auto seed : seeds) { - output << std::setw(16) << stableHash(serialized, seed); - } - output << ':' << std::dec << serialized.size(); - return output.str(); -} - -GrpcCommandTransaction::GrpcCommandTransaction( - safety::SafetyManager& coordinator, - GrpcRequestContext request_context, - GrpcMethodPolicy method_policy, - const google::protobuf::Message& request, - google::protobuf::Message& response) - : coordinator_(&coordinator), - request_context_(std::move(request_context)), - method_policy_(std::move(method_policy)), - response_(&response), - status_(grpc::Status::OK), - dispatch_status_(grpc::Status::OK) -{ - initialize_(request); -} - -GrpcCommandTransaction::~GrpcCommandTransaction() noexcept -{ - abandon_(); -} - -GrpcCommandTransaction::GrpcCommandTransaction( - GrpcCommandTransaction&& other) noexcept - : coordinator_(std::exchange(other.coordinator_, nullptr)), - request_context_(std::move(other.request_context_)), - method_policy_(std::move(other.method_policy_)), - response_(std::exchange(other.response_, nullptr)), - ledger_ticket_(std::move(other.ledger_ticket_)), - permit_(std::move(other.permit_)), - dispatch_guard_(std::move(other.dispatch_guard_)), - admission_decision_(std::move(other.admission_decision_)), - status_(std::move(other.status_)), - dispatch_status_(std::move(other.dispatch_status_)), - device_id_(std::move(other.device_id_)), - command_id_(std::move(other.command_id_)), - payload_hash_(std::move(other.payload_hash_)), - deadline_(other.deadline_), - should_execute_(other.should_execute_), - owns_ledger_record_(other.owns_ledger_record_), - dispatch_started_(other.dispatch_started_), - completed_(other.completed_), - safety_stop_sequence_(other.safety_stop_sequence_) -{ - other.should_execute_ = false; - other.owns_ledger_record_ = false; - other.dispatch_started_ = false; - other.completed_ = true; -} - -GrpcCommandTransaction& GrpcCommandTransaction::operator=( - GrpcCommandTransaction&& other) noexcept -{ - if (this == &other) { - return *this; - } - abandon_(); - coordinator_ = std::exchange(other.coordinator_, nullptr); - request_context_ = std::move(other.request_context_); - method_policy_ = std::move(other.method_policy_); - response_ = std::exchange(other.response_, nullptr); - ledger_ticket_ = std::move(other.ledger_ticket_); - permit_ = std::move(other.permit_); - dispatch_guard_ = std::move(other.dispatch_guard_); - admission_decision_ = std::move(other.admission_decision_); - status_ = std::move(other.status_); - dispatch_status_ = std::move(other.dispatch_status_); - device_id_ = std::move(other.device_id_); - command_id_ = std::move(other.command_id_); - payload_hash_ = std::move(other.payload_hash_); - deadline_ = other.deadline_; - should_execute_ = other.should_execute_; - owns_ledger_record_ = other.owns_ledger_record_; - dispatch_started_ = other.dispatch_started_; - completed_ = other.completed_; - safety_stop_sequence_ = other.safety_stop_sequence_; - other.should_execute_ = false; - other.owns_ledger_record_ = false; - other.dispatch_started_ = false; - other.completed_ = true; - return *this; -} - -void GrpcCommandTransaction::initialize_( - const google::protobuf::Message& request) -{ - const auto* header = findRequestHeader(request); - if (!header) { - rejectBeforeDispatch_( - safety::SafetyReason::InvalidArgument, - "mutating unary request has no CommandHeader.Request", - grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "mutating unary request has no CommandHeader.Request"), - false); - return; - } - device_id_ = header->device_id(); - command_id_ = header->command_id(); - if (!validDeviceId(device_id_)) { - rejectBeforeDispatch_( - safety::SafetyReason::InvalidArgument, - "header.device_id is required and must not exceed 256 bytes", - grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "header.device_id is required and must not exceed 256 bytes"), - false); - return; - } - - const bool enforced = isEnforced(coordinator_->config(), device_id_); - if (commandIdentityRequired(method_policy_, enforced) && - command_id_.empty()) { - rejectBeforeDispatch_( - safety::SafetyReason::CommandIdRequired, - "header.command_id is required for an enforced mutating command", - statusForReason( - safety::SafetyReason::CommandIdRequired, - "header.command_id is required for an enforced mutating command"), - false); - return; - } - if (commandIdentityRequired(method_policy_, enforced) && - header->expected_service_instance_id().empty()) { - rejectBeforeDispatch_( - safety::SafetyReason::GenerationMismatch, - "header.expected_service_instance_id is required for an enforced mutating command", - statusForReason( - safety::SafetyReason::GenerationMismatch, - "header.expected_service_instance_id is required for an enforced mutating command"), - false); - return; - } - if (!header->expected_service_instance_id().empty() && - header->expected_service_instance_id() != - coordinator_->serviceInstanceId()) { - rejectBeforeDispatch_( - safety::SafetyReason::GenerationMismatch, - "header.expected_service_instance_id does not match the active control service instance", - statusForReason( - safety::SafetyReason::GenerationMismatch, - "header.expected_service_instance_id does not match the active control service instance"), - false); - return; - } - if (header->valid_for_ms() != 0 && - std::chrono::milliseconds(header->valid_for_ms()) > - kMaximumCommandValidity) { - rejectBeforeDispatch_( - safety::SafetyReason::InvalidArgument, - "header.valid_for_ms exceeds the five minute server limit", - statusForReason( - safety::SafetyReason::InvalidArgument, - "header.valid_for_ms exceeds the five minute server limit"), - false); - return; - } - if (enforced && - method_policy_.command_intent == safety::CommandIntent::Actuate && - header->valid_for_ms() == 0) { - rejectBeforeDispatch_( - safety::SafetyReason::InvalidArgument, - "header.valid_for_ms is required for an enforced actuation command", - statusForReason( - safety::SafetyReason::InvalidArgument, - "header.valid_for_ms is required for an enforced actuation command"), - false); - return; - } - - const auto validity = header->valid_for_ms() == 0 - ? kDefaultCommandValidity - : std::chrono::duration_cast( - std::chrono::milliseconds(header->valid_for_ms())); - deadline_ = request_context_.received_at + validity; - deadline_ = std::min(deadline_, request_context_.deadline); - - if (!command_id_.empty() && method_policy_.mutating) { - payload_hash_ = deterministicGrpcPayloadHash( - method_policy_.full_method_name, request); - if (payload_hash_.empty()) { - rejectBeforeDispatch_( - safety::SafetyReason::InternalError, - "request payload could not be serialized deterministically", - statusForReason( - safety::SafetyReason::InternalError, - "request payload could not be serialized deterministically"), - false); - return; - } - const safety::CommandKey key{ - request_context_.principal.id.empty() - ? std::string("anonymous") - : request_context_.principal.id, - command_id_}; - auto reservation = coordinator_->commandLedger().reserve( - key, payload_hash_); - switch (reservation.status) { - case safety::CommandReservationStatus::AcceptedNew: - ledger_ticket_ = std::move(reservation.ticket); - owns_ledger_record_ = true; - break; - case safety::CommandReservationStatus::JoinedInFlight: { - const auto outcome = coordinator_->commandLedger().wait( - reservation.ticket, deadline_); - if (!outcome.has_value()) { - rejectBeforeDispatch_( - safety::SafetyReason::DeadlineExceededBeforeDispatch, - "timed out waiting for the in-flight command result", - statusForReason( - safety::SafetyReason::DeadlineExceededBeforeDispatch, - "timed out waiting for the in-flight command result"), - false); - return; - } - if (!restoreOutcome_(*outcome)) { - rejectBeforeDispatch_( - safety::SafetyReason::InternalError, - "cached command response could not be decoded", - statusForReason( - safety::SafetyReason::InternalError, - "cached command response could not be decoded"), - false); - return; - } - status_ = grpc::Status::OK; - completed_ = true; - return; - } - case safety::CommandReservationStatus::CachedResult: - if (!reservation.cached_outcome.has_value() || - !restoreOutcome_(*reservation.cached_outcome)) { - rejectBeforeDispatch_( - safety::SafetyReason::InternalError, - "cached command response could not be decoded", - statusForReason( - safety::SafetyReason::InternalError, - "cached command response could not be decoded"), - false); - return; - } - status_ = grpc::Status::OK; - completed_ = true; - return; - case safety::CommandReservationStatus::CommandIdConflict: - rejectBeforeDispatch_( - safety::SafetyReason::CommandIdConflict, - "command_id is already associated with a different semantic payload", - statusForReason( - safety::SafetyReason::CommandIdConflict, - "command_id is already associated with a different semantic payload"), - false); - return; - case safety::CommandReservationStatus::ResultEvicted: - rejectBeforeDispatch_( - safety::SafetyReason::ResultEvicted, - "command result was evicted and the command will not be dispatched again", - statusForReason( - safety::SafetyReason::ResultEvicted, - "command result was evicted and the command will not be dispatched again"), - false); - return; - case safety::CommandReservationStatus::LedgerExhausted: - rejectBeforeDispatch_( - safety::SafetyReason::LedgerExhausted, - "command ledger capacity is exhausted", - statusForReason( - safety::SafetyReason::LedgerExhausted, - "command ledger capacity is exhausted"), - false); - return; - case safety::CommandReservationStatus::Invalid: - rejectBeforeDispatch_( - safety::SafetyReason::InvalidArgument, - "command_id or effective principal is invalid", - statusForReason( - safety::SafetyReason::InvalidArgument, - "command_id or effective principal is invalid"), - false); - return; - } - } - - safety::AdmissionRequest admission; - admission.command = method_policy_.commandDescriptor(); - admission.actor.principal_id = request_context_.principal.id; - admission.actor.authenticated = request_context_.principal.authenticated; - for (const auto role : request_context_.principal.roles) { - admission.actor.roles.emplace_back(toString(role)); - } - admission.command_id = command_id_.empty() - ? request_context_.correlation_id - : command_id_; - admission.device_id = device_id_; - if (header->has_expected_device_generation()) { - admission.expected_device_generation = - header->expected_device_generation(); - } - admission.deadline = deadline_; - - auto admitted = coordinator_->admit(admission); - admission_decision_ = admitted.decision; - if (!admitted.permit.has_value()) { - const auto reason = admitted.decision.reason == safety::SafetyReason::None - ? safety::SafetyReason::InternalError - : admitted.decision.reason; - rejectBeforeDispatch_( - reason, - admitted.decision.detail, - owns_ledger_record_ - ? grpc::Status::OK - : statusForReason(reason, admitted.decision.detail), - owns_ledger_record_); - return; - } - permit_ = std::move(admitted.permit); - if (owns_ledger_record_) { - (void)coordinator_->commandLedger().setLifecycle( - ledger_ticket_, - safety::CommandLifecycle::Admitted, - admission_decision_.safety_epoch, - admission_decision_.device_generation); - } - should_execute_ = true; - status_ = grpc::Status::OK; -} - -bool GrpcCommandTransaction::beginDispatch() -{ - if (dispatch_guard_.has_value() && dispatch_guard_->acquired()) { - return true; - } - auto guard = beginScopedDispatch(); - if (!guard.acquired()) { - return false; - } - dispatch_guard_.emplace(std::move(guard)); - return true; -} - -bool GrpcCommandTransaction::revalidate() -{ - if (!should_execute_ || completed_ || !permit_.has_value() || - !coordinator_) { - dispatch_status_ = statusForReason( - safety::SafetyReason::InternalError, - "command transaction is not revalidatable"); - return false; - } - const auto check = coordinator_->revalidatePermit(*permit_); - if (!check.safe) { - const auto reason = check.reason == safety::SafetyReason::None - ? safety::SafetyReason::SafetyLatched - : check.reason; - dispatch_status_ = statusForReason(reason, check.detail); - populateFeedback_( - false, - reason, - dispatch_started_ ? safety::CommandLifecycle::Failed - : safety::CommandLifecycle::RejectedBeforeDispatch, - check.detail); - return false; - } - dispatch_status_ = grpc::Status::OK; - return true; -} - -safety::DispatchGuard GrpcCommandTransaction::beginScopedDispatch() -{ - if (!should_execute_ || completed_ || !permit_.has_value() || - !coordinator_) { - dispatch_status_ = statusForReason( - safety::SafetyReason::InternalError, - "command transaction is not dispatchable"); - return {}; - } - auto guard = coordinator_->beginDispatch(*permit_); - if (!guard.acquired()) { - auto hardware = guard.hardwareCheck(); - if (hardware.reason == safety::SafetyReason::None) { - hardware.reason = safety::SafetyReason::SafetyLatched; - } - dispatch_status_ = statusForReason(hardware.reason, hardware.detail); - populateFeedback_( - false, - hardware.reason, - dispatch_started_ ? safety::CommandLifecycle::Failed - : safety::CommandLifecycle::RejectedBeforeDispatch, - hardware.detail); - return guard; - } - const bool first_dispatch = !dispatch_started_; - dispatch_started_ = true; - if (first_dispatch && owns_ledger_record_) { - (void)coordinator_->commandLedger().setLifecycle( - ledger_ticket_, - safety::CommandLifecycle::Dispatching, - admission_decision_.safety_epoch, - admission_decision_.device_generation, - true); - } - dispatch_status_ = grpc::Status::OK; - return guard; -} - -safety::DispatchGuard GrpcCommandTransaction::beginSafetyStopDispatch() -{ - if (!should_execute_ || completed_ || !coordinator_) { - dispatch_status_ = statusForReason( - safety::SafetyReason::InternalError, - "command transaction cannot dispatch an internal safety stop"); - return {}; - } - - safety::AdmissionRequest stop; - stop.command = { - method_policy_.full_method_name + "#internal-stop", - safety::CommandIntent::Stop, - method_policy_.policy_family, - true, - true}; - stop.actor.principal_id = request_context_.principal.id; - stop.actor.authenticated = request_context_.principal.authenticated; - for (const auto role : request_context_.principal.roles) { - stop.actor.roles.emplace_back(toString(role)); - } - const auto base_id = command_id_.empty() - ? request_context_.correlation_id - : command_id_; - stop.command_id = base_id + ":internal-stop:" + - std::to_string(++safety_stop_sequence_); - stop.device_id = device_id_; - stop.deadline = safety::SafetyClock::time_point::max(); - - auto admitted = coordinator_->admit(stop); - if (!admitted.permit.has_value()) { - const auto reason = admitted.decision.reason == safety::SafetyReason::None - ? safety::SafetyReason::SafetyLatched - : admitted.decision.reason; - dispatch_status_ = statusForReason(reason, admitted.decision.detail); - return {}; - } - auto guard = coordinator_->beginDispatch(*admitted.permit); - if (!guard.acquired()) { - const auto& hardware = guard.hardwareCheck(); - const auto reason = hardware.reason == safety::SafetyReason::None - ? safety::SafetyReason::StopUnconfirmed - : hardware.reason; - dispatch_status_ = statusForReason(reason, hardware.detail); - return guard; - } - dispatch_status_ = grpc::Status::OK; - return guard; -} - -grpc::Status GrpcCommandTransaction::finish( - grpc::Status operation_status, - safety::SafetyReason reason, - std::optional lifecycle) -{ - if (completed_) { - return status_; - } - auto* feedback = response_ ? findFeedbackHeader(*response_) : nullptr; - const bool success = feedback && feedback->success() && - operation_status.ok(); - if (reason == safety::SafetyReason::None && !success) { - if (feedback && feedback->reason_code() != - api::COMMAND_REASON_CODE_UNSPECIFIED && - feedback->reason_code() != api::COMMAND_REASON_CODE_NONE) { - reason = fromApiSafetyReason(feedback->reason_code()); - } else { - reason = reasonForStatus(operation_status); - } - } - if (success) { - reason = safety::SafetyReason::None; - } - const bool stop_unconfirmed = - !success && dispatch_started_ && - method_policy_.command_intent == safety::CommandIntent::Stop; - if (stop_unconfirmed) { - reason = safety::SafetyReason::StopUnconfirmed; - if (coordinator_) { - coordinator_->quarantineDevice( - device_id_, reason, command_id_); - } - } - const auto terminal_lifecycle = lifecycle.value_or( - success - ? safety::CommandLifecycle::Completed - : dispatch_started_ ? safety::CommandLifecycle::Failed - : safety::CommandLifecycle::RejectedBeforeDispatch); - const std::string detail = success - ? std::string{} - : feedback && !feedback->error_message().empty() - ? feedback->error_message() - : operation_status.error_message(); - populateFeedback_(success, reason, terminal_lifecycle, detail); - - dispatch_guard_.reset(); - const bool ledger_backed = owns_ledger_record_; - if (ledger_backed && - !completeLedger_( - terminal_lifecycle, reason, detail, dispatch_started_)) { - status_ = statusForReason( - safety::SafetyReason::InternalError, - "command ledger could not store the terminal result"); - completed_ = true; - should_execute_ = false; - return status_; - } - completed_ = true; - should_execute_ = false; - status_ = ledger_backed ? grpc::Status::OK - : std::move(operation_status); - return status_; -} - -grpc::Status GrpcCommandTransaction::finishException( - std::string detail) noexcept -{ - try { - const bool stop_failed = - dispatch_started_ && - method_policy_.command_intent == safety::CommandIntent::Stop; - const bool outcome_unknown = - dispatch_started_ && !stop_failed && - method_policy_.command_intent != - safety::CommandIntent::ResetFault; - const auto reason = stop_failed - ? safety::SafetyReason::StopUnconfirmed - : outcome_unknown ? safety::SafetyReason::OutcomeUnknown - : safety::SafetyReason::InternalError; - const auto lifecycle = outcome_unknown - ? safety::CommandLifecycle::OutcomeUnknown - : dispatch_started_ ? safety::CommandLifecycle::Failed - : safety::CommandLifecycle::RejectedBeforeDispatch; - if ((outcome_unknown || stop_failed) && coordinator_) { - coordinator_->quarantineDevice( - device_id_, reason, command_id_); - } - populateFeedback_(false, reason, lifecycle, detail); - dispatch_guard_.reset(); - const bool ledger_backed = owns_ledger_record_; - if (ledger_backed) { - (void)completeLedger_( - lifecycle, reason, detail, dispatch_started_); - } - completed_ = true; - should_execute_ = false; - status_ = ledger_backed - ? grpc::Status::OK - : statusForReason(reason, detail); - return status_; - } catch (...) { - completed_ = true; - should_execute_ = false; - status_ = grpc::Status( - grpc::StatusCode::INTERNAL, - "command transaction failed while handling an exception"); - return status_; - } -} - -void GrpcCommandTransaction::rejectBeforeDispatch_( - const safety::SafetyReason reason, - std::string detail, - grpc::Status status, - const bool complete_reserved_record) -{ - populateFeedback_( - false, - reason, - safety::CommandLifecycle::RejectedBeforeDispatch, - detail); - if (complete_reserved_record && owns_ledger_record_) { - if (!completeLedger_( - safety::CommandLifecycle::RejectedBeforeDispatch, - reason, - detail, - false)) { - status = statusForReason( - safety::SafetyReason::InternalError, - "command ledger could not store the rejected result"); - } else { - status = grpc::Status::OK; - } - } - status_ = std::move(status); - should_execute_ = false; - completed_ = true; -} - -bool GrpcCommandTransaction::restoreOutcome_( - const safety::CommandOutcome& outcome) -{ - if (!response_) { - return false; - } - if (!outcome.serialized_response.empty()) { - response_->Clear(); - if (!response_->ParseFromString(outcome.serialized_response)) { - return false; - } - } - const bool success = - outcome.lifecycle == safety::CommandLifecycle::Completed; - populateFeedback_( - success, - outcome.reason, - outcome.lifecycle, - outcome.detail); - return true; -} - -bool GrpcCommandTransaction::completeLedger_( - const safety::CommandLifecycle lifecycle, - const safety::SafetyReason reason, - const std::string& detail, - const bool hardware_submission_possible) noexcept -{ - try { - if (!owns_ledger_record_ || !coordinator_ || !response_) { - return !owns_ledger_record_; - } - std::string serialized; - if (!deterministicSerialize(*response_, serialized)) { - return false; - } - safety::CommandOutcome outcome; - outcome.lifecycle = lifecycle; - outcome.reason = reason; - outcome.detail = detail; - outcome.serialized_response = std::move(serialized); - outcome.safety_epoch = admission_decision_.safety_epoch; - outcome.device_generation = admission_decision_.device_generation; - outcome.hardware_submission_possible = hardware_submission_possible; - const bool completed = coordinator_->commandLedger().complete( - ledger_ticket_, std::move(outcome)); - if (completed) { - owns_ledger_record_ = false; - } - return completed; - } catch (...) { - return false; - } -} - -void GrpcCommandTransaction::populateFeedback_( - const bool success, - const safety::SafetyReason reason, - const safety::CommandLifecycle lifecycle, - const std::string& detail) -{ - if (!response_) { - return; - } - auto* feedback = findFeedbackHeader(*response_); - if (!feedback) { - return; - } - feedback->set_success(success); - if (!success && feedback->error_message().empty()) { - feedback->set_error_message( - detail.empty() ? safety::toString(reason) : detail); - } - if (success) { - feedback->clear_error_message(); - } - if (!feedback->has_timestamp()) { - *feedback->mutable_timestamp() = - google::protobuf::util::TimeUtil::GetCurrentTime(); - } - feedback->set_reason_code(toApiSafetyReason(reason)); - feedback->set_command_id(command_id_); - if (coordinator_) { - feedback->set_service_instance_id(coordinator_->serviceInstanceId()); - } - feedback->set_safety_epoch(admission_decision_.safety_epoch); - feedback->set_device_generation( - admission_decision_.device_generation); - feedback->set_execution_state(toApiLifecycle(lifecycle)); -} - -void GrpcCommandTransaction::abandon_() noexcept -{ - if (completed_ || !owns_ledger_record_) { - return; - } - (void)finishException( - dispatch_started_ - ? "command handler exited after crossing the hardware dispatch fence without recording a terminal result" - : "command handler exited without recording a terminal result"); -} - -grpc::Status executeRegisteredGrpcCommand( - const std::shared_ptr& gateway, - grpc::ServerContext* server_context, - safety::SafetyManager& coordinator, - const std::string& full_method_name, - const google::protobuf::Message* request, - google::protobuf::Message* response, - GrpcUnaryCommandOperation operation) -{ - const auto call = beginRegisteredGrpcCall( - gateway, server_context, full_method_name); - if (!call.allowed()) { - return call.status(); - } - if (!request || !response || !operation) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - "gRPC command transaction received a null argument"); - } - const auto policy = - defaultGrpcMethodPolicyRegistry().find(full_method_name); - if (!policy.has_value() || !policy->mutating) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - "gRPC command transaction requires a registered mutating method"); - } - - GrpcCommandTransaction transaction( - coordinator, call.context(), *policy, *request, *response); - if (!transaction.shouldExecute()) { - return transaction.status(); - } - try { - return transaction.finish(operation(transaction)); - } catch (const std::exception& error) { - return transaction.finishException(error.what()); - } catch (...) { - return transaction.finishException( - "command handler threw an unknown exception"); - } -} - -grpc::Status executeServerDerivedGrpcCommand( - const std::shared_ptr& gateway, - grpc::ServerContext* server_context, - safety::SafetyManager& coordinator, - const std::string& full_method_name, - GrpcMethodPolicy effective_policy, - const google::protobuf::Message* request, - google::protobuf::Message* response, - GrpcUnaryCommandOperation operation) -{ - const auto call = beginRegisteredGrpcCall( - gateway, server_context, full_method_name); - if (!call.allowed()) { - return call.status(); - } - if (!request || !response || !operation) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - "server-derived gRPC command received a null argument"); - } - const auto registered = - defaultGrpcMethodPolicyRegistry().find(full_method_name); - if (!registered.has_value() || !registered->mutating || - effective_policy.full_method_name != full_method_name || - !effective_policy.mutating || - effective_policy.policy_family != registered->policy_family) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - "server-derived gRPC command policy is inconsistent with the registered method"); - } - const bool valid_safety_lane = - !effective_policy.safety_lane || - effective_policy.command_intent == safety::CommandIntent::Stop || - effective_policy.command_intent == safety::CommandIntent::ResetFault || - effective_policy.command_intent == - safety::CommandIntent::RecoverAdmission; - if (!valid_safety_lane) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - "server-derived gRPC command has an invalid safety lane"); - } - - GrpcCommandTransaction transaction( - coordinator, - call.context(), - std::move(effective_policy), - *request, - *response); - if (!transaction.shouldExecute()) { - return transaction.status(); - } - try { - return transaction.finish(operation(transaction)); - } catch (const std::exception& error) { - return transaction.finishException(error.what()); - } catch (...) { - return transaction.finishException( - "server-derived command handler threw an unknown exception"); - } -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_error_logging_interceptor.cpp b/cmvr-es/service/grpc/server/src/grpc_error_logging_interceptor.cpp deleted file mode 100644 index d8a7352a..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_error_logging_interceptor.cpp +++ /dev/null @@ -1,504 +0,0 @@ -#include "service/grpc/server/include/grpc_error_logging_interceptor.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "cmvr/api/common.pb.h" -#include "common/base/logging/logger.h" - -namespace cmvr::service { - -namespace { - -using Hook = grpc::experimental::InterceptionHookPoints; -using CodedInputStream = google::protobuf::io::CodedInputStream; -using WireFormatLite = google::protobuf::internal::WireFormatLite; - -constexpr std::string_view kFeedbackType = - "cmvr.api.CommandHeader.Feedback"; -constexpr std::size_t kMaxLogDetailBytes = 1024; -constexpr int kMaxFeedbackBytes = 64 * 1024; - -enum class ResponseShape { - NONE, - DIRECT_FEEDBACK, - ENVELOPE, -}; - -struct ApplicationFeedback { - bool present{false}; - bool parsed{false}; - bool success{false}; - std::string error_message; -}; - -ApplicationFeedback feedbackFromProto( - const api::CommandHeader_Feedback& message) -{ - return ApplicationFeedback{ - true, true, message.success(), message.error_message()}; -} - -std::string boundedDetail(const std::string& detail) -{ - if (detail.size() <= kMaxLogDetailBytes) { - return detail; - } - - constexpr std::string_view suffix = "... [truncated]"; - std::size_t length = kMaxLogDetailBytes - suffix.size(); - while (length > 0 && - (static_cast(detail[length]) & 0xc0U) == 0x80U) { - --length; - } - return detail.substr(0, length) + std::string(suffix); -} - -const char* failureKindName(const GrpcFailureKind kind) -{ - switch (kind) { - case GrpcFailureKind::APPLICATION: - return "application"; - case GrpcFailureKind::GRPC_STATUS: - return "grpc_status"; - case GrpcFailureKind::MALFORMED_RESPONSE: - return "malformed_response"; - } - return "unknown"; -} - -const char* statusCodeName(const grpc::StatusCode code) -{ - switch (code) { - case grpc::StatusCode::OK: return "OK"; - case grpc::StatusCode::CANCELLED: return "CANCELLED"; - case grpc::StatusCode::UNKNOWN: return "UNKNOWN"; - case grpc::StatusCode::INVALID_ARGUMENT: return "INVALID_ARGUMENT"; - case grpc::StatusCode::DEADLINE_EXCEEDED: return "DEADLINE_EXCEEDED"; - case grpc::StatusCode::NOT_FOUND: return "NOT_FOUND"; - case grpc::StatusCode::ALREADY_EXISTS: return "ALREADY_EXISTS"; - case grpc::StatusCode::PERMISSION_DENIED: return "PERMISSION_DENIED"; - case grpc::StatusCode::RESOURCE_EXHAUSTED: return "RESOURCE_EXHAUSTED"; - case grpc::StatusCode::FAILED_PRECONDITION: return "FAILED_PRECONDITION"; - case grpc::StatusCode::ABORTED: return "ABORTED"; - case grpc::StatusCode::OUT_OF_RANGE: return "OUT_OF_RANGE"; - case grpc::StatusCode::UNIMPLEMENTED: return "UNIMPLEMENTED"; - case grpc::StatusCode::INTERNAL: return "INTERNAL"; - case grpc::StatusCode::UNAVAILABLE: return "UNAVAILABLE"; - case grpc::StatusCode::DATA_LOSS: return "DATA_LOSS"; - case grpc::StatusCode::UNAUTHENTICATED: return "UNAUTHENTICATED"; - case grpc::StatusCode::DO_NOT_USE: break; - } - return "UNKNOWN_CODE"; -} - -void logFailure(const GrpcFailureRecord& record) -{ - std::ostringstream message; - message << "[gRPC] request failed, method=" << record.method - << ", peer=" << (record.peer.empty() ? "unknown" : record.peer) - << ", kind=" << failureKindName(record.kind) - << ", code=" << static_cast(record.status_code) - << '(' << statusCodeName(record.status_code) << ')' - << ", detail=" << boundedDetail(record.detail); - switch (record.severity) { - case GrpcFailureSeverity::INFO: - CMVR_LOG(INFO) << message.str(); - break; - case GrpcFailureSeverity::WARNING: - CMVR_LOG(WARNING) << message.str(); - logging::Logger::instance().flush(); - break; - case GrpcFailureSeverity::ERROR: - CMVR_LOG(ERROR) << message.str(); - break; - } -} - -GrpcFailureSeverity severityForStatus(const grpc::StatusCode code) -{ - switch (code) { - case grpc::StatusCode::CANCELLED: - case grpc::StatusCode::INVALID_ARGUMENT: - case grpc::StatusCode::DEADLINE_EXCEEDED: - case grpc::StatusCode::NOT_FOUND: - case grpc::StatusCode::ALREADY_EXISTS: - case grpc::StatusCode::PERMISSION_DENIED: - case grpc::StatusCode::RESOURCE_EXHAUSTED: - case grpc::StatusCode::FAILED_PRECONDITION: - case grpc::StatusCode::ABORTED: - case grpc::StatusCode::OUT_OF_RANGE: - case grpc::StatusCode::UNIMPLEMENTED: - case grpc::StatusCode::UNAVAILABLE: - case grpc::StatusCode::UNAUTHENTICATED: - return GrpcFailureSeverity::WARNING; - case grpc::StatusCode::OK: - case grpc::StatusCode::UNKNOWN: - case grpc::StatusCode::INTERNAL: - case grpc::StatusCode::DATA_LOSS: - case grpc::StatusCode::DO_NOT_USE: - return GrpcFailureSeverity::ERROR; - } - return GrpcFailureSeverity::ERROR; -} - -bool splitMethodName(const std::string_view full_method, - std::string_view& service_name, - std::string_view& method_name) -{ - if (full_method.empty()) { - return false; - } - const std::size_t service_begin = full_method.front() == '/' ? 1 : 0; - const std::size_t separator = full_method.find('/', service_begin); - if (separator == std::string_view::npos || - separator == service_begin || separator + 1 >= full_method.size()) { - return false; - } - service_name = full_method.substr(service_begin, separator - service_begin); - method_name = full_method.substr(separator + 1); - return true; -} - -ResponseShape responseShapeForMethod(const std::string_view full_method) -{ - std::string_view service_name; - std::string_view method_name; - if (!splitMethodName(full_method, service_name, method_name)) { - return ResponseShape::NONE; - } - - const auto* pool = google::protobuf::DescriptorPool::generated_pool(); - const auto* service = pool->FindServiceByName(std::string(service_name)); - if (service == nullptr) { - return ResponseShape::NONE; - } - const auto* method = service->FindMethodByName(std::string(method_name)); - if (method == nullptr || method->output_type() == nullptr) { - return ResponseShape::NONE; - } - - const auto* output = method->output_type(); - if (output->full_name() == kFeedbackType) { - return ResponseShape::DIRECT_FEEDBACK; - } - - const auto* header = output->FindFieldByNumber(1); - if (header == nullptr || header->name() != "header" || - header->is_repeated() || - header->cpp_type() != google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE || - header->message_type() == nullptr || - header->message_type()->full_name() != kFeedbackType) { - return ResponseShape::NONE; - } - return ResponseShape::ENVELOPE; -} - -bool mergeFeedback(CodedInputStream& input, - api::CommandHeader_Feedback& feedback) -{ - return feedback.MergePartialFromCodedStream(&input) && - input.ConsumedEntireMessage(); -} - -bool mergeBoundedFeedback(CodedInputStream& input, - api::CommandHeader_Feedback& feedback) -{ - std::uint32_t length = 0; - if (!input.ReadVarint32(&length) || - length > static_cast(kMaxFeedbackBytes) || - length > static_cast(std::numeric_limits::max())) { - return false; - } - - std::string payload; - if (!input.ReadString(&payload, static_cast(length))) { - return false; - } - return feedback.MergeFromString(payload); -} - -ApplicationFeedback parseDirectFeedback(grpc::ByteBuffer& buffer) -{ - ApplicationFeedback feedback; - grpc::ProtoBufferReader reader(&buffer); - if (!reader.status().ok()) { - return feedback; - } - CodedInputStream input(&reader); - input.SetTotalBytesLimit(kMaxFeedbackBytes); - api::CommandHeader_Feedback message; - if (!mergeFeedback(input, message)) { - feedback.present = true; - return feedback; - } - return feedbackFromProto(message); -} - -ApplicationFeedback parseEnvelopeFeedback(grpc::ByteBuffer& buffer) -{ - ApplicationFeedback feedback; - grpc::ProtoBufferReader reader(&buffer); - if (!reader.status().ok()) { - return feedback; - } - CodedInputStream input(&reader); - const std::uint32_t tag = input.ReadTag(); - if (tag == 0 || WireFormatLite::GetTagFieldNumber(tag) != 1) { - feedback.parsed = true; - return feedback; - } - - feedback.present = true; - if (WireFormatLite::GetTagWireType(tag) != - WireFormatLite::WIRETYPE_LENGTH_DELIMITED) { - return feedback; - } - - api::CommandHeader_Feedback message; - if (!mergeBoundedFeedback(input, message)) { - return feedback; - } - return feedbackFromProto(message); -} - -class GrpcErrorLoggingInterceptor final - : public grpc::experimental::Interceptor { -public: - GrpcErrorLoggingInterceptor(std::string method, - grpc::ServerContextBase* context, - const ResponseShape response_shape, - const bool server_streaming, - const bool client_streaming, - GrpcFailureSink sink) - : method_(std::move(method)), - context_(context), - peer_(context == nullptr ? std::string{} : context->peer()), - response_shape_(response_shape), - server_streaming_(server_streaming), - client_streaming_(client_streaming), - sink_(std::move(sink)) - { - } - - void Intercept(grpc::experimental::InterceptorBatchMethods* methods) override - { - if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_CANCEL)) { - // gRPC forbids delaying this hook. Only publish the signal here; - // the final status hook performs any logging. - server_cancel_requested_.store(true, std::memory_order_release); - return; - } - if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_MESSAGE)) { - inspectResponse(methods->GetSerializedSendMessage()); - } - if (methods->QueryInterceptionHookPoint(Hook::PRE_SEND_STATUS)) { - inspectStatus(methods->GetSendStatus()); - } - methods->Proceed(); - } - -private: - void emit(GrpcFailureRecord record) - { - record.method = method_; - record.peer = peer_; - record.detail = boundedDetail(record.detail); - if (sink_) { - std::lock_guard lock(sink_mutex_); - sink_(record); - } else { - logFailure(record); - } - } - - void inspectResponse(grpc::ByteBuffer* buffer) - { - if (response_shape_ == ResponseShape::NONE || - buffer == nullptr || !buffer->Valid()) { - return; - } - - ApplicationFeedback feedback = - response_shape_ == ResponseShape::DIRECT_FEEDBACK - ? parseDirectFeedback(*buffer) - : parseEnvelopeFeedback(*buffer); - std::optional failure; - if (!feedback.present) { - failure = GrpcFailureRecord{ - GrpcFailureKind::MALFORMED_RESPONSE, - {}, {}, grpc::StatusCode::INTERNAL, - GrpcFailureSeverity::ERROR, - "response is missing CommandHeader.Feedback"}; - } else if (!feedback.parsed) { - failure = GrpcFailureRecord{ - GrpcFailureKind::MALFORMED_RESPONSE, - {}, {}, grpc::StatusCode::INTERNAL, - GrpcFailureSeverity::ERROR, - "response contains an invalid CommandHeader.Feedback"}; - } else if (!feedback.success) { - failure = GrpcFailureRecord{ - GrpcFailureKind::APPLICATION, - {}, {}, grpc::StatusCode::OK, - GrpcFailureSeverity::ERROR, - feedback.error_message.empty() - ? "operation failed without an error message" - : feedback.error_message}; - } - - if (!failure.has_value()) { - return; - } - if (server_streaming_) { - emit(std::move(*failure)); - return; - } - pending_failure_ = std::move(*failure); - } - - void inspectStatus(const grpc::Status& status) - { - const grpc::StatusCode status_code = status.error_code(); - std::string status_detail = status.error_message(); - if (!status.ok() && status_detail.empty()) { - status_detail = "RPC completed with a non-OK status"; - } - - if (!status.ok()) { - // A non-OK unary/client-streaming status suppresses the response - // body, so only the status is visible to the platform. - pending_failure_.reset(); - if (status_code == grpc::StatusCode::CANCELLED) { - emitCancellationOnce(status_code, std::move(status_detail)); - return; - } - emit({GrpcFailureKind::GRPC_STATUS, - {}, {}, status_code, - severityForStatus(status_code), - status_detail}); - return; - } - - const bool deadline_expired = - context_ != nullptr && - context_->deadline() <= std::chrono::system_clock::now(); - // IsCancelled() waits for the client close on the synchronous API. - // Unary requests have already sent that close, but querying it here - // could block an early client-streaming/bidi response indefinitely. - if (deadline_expired || - (context_ != nullptr && !client_streaming_ && - context_->IsCancelled())) { - pending_failure_.reset(); - const grpc::StatusCode code = deadline_expired - ? grpc::StatusCode::DEADLINE_EXCEEDED - : grpc::StatusCode::CANCELLED; - std::string detail; - if (deadline_expired) { - detail = "RPC deadline expired before completion"; - } else if (server_cancel_requested_.load( - std::memory_order_acquire)) { - detail = - "RPC cancellation was requested by the edge server before completion"; - } else { - detail = - "RPC was cancelled by the client or transport before completion"; - } - emitCancellationOnce(code, std::move(detail)); - return; - } - - if (server_cancel_requested_.load(std::memory_order_acquire)) { - pending_failure_.reset(); - emitCancellationOnce( - grpc::StatusCode::CANCELLED, - "RPC cancellation was requested by the edge server before completion"); - return; - } - - if (pending_failure_.has_value()) { - emit(std::move(*pending_failure_)); - pending_failure_.reset(); - } - } - - void emitCancellationOnce(const grpc::StatusCode code, std::string detail) - { - if (cancellation_recorded_.exchange(true, std::memory_order_acq_rel)) { - return; - } - emit({GrpcFailureKind::GRPC_STATUS, - {}, {}, code, severityForStatus(code), std::move(detail)}); - } - - std::string method_; - grpc::ServerContextBase* context_{nullptr}; - std::string peer_; - ResponseShape response_shape_{ResponseShape::NONE}; - bool server_streaming_{false}; - bool client_streaming_{false}; - GrpcFailureSink sink_; - std::optional pending_failure_; - std::mutex sink_mutex_; - std::atomic server_cancel_requested_{false}; - std::atomic cancellation_recorded_{false}; -}; - -class GrpcErrorLoggingInterceptorFactory final - : public grpc::experimental::ServerInterceptorFactoryInterface { -public: - explicit GrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink) - : sink_(std::move(sink)) - { - } - - grpc::experimental::Interceptor* CreateServerInterceptor( - grpc::experimental::ServerRpcInfo* info) override - { - const std::string method = - info != nullptr && info->method() != nullptr - ? info->method() - : ""; - const bool server_streaming = - info != nullptr && - (info->type() == grpc::experimental::ServerRpcInfo::Type::SERVER_STREAMING || - info->type() == grpc::experimental::ServerRpcInfo::Type::BIDI_STREAMING); - const bool client_streaming = - info != nullptr && - (info->type() == grpc::experimental::ServerRpcInfo::Type::CLIENT_STREAMING || - info->type() == grpc::experimental::ServerRpcInfo::Type::BIDI_STREAMING); - return new GrpcErrorLoggingInterceptor( - method, - info == nullptr ? nullptr : info->server_context(), - responseShapeForMethod(method), server_streaming, - client_streaming, sink_); - } - -private: - GrpcFailureSink sink_; -}; - -} // namespace - -std::unique_ptr -makeGrpcErrorLoggingInterceptorFactory(GrpcFailureSink sink) -{ - return std::make_unique( - std::move(sink)); -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_head_service.cpp b/cmvr-es/service/grpc/server/src/grpc_head_service.cpp deleted file mode 100644 index edfe5198..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_head_service.cpp +++ /dev/null @@ -1,561 +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 "service/grpc/server/include/grpc_command_transaction.h" -#include "service/grpc/server/include/media_activity_coordinator.h" -#include "service/grpc/server/include/grpc_security.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" -#include -#include -#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) { - 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; -} - -std::optional admitHeadCommand( - const std::shared_ptr& robot) -{ - auto admission = globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting() || !robot) { - return std::nullopt; - } - return robot->beginOperationalActivity(); -} - -template -grpc::Status failStoppedCommand(ResponseT* response) -{ - return failResponse( - response, - "Biohead command was rejected because StopAll is in progress or " - "the command was preempted"); -} - -template -grpc::Status executeHeadOperationalCommand( - const std::shared_ptr& security_gateway, - grpc::ServerContext* context, - DeviceManager& device_manager, - const char* full_method_name, - const char* rpc_name, - const RequestT* request, - ResponseT* response, - Operation&& operation) -{ - return executeRegisteredGrpcCommand( - security_gateway, context, device_manager.safetyManager(), - full_method_name, request, response, - [&device_manager, request, response, rpc_name, - operation = std::forward(operation)]( - GrpcCommandTransaction& command) mutable { - const std::string device_id = request->header().device_id(); - const auto robot = - device_manager.getDevice(device_id); - if (!robot) { - return failResponse( - response, "Biohead device not found: " + device_id); - } - - const auto activity = admitHeadCommand(robot); - if (!activity) { - return failStoppedCommand(response); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - if (!operation(*robot, *activity)) { - return failStoppedCommand(response); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp( - response->mutable_header()->mutable_timestamp()); - logSuccess(rpc_name, device_id); - return grpc::Status::OK; - }); -} -} - -gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl() - : gRPCMBioHeadServiceImpl(makeDefaultGrpcSecurityGateway()) {} - -gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl( - std::shared_ptr security_gateway) - : dmgr_(DeviceManager::getInstance()), - security_gateway_(security_gateway - ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()) {} - - -// 设置表情（一次性） -grpc::Status gRPCMBioHeadServiceImpl::SetExpression( - grpc::ServerContext* context, - const SetFacialExpression_Request* request, - SetFacialExpression_Feedback* response) { - return executeHeadOperationalCommand( - security_gateway_, context, dmgr_, - "/cmvr.api.BioHeadService/SetExpression", "SetExpression", - request, response, - [request](AbstractBiohead& robot, - const AbstractBiohead::OperationalToken activity) { - FacialExpressionState 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(); - return robot.setExpressionPoseIfCurrent( - activity, expression_state); - }); -} - - -// 流式控制接口 -grpc::Status gRPCMBioHeadServiceImpl::StreamExpression( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.BioHeadService/StreamExpression"); - StreamFacialExpression_Feedback feedback_msg; - std::string dev_id; - std::shared_ptr robot; - bool first_message = true; - AbstractBiohead::OperationalToken activity{0U}; - std::optional safety_session; - auto media_session = globalMediaActivityCoordinator().beginSession( - [context] { - if (context) { - context->TryCancel(); - } - }); - if (!media_session) { - return grpc::Status( - grpc::StatusCode::UNAVAILABLE, - "Biohead stream rejected because StopAll is in progress"); - } - - 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() - time_interval; - - CMVR_LOG(INFO) << "StreamExpression started."; - - while (stream->Read(&request_msg)) { - if (first_message) { - dev_id = request_msg.header().device_id(); - if (dev_id.empty()) { - 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; - 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; - } - - const auto admitted = admitHeadCommand(robot); - if (!admitted) { - feedback_msg.mutable_header()->set_success(false); - feedback_msg.mutable_header()->set_error_message( - "Biohead stream rejected because StopAll is in " - "progress"); - setCurrentTimestamp( - feedback_msg.mutable_header()->mutable_timestamp()); - stream->Write(feedback_msg); - return grpc::Status::OK; - } - activity = *admitted; - - const auto& header = request_msg.header(); - GrpcStreamingSafetyOpen safety_open; - safety_open.full_method_name = - "/cmvr.api.BioHeadService/StreamExpression"; - safety_open.device_id = dev_id; - safety_open.session_id = header.command_id().empty() - ? cmvr_grpc_call_guard.context().correlation_id - : header.command_id(); - safety_open.expected_service_instance_id = - header.expected_service_instance_id(); - if (header.has_expected_device_generation()) { - safety_open.expected_device_generation = - header.expected_device_generation(); - } - safety_open.authority_generation = activity; - safety_open.deadline = - cmvr_grpc_call_guard.context().deadline; - safety_session.emplace( - dmgr_.safetyManager(), - cmvr_grpc_call_guard.context(), - std::move(safety_open)); - if (!safety_session->admitted()) { - feedback_msg.mutable_header()->set_success(false); - feedback_msg.mutable_header()->set_error_message( - safety_session->status().error_message()); - setCurrentTimestamp( - feedback_msg.mutable_header()->mutable_timestamp()); - stream->Write(feedback_msg); - return safety_session->status(); - } - - first_message = false; - CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): streaming success, id=" << dev_id; - } else if (!request_msg.header().device_id().empty() && - request_msg.header().device_id() != dev_id) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "biohead stream cannot change device_id after its first frame"); - } - - // ✅ 如果紧急停止触发，直接退出 - if (media_session.cancelled() || - (context && context->IsCancelled())) { - CMVR_LOG(WARNING) << "[Stream] Emergency stop requested. Terminating stream for device: " << dev_id; - break; - } - - if (!safety_session || !safety_session->revalidate()) { - const auto status = safety_session - ? safety_session->status() - : grpc::Status( - grpc::StatusCode::INTERNAL, - "biohead stream safety session was not initialized"); - feedback_msg.mutable_header()->set_success(false); - feedback_msg.mutable_header()->set_error_message( - status.error_message()); - setCurrentTimestamp( - feedback_msg.mutable_header()->mutable_timestamp()); - stream->Write(feedback_msg); - return status; - } - - 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(); - - bool dispatched = false; - grpc::Status dispatch_status = grpc::Status::OK; - const bool current_session = media_session.runIfCurrent([&] { - auto dispatch = safety_session->beginDispatch(); - if (!dispatch.acquired()) { - dispatch_status = safety_session->status(); - return; - } - dispatched = robot->streamFacialPoseIfCurrent( - activity, expression_state, 0, 0); - }); - if (!dispatch_status.ok()) { - feedback_msg.mutable_header()->set_success(false); - feedback_msg.mutable_header()->set_error_message( - dispatch_status.error_message()); - setCurrentTimestamp( - feedback_msg.mutable_header()->mutable_timestamp()); - stream->Write(feedback_msg); - return dispatch_status; - } - if (!current_session || !dispatched) { - CMVR_LOG(WARNING) - << "[gRPCMBioHeadServiceImpl] StreamExpression was " - "preempted, id=" - << dev_id; - break; - } - 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) { - 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) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.BioHeadService/GetSystemStatus"); - 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) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.BioHeadService/EmergencyStop", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const string dev_id = request->header().device_id(); - const auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse( - response, "Biohead device not found: " + dev_id); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - if (!robot->stopOperationalActivity()) { - return failResponse( - response, - "Biohead could not confirm that operational activity " - "stopped: " + dev_id); - } - response->mutable_header()->set_success(true); - setCurrentTimestamp( - response->mutable_header()->mutable_timestamp()); - logSuccess("EmergencyStop", dev_id); - return grpc::Status::OK; - }); -} - -grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response) -{ - return executeHeadOperationalCommand( - security_gateway_, context, dmgr_, - "/cmvr.api.BioHeadService/SpeakStart", "SpeakStart", - request, response, - [](AbstractBiohead& robot, - const AbstractBiohead::OperationalToken activity) { - return robot.speakStartIfCurrent(activity); - }); -} - - - -grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.BioHeadService/SpeakStop", request, response, - [this, request, response](GrpcCommandTransaction& command) { - const string dev_id = request->header().device_id(); - const auto robot = dmgr_.getDevice(dev_id); - if (!robot) { - return failResponse( - response, "Biohead device not found: " + dev_id); - } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - robot->speakstop(); - response->mutable_header()->set_success(true); - setCurrentTimestamp( - response->mutable_header()->mutable_timestamp()); - logSuccess("SpeakStop", dev_id); - return grpc::Status::OK; - }); -} - - - -grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response) -{ - return executeHeadOperationalCommand( - security_gateway_, context, dmgr_, - "/cmvr.api.BioHeadService/Happy", "Happy", request, response, - [](AbstractBiohead& robot, - const AbstractBiohead::OperationalToken activity) { - return robot.expressionHappyIfCurrent(activity); - }); -} - -grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response) -{ - return executeHeadOperationalCommand( - security_gateway_, context, dmgr_, - "/cmvr.api.BioHeadService/Surprise", "Surprise", request, - response, - [](AbstractBiohead& robot, - const AbstractBiohead::OperationalToken activity) { - return robot.expressionSurprisedIfCurrent(activity); - }); -} - - -grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response) -{ - return executeHeadOperationalCommand( - security_gateway_, context, dmgr_, - "/cmvr.api.BioHeadService/ExpressionTired", "ExpressionTired", - request, response, - [](AbstractBiohead& robot, - const AbstractBiohead::OperationalToken activity) { - return robot.expressionTiredIfCurrent(activity); - }); -} - - - -grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* context, const cmvr::api::ExpressionAngry_Request* request, cmvr::api::ExpressionAngry_Feedback* response) -{ - return executeHeadOperationalCommand( - security_gateway_, context, dmgr_, - "/cmvr.api.BioHeadService/ExpressionAngry", "ExpressionAngry", - request, response, - [](AbstractBiohead& robot, - const AbstractBiohead::OperationalToken activity) { - return robot.expressionAngryIfCurrent(activity); - }); -} - - - -grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* context, const cmvr::api::ExpressionSadness_Request* request, cmvr::api::ExpressionSadness_Feedback* response) -{ - return executeHeadOperationalCommand( - security_gateway_, context, dmgr_, - "/cmvr.api.BioHeadService/ExpressionSadness", - "ExpressionSadness", request, response, - [](AbstractBiohead& robot, - const AbstractBiohead::OperationalToken activity) { - return robot.expressionSadnessIfCurrent(activity); - }); -} - - -grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* context, const cmvr::api::ExpressionYawn_Request* request, cmvr::api::ExpressionYawn_Feedback* response) -{ - return executeHeadOperationalCommand( - security_gateway_, context, dmgr_, - "/cmvr.api.BioHeadService/ExpressionYawn", "ExpressionYawn", - request, response, - [](AbstractBiohead& robot, - const AbstractBiohead::OperationalToken activity) { - return robot.expressionYawnIfCurrent(activity); - }); -} diff --git a/cmvr-es/service/grpc/server/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/server/src/grpc_hlc_service.cpp deleted file mode 100644 index e4e4d7f1..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_hlc_service.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// -// Created by lgv on 2025/8/25. -// - - -#include "../include/grpc_hlc_service.h" - -#include -#include -#include -#include - -#include - -#include "common/base/logging/logger.h" -#include "manager/task_manager/include/task_manager.h" -#include "service/grpc/server/include/grpc_command_transaction.h" -#include "service/grpc/server/include/grpc_security.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.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() - : gRPCHlcServiceImpl(makeDefaultGrpcSecurityGateway()) {} - -gRPCHlcServiceImpl::gRPCHlcServiceImpl( - std::shared_ptr security_gateway) - : dmgr_(device::DeviceManager::getInstance()), - security_gateway_(security_gateway - ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()) {} - -grpc::Status gRPCHlcServiceImpl::touch( - grpc::ServerContext* context, - const cmvr::api::Touch_Request* request, - cmvr::api::Touch_Response* response) -{ - if (!request || !response) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - "touch received a null request or response"); - } - 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); - } - const auto arm_id = touch_task->controlDeviceId(); - if (arm_id.empty()) { - const std::string error = - "TouchScreenTask has no resolved arm safety target"; - fillTouchResponse(response, false, error); - return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); - } - - // HLC is a composite command. The server-resolved arm, rather than a - // caller-supplied task alias, is the authority and safety target. - api::Touch_Request normalized_request = *request; - if (!request->header().device_id().empty() && - request->header().device_id() != arm_id && - request->header().device_id() != touch_task->id()) { - CMVR_LOG(WARNING) - << "[gRPCHlcServiceImpl] ignoring composite touch target alias '" - << request->header().device_id() << "'; resolved arm=" << arm_id; - } - normalized_request.mutable_header()->set_device_id(arm_id); - - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.HlcService/touch", &normalized_request, response, - [context, request, response, touch_task]( - GrpcCommandTransaction& command) { - try { - auto& admission_gate = globalStopAllAdmissionGate(); - std::uint64_t admission_generation = 0U; - { - auto admission = admission_gate.lockAdmission(); - if (!admission.accepting()) { - const std::string error = - "TouchScreenTask is temporarily paused by StopAll"; - fillTouchResponse(response, false, error); - return grpc::Status(grpc::StatusCode::UNAVAILABLE, error); - } - admission_generation = admission.generation(); - } - - task::TouchScreenTask::SafetyHooks safety_hooks; - safety_hooks.revalidate = [&command] { - return command.revalidate(); - }; - safety_hooks.dispatch_actuation = [&command]( - const task::TouchScreenTask::SafetyHooks::HardwareOperation& - operation) { - auto dispatch = command.beginScopedDispatch(); - return dispatch.acquired() && operation(); - }; - safety_hooks.dispatch_stop = [&command]( - const task::TouchScreenTask::SafetyHooks::HardwareOperation& - operation) { - auto dispatch = command.beginSafetyStopDispatch(); - return dispatch.acquired() && operation(); - }; - - if (!touch_task->touchIfCurrent( - request->u(), request->v(), - [&admission_gate, admission_generation, &command] { - auto admission = admission_gate.lockAdmission(); - return admission.accepting() && - admission.generation() == admission_generation && - command.revalidate(); - }, - std::move(safety_hooks))) { - const std::string error = - buildTouchFailureMessage(*touch_task, "TouchScreenTask touch request rejected"); - fillTouchResponse(response, false, error); - return command.dispatchStatus().ok() - ? grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, error) - : command.dispatchStatus(); - } - - while (touch_task->isBusy()) { - if (context != nullptr && context->IsCancelled()) { - const bool stopped = touch_task->stopActivity(); - const std::string error = "touch request cancelled"; - fillTouchResponse(response, false, error); - return grpc::Status( - stopped ? grpc::StatusCode::CANCELLED - : grpc::StatusCode::ABORTED, - stopped ? error - : error + "; arm stop was not confirmed"); - } - if (!command.revalidate()) { - (void)touch_task->stopActivity(); - fillTouchResponse( - response, - false, - buildTouchFailureMessage( - *touch_task, - "TouchScreenTask safety admission was revoked")); - return command.dispatchStatus(); - } - 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 command.dispatchStatus().ok() - ? grpc::Status(grpc::StatusCode::INTERNAL, error) - : command.dispatchStatus(); - } - - 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 (...) { - (void)touch_task->stopActivity(); - throw; - } - }); -} diff --git a/cmvr-es/service/grpc/server/src/grpc_motor_service.cpp b/cmvr-es/service/grpc/server/src/grpc_motor_service.cpp deleted file mode 100644 index 64b67c6c..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_motor_service.cpp +++ /dev/null @@ -1,2331 +0,0 @@ -#include "service/grpc/server/include/grpc_motor_service.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "common/base/logging/logger.h" -#include "devices/motor/manager/include/motor_manager.h" -#include "manager/device_manager/include/device_manager.h" -#include "service/grpc/server/include/grpc_command_transaction.h" -#include "service/grpc/server/include/grpc_security.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { - -namespace { - -using Clock = std::chrono::steady_clock; -using google::protobuf::util::TimeUtil; - -constexpr std::uint32_t kDefaultCommandTimeoutMs = 30000; -constexpr std::uint32_t kDefaultPollPeriodMs = 10; -constexpr std::uint32_t kDefaultSettleSamples = 3; -constexpr double kDefaultPositionToleranceRad = 1e-3; -constexpr double kDefaultVelocityToleranceRadS = 1e-2; -constexpr std::uint32_t kDefaultStreamWatchdogMs = 500; - -void fillFeedback(api::CommandHeader_Feedback* feedback, - const bool success, - const std::string& error = {}) -{ - feedback->set_success(success); - feedback->set_error_message(error); - *feedback->mutable_timestamp() = TimeUtil::GetCurrentTime(); -} - -std::uint64_t elapsedMs(const Clock::time_point started) -{ - return static_cast( - std::chrono::duration_cast(Clock::now() - started).count()); -} - -bool isFinite(const double value) -{ - return std::isfinite(value); -} - -std::uint32_t commandTimeoutMs(const api::MotorWaitOptions& options) -{ - const auto requested = options.timeout_ms(); - return requested == 0 ? kDefaultCommandTimeoutMs - : std::clamp(requested, 1, 600000); -} - -std::uint32_t pollPeriodMs(const api::MotorWaitOptions& options) -{ - const auto requested = options.poll_period_ms(); - return requested == 0 ? kDefaultPollPeriodMs - : std::clamp(requested, 1, 1000); -} - -std::uint32_t settleSamples(const api::MotorWaitOptions& options) -{ - const auto requested = options.settle_sample_count(); - return requested == 0 ? kDefaultSettleSamples - : std::clamp(requested, 1, 1000); -} - -double positionTolerance(const api::MotorWaitOptions& options) -{ - return options.position_tolerance_rad() > 0.0 - ? options.position_tolerance_rad() - : kDefaultPositionToleranceRad; -} - -double velocityTolerance(const api::MotorWaitOptions& options) -{ - return options.velocity_tolerance_rad_s() > 0.0 - ? options.velocity_tolerance_rad_s() - : kDefaultVelocityToleranceRadS; -} - -grpc::Status validateWaitOptions(const api::MotorWaitOptions& options) -{ - const double position_tolerance = options.position_tolerance_rad(); - if (position_tolerance != 0.0 && - (!isFinite(position_tolerance) || position_tolerance <= 0.0)) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "position_tolerance_rad must be finite and positive when specified"); - } - const double velocity_tolerance = options.velocity_tolerance_rad_s(); - if (velocity_tolerance != 0.0 && - (!isFinite(velocity_tolerance) || velocity_tolerance <= 0.0)) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "velocity_tolerance_rad_s must be finite and positive when specified"); - } - return grpc::Status::OK; -} - -grpc::Status cancelledStatus(grpc::ServerContext* context) -{ - if (std::chrono::system_clock::now() >= context->deadline()) { - return grpc::Status(grpc::StatusCode::DEADLINE_EXCEEDED, - "motor command gRPC deadline exceeded"); - } - return grpc::Status(grpc::StatusCode::CANCELLED, "motor command cancelled"); -} - -template -void sleepInterruptibly(grpc::ServerContext* context, - const Clock::time_point wake_time, - IsPreempted&& is_preempted) -{ - constexpr auto kCancellationSlice = std::chrono::milliseconds(10); - while (Clock::now() < wake_time) { - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline() || - is_preempted()) { - return; - } - const auto remaining = wake_time - Clock::now(); - std::this_thread::sleep_for(std::min( - std::chrono::duration_cast(kCancellationSlice), - remaining)); - } -} - -class ScopeExit final { -public: - explicit ScopeExit(std::function callback) - : callback_(std::move(callback)) - { - } - - ~ScopeExit() noexcept - { - if (!callback_) { - return; - } - try { - callback_(); - } catch (...) { - } - } - - void release() noexcept { callback_ = {}; } - -private: - std::function callback_; -}; - -template -grpc::Status runUnaryGuarded(Response* response, - const char* rpc_name, - Body&& body, - Cleanup&& cleanup, - GrpcCommandTransaction* command = nullptr) -{ - try { - return body(); - } catch (const std::exception& e) { - const std::string error = - std::string(rpc_name) + " backend exception: " + e.what(); - try { - cleanup(error); - } catch (...) { - } - response->Clear(); - fillFeedback(response->mutable_header(), false, error); - if (command) { - return command->finishException(error); - } - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } catch (...) { - const std::string error = - std::string(rpc_name) + " backend exception: unknown exception"; - try { - cleanup(error); - } catch (...) { - } - response->Clear(); - fillFeedback(response->mutable_header(), false, error); - if (command) { - return command->finishException(error); - } - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } -} - -template -grpc::Status runStreamingGuarded(const char* rpc_name, - Body&& body, - Cleanup&& cleanup) -{ - try { - return body(); - } catch (const std::exception& e) { - const std::string error = - std::string(rpc_name) + " backend exception: " + e.what(); - try { - cleanup(error); - } catch (...) { - } - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } catch (...) { - const std::string error = - std::string(rpc_name) + " backend exception: unknown exception"; - try { - cleanup(error); - } catch (...) { - } - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } -} - -template -grpc::Status runCyclicLoop( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream, - const std::uint64_t generation, - std::uint32_t watchdog_timeout_ms, - Apply&& apply, - FillStatus&& fill_status, - IsPreempted&& is_preempted, - ValidateSafety&& validate_safety, - SetLastError&& set_last_error, - Stop&& stop) -{ - watchdog_timeout_ms = watchdog_timeout_ms == 0 - ? kDefaultStreamWatchdogMs - : std::clamp( - watchdog_timeout_ms, 20, 60000); - - struct InputSlot { - std::mutex mutex; - std::condition_variable cv; - std::optional pending; - bool ended{false}; - bool reader_failed{false}; - std::uint64_t dropped{0}; - Clock::time_point last_receive{Clock::now()}; - } input; - - api::CyclicControlResponse opened; - fillFeedback(opened.mutable_header(), true); - opened.set_phase(api::CYCLIC_STREAM_OPENED); - fill_status(opened.mutable_status()); - if (!stream->Write(opened)) { - try { - stop(); - } catch (...) { - } - return grpc::Status(grpc::StatusCode::CANCELLED, - "cyclic stream closed while opening"); - } - - std::thread reader; - bool reader_joined = false; - const auto safeStop = [&]() noexcept { - try { - return stop(); - } catch (...) { - return false; - } - }; - const auto joinReader = [&](const bool cancel_context) noexcept { - if (cancel_context) { - bool ended = false; - try { - std::unique_lock lock(input.mutex); - input.cv.wait_for( - lock, std::chrono::milliseconds(50), [&]() { - return input.ended; - }); - ended = input.ended; - } catch (...) { - } - if (!ended) { - context->TryCancel(); - } - } - input.cv.notify_all(); - if (reader.joinable()) { - try { - reader.join(); - } catch (...) { - } - } - reader_joined = true; - }; - ScopeExit reader_guard([&]() { - if (!reader_joined) { - safeStop(); - joinReader(true); - } - }); - try { - reader = std::thread([&]() { - try { - Request incoming; - while (stream->Read(&incoming)) { - { - std::lock_guard lock(input.mutex); - if (input.pending.has_value()) { - ++input.dropped; - } - input.pending = std::move(incoming); - input.last_receive = Clock::now(); - } - input.cv.notify_one(); - incoming.Clear(); - } - } catch (...) { - std::lock_guard lock(input.mutex); - input.reader_failed = true; - } - { - std::lock_guard lock(input.mutex); - input.ended = true; - } - input.cv.notify_one(); - }); - } catch (const std::exception& e) { - const std::string error = - std::string("failed to start cyclic stream reader: ") + e.what(); - set_last_error(error); - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } catch (...) { - const std::string error = - "failed to start cyclic stream reader: unknown exception"; - set_last_error(error); - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } - - std::uint64_t last_sequence = 0; - std::uint64_t last_dropped = 0; - const auto finishTerminal = [&](const api::CyclicStreamPhase requested_phase, - const bool success, - const std::string& requested_error, - const grpc::Status requested_status, - const bool cancel_reader) -> grpc::Status { - const bool stopped = safeStop(); - const std::string error = stopped - ? requested_error - : "failed to quick-stop motor while terminating cyclic stream"; - if (!error.empty()) { - set_last_error(error); - } - try { - api::CyclicControlResponse terminal; - fillFeedback(terminal.mutable_header(), success && stopped, error); - terminal.set_phase( - stopped ? requested_phase : api::CYCLIC_STREAM_FAILED); - terminal.set_sequence(last_sequence); - terminal.set_dropped_setpoints(last_dropped); - fill_status(terminal.mutable_status()); - stream->Write(terminal); - } catch (...) { - joinReader(true); - reader_guard.release(); - return grpc::Status( - grpc::StatusCode::INTERNAL, - "failed to publish cyclic stream terminal status"); - } - joinReader(cancel_reader); - reader_guard.release(); - if (!stopped) { - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } - return requested_status; - }; - - try { - for (;;) { - if (context->IsCancelled()) { - const auto status = cancelledStatus(context); - set_last_error(status.error_message()); - const bool stopped = safeStop(); - joinReader(true); - reader_guard.release(); - return stopped - ? status - : grpc::Status( - grpc::StatusCode::INTERNAL, - "failed to quick-stop cancelled cyclic stream"); - } - if (is_preempted(generation)) { - const std::string error = - "cyclic stream preempted by a stop request"; - set_last_error(error); - const bool stopped = safeStop(); - joinReader(true); - reader_guard.release(); - return stopped - ? grpc::Status(grpc::StatusCode::ABORTED, error) - : grpc::Status( - grpc::StatusCode::INTERNAL, - "failed to quick-stop preempted cyclic stream"); - } - const auto safety_status = validate_safety(); - if (!safety_status.ok()) { - const std::string error = - "cyclic stream safety session was invalidated: " + - safety_status.error_message(); - return finishTerminal( - api::CYCLIC_STREAM_FAILED, false, error, - safety_status, true); - } - - std::optional request; - bool ended = false; - bool reader_failed = false; - Clock::time_point last_receive; - { - std::unique_lock lock(input.mutex); - input.cv.wait_for(lock, std::chrono::milliseconds(10), [&]() { - return input.pending.has_value() || input.ended; - }); - if (input.pending.has_value()) { - request = std::move(input.pending); - input.pending.reset(); - } - ended = input.ended; - reader_failed = input.reader_failed; - last_dropped = input.dropped; - last_receive = input.last_receive; - } - - if (!request.has_value()) { - if (ended) { - if (reader_failed) { - const std::string error = - "cyclic stream reader terminated with an exception"; - return finishTerminal( - api::CYCLIC_STREAM_FAILED, false, error, - grpc::Status(grpc::StatusCode::INTERNAL, error), - false); - } - return finishTerminal( - api::CYCLIC_STREAM_STOPPED, true, {}, - grpc::Status::OK, false); - } - if (Clock::now() - last_receive >= - std::chrono::milliseconds(watchdog_timeout_ms)) { - const std::string error = - "cyclic stream watchdog expired"; - return finishTerminal( - api::CYCLIC_STREAM_WATCHDOG_EXPIRED, false, error, - grpc::Status(grpc::StatusCode::DEADLINE_EXCEEDED, error), - true); - } - continue; - } - - if (!request->has_setpoint()) { - const std::string error = - "only the first cyclic stream message may contain open"; - return finishTerminal( - api::CYCLIC_STREAM_FAILED, false, error, - grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, error), - true); - } - - const auto& setpoint = request->setpoint(); - if (setpoint.sequence() == 0 || - setpoint.sequence() <= last_sequence) { - const std::string error = - "cyclic setpoint sequence must be strictly increasing and non-zero"; - return finishTerminal( - api::CYCLIC_STREAM_FAILED, false, error, - grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, error), - true); - } - - const auto apply_status = apply(setpoint); - if (!apply_status.ok()) { - return finishTerminal( - api::CYCLIC_STREAM_FAILED, false, - apply_status.error_message(), apply_status, true); - } - if (is_preempted(generation)) { - const std::string error = - "cyclic setpoint preempted during backend dispatch"; - return finishTerminal( - api::CYCLIC_STREAM_FAILED, false, error, - grpc::Status(grpc::StatusCode::ABORTED, error), true); - } - - last_sequence = setpoint.sequence(); - api::CyclicControlResponse applied; - fillFeedback(applied.mutable_header(), true); - applied.set_phase(api::CYCLIC_STREAM_APPLIED); - applied.set_sequence(last_sequence); - applied.set_dropped_setpoints(last_dropped); - if (!stream->Write(applied)) { - const std::string error = - "cyclic stream client stopped reading"; - set_last_error(error); - const bool stopped = safeStop(); - joinReader(true); - reader_guard.release(); - return stopped - ? grpc::Status(grpc::StatusCode::CANCELLED, error) - : grpc::Status( - grpc::StatusCode::INTERNAL, - "failed to quick-stop closed cyclic stream"); - } - } - } catch (const std::exception& e) { - const std::string error = - std::string("cyclic stream internal exception: ") + e.what(); - set_last_error(error); - return finishTerminal( - api::CYCLIC_STREAM_FAILED, false, error, - grpc::Status(grpc::StatusCode::INTERNAL, error), true); - } catch (...) { - const std::string error = "cyclic stream unknown internal exception"; - set_last_error(error); - return finishTerminal( - api::CYCLIC_STREAM_FAILED, false, error, - grpc::Status(grpc::StatusCode::INTERNAL, error), true); - } -} - -} // namespace - -gRPCMotorServiceImpl::ControlLease::ControlLease( - std::shared_ptr state, - const std::uint64_t generation) - : state_(std::move(state)), - generation_(generation), - uncaught_on_entry_(std::uncaught_exceptions()) -{ -} - -gRPCMotorServiceImpl::ControlLease::~ControlLease() -{ - if (!state_) { - return; - } - { - std::lock_guard lock(state_->mutex); - if (std::uncaught_exceptions() > uncaught_on_entry_) { - // Keep ownership reserved until the public RPC exception barrier - // has completed its best-effort stop. This closes the window where - // a new RPC could acquire the motor between stack unwinding and - // cleanup. - state_->exception_cleanup_pending = true; - ++state_->cancel_generation; - } else { - state_->busy = false; - state_->active_control = api::MOTOR_CONTROL_NONE; - } - } - globalMotorActivityCoordinator().notifyStateChanged(); -} - -gRPCMotorServiceImpl::gRPCMotorServiceImpl() - : gRPCMotorServiceImpl(makeDefaultGrpcSecurityGateway()) -{ -} - -gRPCMotorServiceImpl::gRPCMotorServiceImpl( - std::shared_ptr security_gateway) - : dmgr_(device::DeviceManager::getInstance()), - security_gateway_(security_gateway - ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()) -{ -} - -std::shared_ptr -gRPCMotorServiceImpl::stateFor( - const std::shared_ptr& motor) const -{ - std::lock_guard lock(states_mutex_); - for (auto it = states_.begin(); it != states_.end();) { - if (it->second.owner.expired()) { - it = states_.erase(it); - } else { - ++it; - } - } - - const auto existing = states_.find(motor.get()); - if (existing != states_.end()) { - const auto owner = existing->second.owner.lock(); - if (owner && owner == motor) { - return existing->second.state; - } - states_.erase(existing); - } - - auto state = std::make_shared(); - const std::weak_ptr weak_state = state; - const std::weak_ptr weak_motor = motor; - auto registration = globalMotorActivityCoordinator().registerControl( - [weak_state]() { - if (const auto state = weak_state.lock()) { - std::lock_guard state_lock(state->mutex); - ++state->cancel_generation; - state->last_error = "motor command preempted by System StopAll"; - } - }, - [weak_state, weak_motor]() { - const auto state = weak_state.lock(); - const auto motor = weak_motor.lock(); - if (!state || !motor) { - return true; - } - - bool stopped = false; - try { - // This confirmed stop is ordered after any device write that - // passed its generation check before StopAll invalidated it. - std::lock_guard command_lock(state->command_mutex); - stopped = motor->quickStop(); - } catch (...) { - stopped = false; - } - if (!stopped) { - std::lock_guard state_lock(state->mutex); - state->last_error = - "System StopAll could not confirm motor quick-stop"; - } - return stopped; - }, - [weak_state]() { - const auto state = weak_state.lock(); - if (!state) { - return true; - } - std::lock_guard state_lock(state->mutex); - return !state->busy && !state->exception_cleanup_pending; - }, - "motor " + std::to_string(motor->id()) + " (" + - motor->jointName() + ")"); - states_.emplace( - motor.get(), - MotorControlEntry{ - std::weak_ptr(motor), state, - std::move(registration)}); - return state; -} - -std::shared_ptr -gRPCMotorServiceImpl::existingStateFor( - const std::shared_ptr& motor) const -{ - std::lock_guard lock(states_mutex_); - const auto existing = states_.find(motor.get()); - if (existing == states_.end()) { - return nullptr; - } - const auto owner = existing->second.owner.lock(); - return owner && owner == motor ? existing->second.state : nullptr; -} - -grpc::Status gRPCMotorServiceImpl::resolveMotor( - const api::MotorTarget& target, - ResolvedMotor& resolved, - const ResolveAccess access) const -{ - const std::string& manager_id = target.header().device_id(); - if (manager_id.empty()) { - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, - "target.header.device_id is required"); - } - auto manager = dmgr_.getDevice(manager_id); - if (!manager) { - return grpc::Status(grpc::StatusCode::NOT_FOUND, - "MotorManager not found: " + manager_id); - } - - switch (target.selector_case()) { - case api::MotorTarget::kMotorId: - if (target.motor_id() > 255) { - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, - "motor_id must fit in uint8"); - } - resolved.motor = manager->getMotor( - static_cast(target.motor_id())); - break; - case api::MotorTarget::kJointName: - if (target.joint_name().empty()) { - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, - "joint_name cannot be empty"); - } - resolved.motor = manager->getMotor(target.joint_name()); - break; - case api::MotorTarget::SELECTOR_NOT_SET: - default: - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, - "exactly one of motor_id or joint_name is required"); - } - - if (!resolved.motor) { - return grpc::Status(grpc::StatusCode::NOT_FOUND, - "motor not found in MotorManager: " + manager_id); - } - const auto arm_owner = manager->armOwnerForJoint( - resolved.motor->jointName()); - if (access == ResolveAccess::Control && !arm_owner.empty()) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "motor joint is controlled by RobotArm '" + arm_owner + - "'; use ArmService for control: " + - resolved.motor->jointName()); - } - resolved.control = - access == ResolveAccess::Observe && !arm_owner.empty() - ? existingStateFor(resolved.motor) - : stateFor(resolved.motor); - return grpc::Status::OK; -} - -std::unique_ptr -gRPCMotorServiceImpl::acquireControl( - const ResolvedMotor& resolved, - const api::MotorControlType control, - grpc::Status& failure, - const bool allow_emergency_stopped) const -{ - auto system_admission = globalStopAllAdmissionGate().lockAdmission(); - auto motor_admission = globalMotorActivityCoordinator().lockAdmission(); - if (!system_admission.accepting() || !motor_admission.accepting()) { - failure = grpc::Status( - grpc::StatusCode::ABORTED, - "motor control admission is paused by System StopAll"); - return nullptr; - } - - std::lock_guard lock(resolved.control->mutex); - if (resolved.control->busy) { - failure = grpc::Status(grpc::StatusCode::RESOURCE_EXHAUSTED, - "motor is controlled by another RPC or stream"); - return nullptr; - } - if (resolved.control->emergency_stop_in_progress) { - failure = grpc::Status( - grpc::StatusCode::ABORTED, - "motor emergency stop is still in progress"); - return nullptr; - } - if (resolved.control->emergency_stopped && !allow_emergency_stopped) { - failure = grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "motor is emergency-stopped; enable it explicitly before commanding motion"); - return nullptr; - } - resolved.control->busy = true; - resolved.control->active_control = control; - resolved.control->last_error.clear(); - failure = grpc::Status::OK; - return std::make_unique( - resolved.control, resolved.control->cancel_generation); -} - -void gRPCMotorServiceImpl::setLastError( - const std::shared_ptr& state, - const std::string& error) const -{ - std::lock_guard lock(state->mutex); - state->last_error = error; -} - -void gRPCMotorServiceImpl::latchUnsafeAfterFailedStop( - const std::shared_ptr& state, - const std::string& error) const -{ - std::lock_guard lock(state->mutex); - state->emergency_stopped = true; - state->last_error = error; -} - -void gRPCMotorServiceImpl::bestEffortQuickStop( - const api::MotorTarget& target, - const std::string& error) noexcept -{ - try { - ResolvedMotor resolved; - if (!resolveMotor(target, resolved).ok()) { - return; - } - // Hold this across claim, quick-stop, and release. Otherwise two stale - // exception barriers can both observe one pending cleanup; the second - // may wake after a new RPC acquires the motor and stop that new owner. - std::lock_guard cleanup_lock( - resolved.control->exception_cleanup_mutex); - { - std::lock_guard state_lock(resolved.control->mutex); - if (!resolved.control->exception_cleanup_pending) { - if (resolved.control->busy) { - // A newer RPC already owns the motor. Stopping here would - // let an older exception cancel the new owner's command. - return; - } - resolved.control->busy = true; - resolved.control->exception_cleanup_pending = true; - ++resolved.control->cancel_generation; - } - resolved.control->last_error = error; - } - - bool stopped = false; - try { - std::lock_guard command_lock(resolved.control->command_mutex); - stopped = resolved.motor->quickStop(); - } catch (...) { - } - if (!stopped) { - latchUnsafeAfterFailedStop( - resolved.control, - error + "; best-effort quick-stop was not confirmed"); - } - { - std::lock_guard state_lock(resolved.control->mutex); - resolved.control->exception_cleanup_pending = false; - resolved.control->busy = false; - resolved.control->active_control = api::MOTOR_CONTROL_NONE; - } - globalMotorActivityCoordinator().notifyStateChanged(); - } catch (...) { - } -} - -void gRPCMotorServiceImpl::fillMotorStatus( - const ResolvedMotor& resolved, - api::MotorStatus* status) const -{ - bool busy = false; - bool emergency_stopped = false; - api::MotorControlType active_control = api::MOTOR_CONTROL_NONE; - std::string last_error; - if (resolved.control) { - std::lock_guard lock(resolved.control->mutex); - busy = resolved.control->busy; - emergency_stopped = resolved.control->emergency_stopped; - active_control = resolved.control->active_control; - last_error = resolved.control->last_error; - } - - status->set_motor_id(resolved.motor->id()); - status->set_joint_name(resolved.motor->jointName()); - status->set_run_mode(resolved.motor->getMode()); - status->set_position_rad(resolved.motor->getQ()); - status->set_velocity_rad_s(resolved.motor->getQd()); - status->set_target_reached(resolved.motor->reachedTargetQ()); - status->set_service_busy(busy); - status->set_active_control(active_control); - status->set_emergency_stopped(emergency_stopped); - status->set_last_error(last_error); -} - -grpc::Status gRPCMotorServiceImpl::setZero( - grpc::ServerContext* context, - const api::SetMotorZeroRequest* request, - api::MotorCommandResponse* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MotorService/setZero", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - return runUnaryGuarded( - response, "setZero", - [&]() { - return setZeroImpl( - context, request, response, command); - }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); - }, - &command); - }); -} - -grpc::Status gRPCMotorServiceImpl::moveToZero( - grpc::ServerContext* context, - const api::MoveMotorToZeroRequest* request, - api::MotorCommandResponse* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MotorService/moveToZero", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - return runUnaryGuarded( - response, "moveToZero", - [&]() { - return moveToZeroImpl( - context, request, response, command); - }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); - }, - &command); - }); -} - -grpc::Status gRPCMotorServiceImpl::profilePosition( - grpc::ServerContext* context, - const api::ProfilePositionRequest* request, - api::MotorCommandResponse* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MotorService/profilePosition", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - return runUnaryGuarded( - response, "profilePosition", - [&]() { - return profilePositionImpl( - context, request, response, command); - }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); - }, - &command); - }); -} - -grpc::Status gRPCMotorServiceImpl::profileVelocity( - grpc::ServerContext* context, - const api::ProfileVelocityRequest* request, - api::MotorCommandResponse* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MotorService/profileVelocity", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - return runUnaryGuarded( - response, "profileVelocity", - [&]() { - return profileVelocityImpl( - context, request, response, command); - }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); - }, - &command); - }); -} - -grpc::Status gRPCMotorServiceImpl::streamCyclicPosition( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.MotorService/streamCyclicPosition"); - std::optional cleanup_target; - return runStreamingGuarded( - "streamCyclicPosition", - [&]() { - return streamCyclicPositionImpl( - context, stream, cleanup_target, - cmvr_grpc_call_guard.context()); - }, - [&](const std::string& error) { - if (cleanup_target.has_value()) { - bestEffortQuickStop(*cleanup_target, error); - } - }); -} - -grpc::Status gRPCMotorServiceImpl::streamCyclicVelocity( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.MotorService/streamCyclicVelocity"); - std::optional cleanup_target; - return runStreamingGuarded( - "streamCyclicVelocity", - [&]() { - return streamCyclicVelocityImpl( - context, stream, cleanup_target, - cmvr_grpc_call_guard.context()); - }, - [&](const std::string& error) { - if (cleanup_target.has_value()) { - bestEffortQuickStop(*cleanup_target, error); - } - }); -} - -grpc::Status gRPCMotorServiceImpl::emergencyStop( - grpc::ServerContext* context, - const api::EmergencyStopRequest* request, - api::MotorCommandResponse* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MotorService/emergencyStop", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - return runUnaryGuarded( - response, "emergencyStop", - [&]() { - return emergencyStopImpl( - context, request, response, command); - }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); - }, - &command); - }); -} - -grpc::Status gRPCMotorServiceImpl::getStatus( - grpc::ServerContext* context, - const api::GetMotorStatusRequest* request, - api::GetMotorStatusResponse* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.MotorService/getStatus"); - return runUnaryGuarded( - response, "getStatus", - [&]() { return getStatusImpl(context, request, response); }, - [](const std::string&) {}); -} - -grpc::Status gRPCMotorServiceImpl::setEnabled( - grpc::ServerContext* context, - const api::SetMotorEnabledRequest* request, - api::MotorCommandResponse* response) -{ - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MotorService/setEnabled", request, response, - [this, context, request, response](GrpcCommandTransaction& command) { - return runUnaryGuarded( - response, "setEnabled", - [&]() { - return setEnabledImpl( - context, request, response, command); - }, - [&](const std::string& error) { - bestEffortQuickStop(request->target(), error); - }, - &command); - }); -} - -grpc::Status gRPCMotorServiceImpl::setZeroImpl( - grpc::ServerContext* context, - const api::SetMotorZeroRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command) -{ - const auto started = Clock::now(); - ResolvedMotor resolved; - auto status = resolveMotor(request->target(), resolved); - if (!status.ok()) { - fillFeedback(response->mutable_header(), false, status.error_message()); - return status; - } - grpc::Status acquire_status; - auto lease = acquireControl( - resolved, api::MOTOR_CONTROL_SET_ZERO, acquire_status); - if (!lease) { - fillFeedback(response->mutable_header(), false, - acquire_status.error_message()); - fillMotorStatus(resolved, response->mutable_status()); - return acquire_status; - } - bool calibrated = false; - bool preempted_during_calibration = false; - bool cancelled_during_calibration = false; - grpc::Status post_calibration_cancel_status; - { - std::lock_guard command_lock(resolved.control->command_mutex); - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - const auto cancelled = cancelledStatus(context); - lease.reset(); - fillFeedback(response->mutable_header(), false, - cancelled.error_message()); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return cancelled; - } - bool preempted = false; - { - std::lock_guard state_lock(resolved.control->mutex); - preempted = - resolved.control->cancel_generation != lease->generation(); - } - if (preempted) { - const std::string error = - "zero calibration preempted by a stop request"; - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (!command.beginDispatch()) { - lease.reset(); - return command.dispatchStatus(); - } - calibrated = resolved.motor->calibrateZeroQ(); - { - std::lock_guard state_lock(resolved.control->mutex); - preempted_during_calibration = - resolved.control->cancel_generation != lease->generation() || - resolved.control->emergency_stop_in_progress; - } - if (!preempted_during_calibration && - (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline())) { - cancelled_during_calibration = true; - post_calibration_cancel_status = cancelledStatus(context); - } - } - if (preempted_during_calibration) { - const std::string error = - "zero calibration preempted during backend dispatch"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (cancelled_during_calibration) { - const std::string error = - post_calibration_cancel_status.error_message() + - "; zero calibration outcome may already be committed"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status( - post_calibration_cancel_status.error_code(), error); - } - if (!calibrated) { - const std::string error = - "zero calibration outcome unknown; inspect device state before retry"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); - } - lease.reset(); - fillFeedback(response->mutable_header(), true); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status::OK; -} - -grpc::Status gRPCMotorServiceImpl::runProfilePosition( - grpc::ServerContext* context, - const ResolvedMotor& resolved, - const double target_position_rad, - const double max_velocity_rad_s, - const double acceleration_rad_s2, - const api::MotorWaitOptions& wait, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command) -{ - const auto started = Clock::now(); - if (!isFinite(target_position_rad) || - !isFinite(max_velocity_rad_s) || max_velocity_rad_s <= 0.0 || - !isFinite(acceleration_rad_s2) || acceleration_rad_s2 <= 0.0) { - const std::string error = - "target position must be finite and velocity/acceleration must be positive"; - fillFeedback(response->mutable_header(), false, error); - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, error); - } - const auto wait_status = validateWaitOptions(wait); - if (!wait_status.ok()) { - fillFeedback(response->mutable_header(), false, - wait_status.error_message()); - return wait_status; - } - - grpc::Status acquire_status; - auto lease = acquireControl( - resolved, api::MOTOR_CONTROL_PROFILE_POSITION, acquire_status); - if (!lease) { - fillFeedback(response->mutable_header(), false, - acquire_status.error_message()); - fillMotorStatus(resolved, response->mutable_status()); - return acquire_status; - } - - bool submitted = false; - bool preempted_during_dispatch = false; - { - std::lock_guard command_lock(resolved.control->command_mutex); - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - const auto cancelled = cancelledStatus(context); - lease.reset(); - fillFeedback(response->mutable_header(), false, - cancelled.error_message()); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return cancelled; - } - bool preempted = false; - { - std::lock_guard state_lock(resolved.control->mutex); - preempted = - resolved.control->cancel_generation != lease->generation(); - } - if (preempted) { - const std::string error = - "profile position preempted by a stop request"; - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (!command.beginDispatch()) { - lease.reset(); - return command.dispatchStatus(); - } - resolved.motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); - submitted = resolved.motor->commandProfilePosition( - target_position_rad, max_velocity_rad_s, acceleration_rad_s2); - { - std::lock_guard state_lock(resolved.control->mutex); - preempted_during_dispatch = - resolved.control->cancel_generation != lease->generation() || - resolved.control->emergency_stop_in_progress; - } - } - if (preempted_during_dispatch) { - const std::string error = - "profile position preempted during backend dispatch"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (!submitted) { - bool stopped = false; - { - std::lock_guard command_lock(resolved.control->command_mutex); - { - std::lock_guard state_lock(resolved.control->mutex); - preempted_during_dispatch = - resolved.control->cancel_generation != lease->generation() || - resolved.control->emergency_stop_in_progress; - } - if (!preempted_during_dispatch) { - stopped = resolved.motor->quickStop(); - std::lock_guard state_lock(resolved.control->mutex); - preempted_during_dispatch = - resolved.control->cancel_generation != lease->generation() || - resolved.control->emergency_stop_in_progress; - } - } - if (preempted_during_dispatch) { - const std::string error = - "profile position rejected while being preempted by a stop request"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (!stopped) { - const std::string error = - "failed to quick-stop after uncertain profile position dispatch"; - latchUnsafeAfterFailedStop(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - const auto cancelled = cancelledStatus(context); - const std::string error = - cancelled.error_message() + - "; profile position dispatch outcome may have been committed"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(cancelled.error_code(), error); - } - const std::string error = - "AbstractMotor rejected profile position command after safe stop"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); - } - - auto completion_status = waitForPosition( - context, resolved, lease->generation(), target_position_rad, - wait, response, started); - lease.reset(); - fillMotorStatus(resolved, response->mutable_status()); - return completion_status; -} - -grpc::Status gRPCMotorServiceImpl::waitForPosition( - grpc::ServerContext* context, - const ResolvedMotor& resolved, - const std::uint64_t generation, - const double target_position_rad, - const api::MotorWaitOptions& wait, - api::MotorCommandResponse* response, - const Clock::time_point started) -{ - const auto timeout = std::chrono::milliseconds(commandTimeoutMs(wait)); - const auto poll = std::chrono::milliseconds(pollPeriodMs(wait)); - const auto required_samples = settleSamples(wait); - const double q_tolerance = positionTolerance(wait); - const double qd_tolerance = velocityTolerance(wait); - std::uint32_t settled = 0; - - for (;;) { - bool preempted = false; - { - std::lock_guard lock(resolved.control->mutex); - preempted = resolved.control->cancel_generation != generation; - } - if (preempted) { - const std::string error = - "profile position preempted by a stop request"; - setLastError(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - const auto cancelled = cancelledStatus(context); - setLastError(resolved.control, cancelled.error_message()); - bool stopped = false; - { - std::lock_guard command_lock( - resolved.control->command_mutex); - stopped = resolved.motor->quickStop(); - } - if (!stopped) { - const std::string error = - "failed to quick-stop cancelled profile position"; - latchUnsafeAfterFailedStop(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } - fillFeedback(response->mutable_header(), false, - cancelled.error_message()); - response->set_elapsed_ms(elapsedMs(started)); - return cancelled; - } - if (Clock::now() - started >= timeout) { - const std::string error = "profile position wait timed out"; - setLastError(resolved.control, error); - bool stopped = false; - { - std::lock_guard command_lock( - resolved.control->command_mutex); - stopped = resolved.motor->quickStop(); - } - if (!stopped) { - const std::string stop_error = - "failed to quick-stop timed-out profile position"; - latchUnsafeAfterFailedStop(resolved.control, stop_error); - fillFeedback(response->mutable_header(), false, stop_error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::INTERNAL, stop_error); - } - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::DEADLINE_EXCEEDED, error); - } - - const double q = resolved.motor->getQ(); - const double qd = resolved.motor->getQd(); - const bool reached_target = resolved.motor->reachedTargetQ(); - { - std::lock_guard lock(resolved.control->mutex); - preempted = resolved.control->cancel_generation != generation; - } - if (preempted) { - const std::string error = - "profile position preempted during status sampling"; - setLastError(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (!isFinite(q) || !isFinite(qd)) { - bool stopped = false; - try { - std::lock_guard command_lock( - resolved.control->command_mutex); - stopped = resolved.motor->quickStop(); - } catch (...) { - stopped = false; - } - if (!stopped) { - const std::string error = - "failed to quick-stop profile position after non-finite feedback"; - latchUnsafeAfterFailedStop(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } - const std::string error = - "profile position feedback became non-finite"; - setLastError(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::UNAVAILABLE, error); - } - const bool in_tolerance = - reached_target && - std::abs(q - target_position_rad) <= q_tolerance && - std::abs(qd) <= qd_tolerance; - settled = in_tolerance ? settled + 1 : 0; - if (settled >= required_samples) { - fillFeedback(response->mutable_header(), true); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status::OK; - } - sleepInterruptibly( - context, Clock::now() + poll, [&]() { - std::lock_guard lock(resolved.control->mutex); - return resolved.control->cancel_generation != generation; - }); - } -} - -grpc::Status gRPCMotorServiceImpl::moveToZeroImpl( - grpc::ServerContext* context, - const api::MoveMotorToZeroRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command) -{ - ResolvedMotor resolved; - auto status = resolveMotor(request->target(), resolved); - if (!status.ok()) { - fillFeedback(response->mutable_header(), false, status.error_message()); - return status; - } - return runProfilePosition( - context, resolved, 0.0, request->max_velocity_rad_s(), - request->acceleration_rad_s2(), request->wait(), response, command); -} - -grpc::Status gRPCMotorServiceImpl::profilePositionImpl( - grpc::ServerContext* context, - const api::ProfilePositionRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command) -{ - ResolvedMotor resolved; - auto status = resolveMotor(request->target(), resolved); - if (!status.ok()) { - fillFeedback(response->mutable_header(), false, status.error_message()); - return status; - } - return runProfilePosition( - context, resolved, request->target_position_rad(), - request->max_velocity_rad_s(), request->acceleration_rad_s2(), - request->wait(), response, command); -} - -grpc::Status gRPCMotorServiceImpl::waitForVelocity( - grpc::ServerContext* context, - const ResolvedMotor& resolved, - const std::uint64_t generation, - const double target_velocity_rad_s, - const api::MotorWaitOptions& wait, - api::MotorCommandResponse* response, - const Clock::time_point started) -{ - const auto timeout = std::chrono::milliseconds(commandTimeoutMs(wait)); - const auto poll = std::chrono::milliseconds(pollPeriodMs(wait)); - const auto required_samples = settleSamples(wait); - const double tolerance = velocityTolerance(wait); - std::uint32_t settled = 0; - - for (;;) { - bool preempted = false; - { - std::lock_guard lock(resolved.control->mutex); - preempted = resolved.control->cancel_generation != generation; - } - if (preempted) { - const std::string error = - "profile velocity preempted by a stop request"; - setLastError(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - const auto cancelled = cancelledStatus(context); - setLastError(resolved.control, cancelled.error_message()); - bool stopped = false; - { - std::lock_guard command_lock( - resolved.control->command_mutex); - stopped = resolved.motor->quickStop(); - } - if (!stopped) { - const std::string error = - "failed to quick-stop cancelled profile velocity"; - latchUnsafeAfterFailedStop(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } - fillFeedback(response->mutable_header(), false, - cancelled.error_message()); - response->set_elapsed_ms(elapsedMs(started)); - return cancelled; - } - if (Clock::now() - started >= timeout) { - const std::string error = "profile velocity wait timed out"; - setLastError(resolved.control, error); - bool stopped = false; - { - std::lock_guard command_lock( - resolved.control->command_mutex); - stopped = resolved.motor->quickStop(); - } - if (!stopped) { - const std::string stop_error = - "failed to quick-stop timed-out profile velocity"; - latchUnsafeAfterFailedStop(resolved.control, stop_error); - fillFeedback(response->mutable_header(), false, stop_error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::INTERNAL, stop_error); - } - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::DEADLINE_EXCEEDED, error); - } - - const double qd = resolved.motor->getQd(); - { - std::lock_guard lock(resolved.control->mutex); - preempted = resolved.control->cancel_generation != generation; - } - if (preempted) { - const std::string error = - "profile velocity preempted during status sampling"; - setLastError(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (!isFinite(qd)) { - bool stopped = false; - try { - std::lock_guard command_lock( - resolved.control->command_mutex); - stopped = resolved.motor->quickStop(); - } catch (...) { - stopped = false; - } - if (!stopped) { - const std::string error = - "failed to quick-stop profile velocity after non-finite feedback"; - latchUnsafeAfterFailedStop(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } - const std::string error = - "profile velocity feedback became non-finite"; - setLastError(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::UNAVAILABLE, error); - } - settled = std::abs(qd - target_velocity_rad_s) <= tolerance - ? settled + 1 - : 0; - if (settled >= required_samples) { - fillFeedback(response->mutable_header(), true); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status::OK; - } - sleepInterruptibly( - context, Clock::now() + poll, [&]() { - std::lock_guard lock(resolved.control->mutex); - return resolved.control->cancel_generation != generation; - }); - } -} - -grpc::Status gRPCMotorServiceImpl::profileVelocityImpl( - grpc::ServerContext* context, - const api::ProfileVelocityRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command) -{ - const auto started = Clock::now(); - ResolvedMotor resolved; - auto status = resolveMotor(request->target(), resolved); - if (!status.ok()) { - fillFeedback(response->mutable_header(), false, status.error_message()); - return status; - } - if (!isFinite(request->target_velocity_rad_s()) || - !isFinite(request->acceleration_rad_s2()) || - request->acceleration_rad_s2() <= 0.0) { - const std::string error = - "target velocity must be finite and acceleration must be positive"; - fillFeedback(response->mutable_header(), false, error); - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, error); - } - const auto wait_options_status = validateWaitOptions(request->wait()); - if (!wait_options_status.ok()) { - fillFeedback(response->mutable_header(), false, - wait_options_status.error_message()); - return wait_options_status; - } - - grpc::Status acquire_status; - auto lease = acquireControl( - resolved, api::MOTOR_CONTROL_PROFILE_VELOCITY, acquire_status); - if (!lease) { - fillFeedback(response->mutable_header(), false, - acquire_status.error_message()); - fillMotorStatus(resolved, response->mutable_status()); - return acquire_status; - } - - bool submitted = false; - bool preempted_during_dispatch = false; - { - std::lock_guard command_lock(resolved.control->command_mutex); - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - const auto cancelled = cancelledStatus(context); - lease.reset(); - fillFeedback(response->mutable_header(), false, - cancelled.error_message()); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return cancelled; - } - bool preempted = false; - { - std::lock_guard state_lock(resolved.control->mutex); - preempted = - resolved.control->cancel_generation != lease->generation(); - } - if (preempted) { - const std::string error = - "profile velocity preempted by a stop request"; - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (!command.beginDispatch()) { - lease.reset(); - return command.dispatchStatus(); - } - resolved.motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); - submitted = resolved.motor->commandProfileVelocity( - request->target_velocity_rad_s(), - request->acceleration_rad_s2()); - { - std::lock_guard state_lock(resolved.control->mutex); - preempted_during_dispatch = - resolved.control->cancel_generation != lease->generation() || - resolved.control->emergency_stop_in_progress; - } - } - if (preempted_during_dispatch) { - const std::string error = - "profile velocity preempted during backend dispatch"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (!submitted) { - bool stopped = false; - { - std::lock_guard command_lock(resolved.control->command_mutex); - { - std::lock_guard state_lock(resolved.control->mutex); - preempted_during_dispatch = - resolved.control->cancel_generation != lease->generation() || - resolved.control->emergency_stop_in_progress; - } - if (!preempted_during_dispatch) { - stopped = resolved.motor->quickStop(); - std::lock_guard state_lock(resolved.control->mutex); - preempted_during_dispatch = - resolved.control->cancel_generation != lease->generation() || - resolved.control->emergency_stop_in_progress; - } - } - if (preempted_during_dispatch) { - const std::string error = - "profile velocity rejected while being preempted by a stop request"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (!stopped) { - const std::string error = - "failed to quick-stop after uncertain profile velocity dispatch"; - latchUnsafeAfterFailedStop(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - const auto cancelled = cancelledStatus(context); - const std::string error = - cancelled.error_message() + - "; profile velocity dispatch outcome may have been committed"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(cancelled.error_code(), error); - } - const std::string error = - "AbstractMotor rejected profile velocity command after safe stop"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); - } - - auto wait_status = waitForVelocity( - context, resolved, lease->generation(), - request->target_velocity_rad_s(), request->wait(), response, started); - lease.reset(); - fillMotorStatus(resolved, response->mutable_status()); - return wait_status; -} - -grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream, - std::optional& cleanup_target, - const GrpcRequestContext& request_context) -{ - api::CyclicPositionRequest first; - if (!stream->Read(&first) || !first.has_open()) { - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, - "first cyclic position message must contain open"); - } - cleanup_target = first.open().target(); - ResolvedMotor resolved; - auto status = resolveMotor(first.open().target(), resolved); - if (!status.ok()) { - return status; - } - grpc::Status acquire_status; - auto lease = acquireControl( - resolved, api::MOTOR_CONTROL_CYCLIC_POSITION, acquire_status); - if (!lease) { - return acquire_status; - } - - const auto generation = lease->generation(); - const auto& header = first.open().target().header(); - GrpcStreamingSafetyOpen safety_open; - safety_open.full_method_name = - "/cmvr.api.MotorService/streamCyclicPosition"; - safety_open.device_id = header.device_id(); - safety_open.session_id = header.command_id().empty() - ? request_context.correlation_id - : header.command_id(); - safety_open.expected_service_instance_id = - header.expected_service_instance_id(); - if (header.has_expected_device_generation()) { - safety_open.expected_device_generation = - header.expected_device_generation(); - } - safety_open.authority_generation = generation; - safety_open.deadline = request_context.deadline; - GrpcStreamingSafetySession safety_session( - dmgr_.safetyManager(), request_context, - std::move(safety_open)); - if (!safety_session.admitted()) { - return safety_session.status(); - } - { - std::lock_guard command_lock(resolved.control->command_mutex); - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - return cancelledStatus(context); - } - bool preempted = false; - { - std::lock_guard state_lock(resolved.control->mutex); - preempted = - resolved.control->cancel_generation != generation; - } - if (preempted) { - return grpc::Status( - grpc::StatusCode::ABORTED, - "cyclic position open preempted by a stop request"); - } - auto safety_dispatch = safety_session.beginDispatch(); - if (!safety_dispatch.acquired()) { - return safety_session.status(); - } - resolved.motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - std::lock_guard state_lock(resolved.control->mutex); - if (resolved.control->cancel_generation != generation) { - return grpc::Status( - grpc::StatusCode::ABORTED, - "cyclic position open preempted during backend dispatch"); - } - } - return runCyclicLoop( - context, stream, generation, - first.open().watchdog_timeout_ms(), - [&](const api::CyclicPositionSetpoint& setpoint) { - std::lock_guard command_lock( - resolved.control->command_mutex); - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - return cancelledStatus(context); - } - { - std::lock_guard state_lock(resolved.control->mutex); - if (resolved.control->cancel_generation != generation) { - return grpc::Status( - grpc::StatusCode::ABORTED, - "cyclic position setpoint preempted by a stop request"); - } - } - if (!isFinite(setpoint.target_position_rad()) || - (setpoint.has_target_velocity_rad_s() && - !isFinite(setpoint.target_velocity_rad_s()))) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "cyclic position setpoint contains a non-finite value"); - } - auto safety_dispatch = safety_session.beginDispatch(); - if (!safety_dispatch.acquired()) { - return safety_session.status(); - } - const bool submitted = resolved.motor->commandCyclicPosition( - setpoint.target_position_rad(), - setpoint.has_target_velocity_rad_s() - ? setpoint.target_velocity_rad_s() - : 0.0); - { - std::lock_guard state_lock(resolved.control->mutex); - if (resolved.control->cancel_generation != generation) { - return grpc::Status( - grpc::StatusCode::ABORTED, - "cyclic position setpoint preempted during backend dispatch"); - } - } - if (!submitted) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "AbstractMotor rejected cyclic position setpoint"); - } - return grpc::Status::OK; - }, - [&](api::MotorStatus* motor_status) { - fillMotorStatus(resolved, motor_status); - }, - [&](const std::uint64_t expected_generation) { - std::lock_guard lock(resolved.control->mutex); - return resolved.control->cancel_generation != expected_generation; - }, - [&]() { - return safety_session.revalidate() - ? grpc::Status::OK - : safety_session.status(); - }, - [&](const std::string& error) { - setLastError(resolved.control, error); - }, - [&]() { - bool stopped = false; - try { - std::lock_guard command_lock( - resolved.control->command_mutex); - stopped = resolved.motor->quickStop(); - } catch (...) { - stopped = false; - } - if (!stopped) { - latchUnsafeAfterFailedStop( - resolved.control, - "failed to quick-stop cyclic position stream"); - } - return stopped; - }); -} - -grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream, - std::optional& cleanup_target, - const GrpcRequestContext& request_context) -{ - api::CyclicVelocityRequest first; - if (!stream->Read(&first) || !first.has_open()) { - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, - "first cyclic velocity message must contain open"); - } - cleanup_target = first.open().target(); - ResolvedMotor resolved; - auto status = resolveMotor(first.open().target(), resolved); - if (!status.ok()) { - return status; - } - grpc::Status acquire_status; - auto lease = acquireControl( - resolved, api::MOTOR_CONTROL_CYCLIC_VELOCITY, acquire_status); - if (!lease) { - return acquire_status; - } - - const auto generation = lease->generation(); - const auto& header = first.open().target().header(); - GrpcStreamingSafetyOpen safety_open; - safety_open.full_method_name = - "/cmvr.api.MotorService/streamCyclicVelocity"; - safety_open.device_id = header.device_id(); - safety_open.session_id = header.command_id().empty() - ? request_context.correlation_id - : header.command_id(); - safety_open.expected_service_instance_id = - header.expected_service_instance_id(); - if (header.has_expected_device_generation()) { - safety_open.expected_device_generation = - header.expected_device_generation(); - } - safety_open.authority_generation = generation; - safety_open.deadline = request_context.deadline; - GrpcStreamingSafetySession safety_session( - dmgr_.safetyManager(), request_context, - std::move(safety_open)); - if (!safety_session.admitted()) { - return safety_session.status(); - } - { - std::lock_guard command_lock(resolved.control->command_mutex); - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - return cancelledStatus(context); - } - bool preempted = false; - { - std::lock_guard state_lock(resolved.control->mutex); - preempted = - resolved.control->cancel_generation != generation; - } - if (preempted) { - return grpc::Status( - grpc::StatusCode::ABORTED, - "cyclic velocity open preempted by a stop request"); - } - auto safety_dispatch = safety_session.beginDispatch(); - if (!safety_dispatch.acquired()) { - return safety_session.status(); - } - resolved.motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); - std::lock_guard state_lock(resolved.control->mutex); - if (resolved.control->cancel_generation != generation) { - return grpc::Status( - grpc::StatusCode::ABORTED, - "cyclic velocity open preempted during backend dispatch"); - } - } - return runCyclicLoop( - context, stream, generation, - first.open().watchdog_timeout_ms(), - [&](const api::CyclicVelocitySetpoint& setpoint) { - std::lock_guard command_lock( - resolved.control->command_mutex); - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - return cancelledStatus(context); - } - { - std::lock_guard state_lock(resolved.control->mutex); - if (resolved.control->cancel_generation != generation) { - return grpc::Status( - grpc::StatusCode::ABORTED, - "cyclic velocity setpoint preempted by a stop request"); - } - } - if (!isFinite(setpoint.target_velocity_rad_s())) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "cyclic velocity setpoint contains a non-finite value"); - } - auto safety_dispatch = safety_session.beginDispatch(); - if (!safety_dispatch.acquired()) { - return safety_session.status(); - } - const bool submitted = resolved.motor->commandCyclicVelocity( - setpoint.target_velocity_rad_s()); - { - std::lock_guard state_lock(resolved.control->mutex); - if (resolved.control->cancel_generation != generation) { - return grpc::Status( - grpc::StatusCode::ABORTED, - "cyclic velocity setpoint preempted during backend dispatch"); - } - } - if (!submitted) { - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "AbstractMotor rejected cyclic velocity setpoint"); - } - return grpc::Status::OK; - }, - [&](api::MotorStatus* motor_status) { - fillMotorStatus(resolved, motor_status); - }, - [&](const std::uint64_t expected_generation) { - std::lock_guard lock(resolved.control->mutex); - return resolved.control->cancel_generation != expected_generation; - }, - [&]() { - return safety_session.revalidate() - ? grpc::Status::OK - : safety_session.status(); - }, - [&](const std::string& error) { - setLastError(resolved.control, error); - }, - [&]() { - bool stopped = false; - try { - std::lock_guard command_lock( - resolved.control->command_mutex); - stopped = resolved.motor->quickStop(); - } catch (...) { - stopped = false; - } - if (!stopped) { - latchUnsafeAfterFailedStop( - resolved.control, - "failed to quick-stop cyclic velocity stream"); - } - return stopped; - }); -} - -grpc::Status gRPCMotorServiceImpl::emergencyStopImpl( - grpc::ServerContext*, - const api::EmergencyStopRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command) -{ - const auto started = Clock::now(); - ResolvedMotor resolved; - auto status = resolveMotor(request->target(), resolved); - if (!status.ok()) { - fillFeedback(response->mutable_header(), false, status.error_message()); - return status; - } - std::lock_guard emergency_lock(resolved.control->emergency_mutex); - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } - bool stopped = false; - { - { - std::lock_guard state_lock(resolved.control->mutex); - ++resolved.control->cancel_generation; - resolved.control->emergency_stopped = true; - resolved.control->emergency_stop_in_progress = true; - resolved.control->last_error = "emergency stop requested"; - } - // First stop is deliberately issued before waiting for the service - // dispatch mutex. AbstractMotor serializes it with an in-flight driver - // call, so it takes effect at the earliest point the driver permits. - try { - resolved.motor->quickStop(); - } catch (...) { - } - // The confirmed second stop is ordered after every ordinary write that - // passed its generation check before this E-stop. No stale write can - // therefore occur after this final stop. - { - std::lock_guard command_lock(resolved.control->command_mutex); - try { - stopped = resolved.motor->quickStop(); - } catch (...) { - stopped = false; - } - } - std::lock_guard state_lock(resolved.control->mutex); - resolved.control->emergency_stop_in_progress = false; - } - if (!stopped) { - const std::string error = "AbstractMotor rejected emergency quick stop"; - setLastError(resolved.control, error); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); - } - fillFeedback(response->mutable_header(), true); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status::OK; -} - -grpc::Status gRPCMotorServiceImpl::getStatusImpl( - grpc::ServerContext*, - const api::GetMotorStatusRequest* request, - api::GetMotorStatusResponse* response) -{ - ResolvedMotor resolved; - auto status = resolveMotor( - request->target(), resolved, ResolveAccess::Observe); - if (!status.ok()) { - fillFeedback(response->mutable_header(), false, status.error_message()); - return status; - } - fillMotorStatus(resolved, response->mutable_status()); - if (!isFinite(response->status().position_rad()) || - !isFinite(response->status().velocity_rad_s())) { - const std::string error = - "motor status contains a non-finite position or velocity"; - fillFeedback(response->mutable_header(), false, error); - return grpc::Status(grpc::StatusCode::UNAVAILABLE, error); - } - fillFeedback(response->mutable_header(), true); - return grpc::Status::OK; -} - -grpc::Status gRPCMotorServiceImpl::setEnabledImpl( - grpc::ServerContext* context, - const api::SetMotorEnabledRequest* request, - api::MotorCommandResponse* response, - GrpcCommandTransaction& command) -{ - const auto started = Clock::now(); - ResolvedMotor resolved; - auto status = resolveMotor(request->target(), resolved); - if (!status.ok()) { - fillFeedback(response->mutable_header(), false, status.error_message()); - return status; - } - grpc::Status acquire_status; - auto lease = acquireControl( - resolved, api::MOTOR_CONTROL_SET_ENABLED, acquire_status, true); - if (!lease) { - fillFeedback(response->mutable_header(), false, - acquire_status.error_message()); - fillMotorStatus(resolved, response->mutable_status()); - return acquire_status; - } - - bool success = false; - bool preempted_after_dispatch = false; - bool cancelled_after_dispatch = false; - grpc::Status post_dispatch_cancel_status; - bool cleanup_succeeded = true; - { - std::lock_guard command_lock(resolved.control->command_mutex); - if (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline()) { - const auto cancelled = cancelledStatus(context); - lease.reset(); - fillFeedback(response->mutable_header(), false, - cancelled.error_message()); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return cancelled; - } - bool preempted = false; - { - std::lock_guard state_lock(resolved.control->mutex); - preempted = - resolved.control->cancel_generation != lease->generation() || - resolved.control->emergency_stop_in_progress; - } - if (preempted) { - const std::string error = - "enable/disable preempted by a stop request"; - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (!command.beginDispatch()) { - lease.reset(); - return command.dispatchStatus(); - } - success = request->enabled() - ? resolved.motor->torqueOn() - : resolved.motor->torqueOff(); - { - std::lock_guard state_lock(resolved.control->mutex); - preempted_after_dispatch = - resolved.control->cancel_generation != lease->generation() || - resolved.control->emergency_stop_in_progress; - } - if (!preempted_after_dispatch && - (context->IsCancelled() || - std::chrono::system_clock::now() >= context->deadline())) { - cancelled_after_dispatch = true; - post_dispatch_cancel_status = cancelledStatus(context); - } - - if (!preempted_after_dispatch && !cancelled_after_dispatch && - success && request->enabled()) { - std::lock_guard state_lock(resolved.control->mutex); - if (resolved.control->cancel_generation == lease->generation() && - !resolved.control->emergency_stop_in_progress) { - resolved.control->emergency_stopped = false; - resolved.control->last_error.clear(); - } else { - preempted_after_dispatch = true; - } - } - - // A false acknowledgement may still mean the backend committed the - // request. A successful enable also needs rollback if cancellation or - // E-stop won while torqueOn was in flight. - const bool cleanup_required = - (!success && !preempted_after_dispatch) || - (success && request->enabled() && - (preempted_after_dispatch || cancelled_after_dispatch)); - if (cleanup_required) { - const bool stopped = resolved.motor->quickStop(); - bool disabled = true; - if (request->enabled()) { - disabled = resolved.motor->torqueOff(); - } - cleanup_succeeded = stopped && disabled; - } - } - if (!cleanup_succeeded) { - const std::string error = - "failed to reach a safe state after uncertain enable/disable dispatch"; - latchUnsafeAfterFailedStop(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::INTERNAL, error); - } - if (preempted_after_dispatch) { - const std::string error = - "enable/disable preempted by a stop request during dispatch"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::ABORTED, error); - } - if (cancelled_after_dispatch) { - setLastError( - resolved.control, post_dispatch_cancel_status.error_message()); - lease.reset(); - fillFeedback(response->mutable_header(), false, - post_dispatch_cancel_status.error_message()); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return post_dispatch_cancel_status; - } - if (!success) { - const std::string error = request->enabled() - ? "AbstractMotor rejected enable after safe disable" - : "AbstractMotor rejected disable after safe stop"; - setLastError(resolved.control, error); - lease.reset(); - fillFeedback(response->mutable_header(), false, error); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); - } - lease.reset(); - fillFeedback(response->mutable_header(), true); - fillMotorStatus(resolved, response->mutable_status()); - response->set_elapsed_ms(elapsedMs(started)); - return grpc::Status::OK; -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_recovery_audit.cpp b/cmvr-es/service/grpc/server/src/grpc_recovery_audit.cpp deleted file mode 100644 index 915b9188..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_recovery_audit.cpp +++ /dev/null @@ -1,164 +0,0 @@ -#include "service/grpc/server/include/grpc_recovery_audit.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace cmvr::service { - -namespace { - -void setError(std::string* error, std::string message) noexcept -{ - if (!error) { - return; - } - try { - *error = std::move(message); - } catch (...) { - } -} - -class FileRecoveryAuditSink final : public RecoveryAuditSink { -public: - explicit FileRecoveryAuditSink(std::string path) - : path_(std::move(path)) - { - } - - bool append( - const RecoveryAuditRecord& record, - std::string* error) noexcept override - { - try { - google::protobuf::Struct event; - auto* fields = event.mutable_fields(); - (*fields)["schema_version"].set_string_value("1"); - (*fields)["occurred_at_unix_ms"].set_string_value( - std::to_string(record.occurred_at_unix_ms)); - (*fields)["stage"].set_string_value(record.stage); - (*fields)["correlation_id"].set_string_value( - record.correlation_id); - (*fields)["principal_id"].set_string_value(record.principal_id); - (*fields)["peer"].set_string_value(record.peer); - (*fields)["recovery_id"].set_string_value(record.recovery_id); - (*fields)["reason"].set_string_value(record.reason); - (*fields)["mode"].set_string_value(record.mode); - (*fields)["all_devices"].set_bool_value(record.all_devices); - (*fields)["expected_safety_epoch"].set_string_value( - std::to_string(record.expected_safety_epoch)); - (*fields)["previous_safety_epoch"].set_string_value( - std::to_string(record.previous_safety_epoch)); - (*fields)["current_safety_epoch"].set_string_value( - std::to_string(record.current_safety_epoch)); - (*fields)["result"].set_string_value(record.result); - auto* ids = (*fields)["device_ids"].mutable_list_value(); - for (const auto& id : record.device_ids) { - ids->add_values()->set_string_value(id); - } - - google::protobuf::util::JsonPrintOptions options; - options.preserve_proto_field_names = true; - std::string line; - const auto json_status = - google::protobuf::util::MessageToJsonString( - event, &line, options); - if (!json_status.ok()) { - setError(error, "failed to serialize recovery audit event"); - return false; - } - line.push_back('\n'); - - std::lock_guard lock(mutex_); - if (path_.empty()) { - setError(error, "recovery audit file is not configured"); - return false; - } - - int flags = O_WRONLY | O_CREAT | O_APPEND | O_CLOEXEC; -#ifdef O_NOFOLLOW - flags |= O_NOFOLLOW; -#endif - const int descriptor = ::open(path_.c_str(), flags, S_IRUSR | S_IWUSR); - if (descriptor < 0) { - setError( - error, - "failed to open recovery audit file: " + - std::string(std::strerror(errno))); - return false; - } - - const auto close_descriptor = [&] { (void)::close(descriptor); }; - struct stat metadata {}; - if (::fstat(descriptor, &metadata) != 0 || - !S_ISREG(metadata.st_mode) || metadata.st_uid != ::geteuid() || - (metadata.st_mode & (S_IRWXG | S_IRWXO)) != 0) { - close_descriptor(); - setError( - error, - "recovery audit file must be an owner-only regular file"); - return false; - } - - std::size_t written = 0; - while (written < line.size()) { - const auto count = ::write( - descriptor, - line.data() + written, - line.size() - written); - if (count < 0 && errno == EINTR) { - continue; - } - if (count <= 0) { - const auto message = std::string(std::strerror(errno)); - close_descriptor(); - setError( - error, - "failed to write recovery audit file: " + message); - return false; - } - written += static_cast(count); - } - if (::fdatasync(descriptor) != 0) { - const auto message = std::string(std::strerror(errno)); - close_descriptor(); - setError( - error, - "failed to sync recovery audit file: " + message); - return false; - } - close_descriptor(); - return true; - } catch (const std::exception& exception) { - setError( - error, - "recovery audit append threw: " + - std::string(exception.what())); - return false; - } catch (...) { - setError(error, "recovery audit append threw an unknown exception"); - return false; - } - } - -private: - std::string path_; - std::mutex mutex_; -}; - -} // namespace - -std::shared_ptr makeFileRecoveryAuditSink(std::string path) -{ - return std::make_shared(std::move(path)); -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_robot_arm_teleop_backend.cpp b/cmvr-es/service/grpc/server/src/grpc_robot_arm_teleop_backend.cpp deleted file mode 100644 index e8527717..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_robot_arm_teleop_backend.cpp +++ /dev/null @@ -1,649 +0,0 @@ -#include "service/grpc/server/include/grpc_robot_arm_teleop_backend.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::service { -namespace { - -using Clock = std::chrono::steady_clock; - -constexpr double kMinimumServoPeriodS = 0.0001; -constexpr double kMaximumServoPeriodS = 0.1; - -bool isDigest(const std::string& value) -{ - return value.size() == 64 && - std::all_of( - value.begin(), value.end(), [](const unsigned char value) { - return std::isxdigit(value) != 0; - }); -} - -arm_teleop::EffortSource toProtoEffortSource( - const device::JointEffortSource source) -{ - switch (source) { - case device::JointEffortSource::MotorEstimate: - return arm_teleop::EFFORT_SOURCE_MOTOR_ESTIMATE; - case device::JointEffortSource::JointSensor: - return arm_teleop::EFFORT_SOURCE_JOINT_SENSOR; - case device::JointEffortSource::ForceTorqueSensor: - return arm_teleop::EFFORT_SOURCE_FORCE_TORQUE_SENSOR; - case device::JointEffortSource::Observer: - return arm_teleop::EFFORT_SOURCE_OBSERVER; - case device::JointEffortSource::Unspecified: - default: - return arm_teleop::EFFORT_SOURCE_UNSPECIFIED; - } -} - -grpc::StatusCode toStatusCode(const device::ArmErrorCode code) -{ - switch (code) { - case device::ArmErrorCode::InvalidArgument: - case device::ArmErrorCode::InvalidDof: - return grpc::StatusCode::INVALID_ARGUMENT; - case device::ArmErrorCode::OutOfJointLimit: - case device::ArmErrorCode::OutOfVelocityLimit: - case device::ArmErrorCode::OutOfAccelerationLimit: - case device::ArmErrorCode::OutOfWorkspace: - return grpc::StatusCode::OUT_OF_RANGE; - case device::ArmErrorCode::NotConnected: - case device::ArmErrorCode::RobotNotReady: - case device::ArmErrorCode::RobotNotPowered: - case device::ArmErrorCode::RobotInFault: - case device::ArmErrorCode::RobotInProtectiveStop: - case device::ArmErrorCode::RobotInEmergencyStop: - case device::ArmErrorCode::CommandRejected: - case device::ArmErrorCode::UnsupportedCommand: - return grpc::StatusCode::FAILED_PRECONDITION; - case device::ArmErrorCode::Timeout: - return grpc::StatusCode::DEADLINE_EXCEEDED; - case device::ArmErrorCode::ConnectionFailed: - return grpc::StatusCode::UNAVAILABLE; - case device::ArmErrorCode::AlreadyConnected: - return grpc::StatusCode::ALREADY_EXISTS; - case device::ArmErrorCode::CommandFailed: - case device::ArmErrorCode::UnknownError: - case device::ArmErrorCode::OK: - default: - return grpc::StatusCode::INTERNAL; - } -} - -ArmTeleopBackendResult fromArmResult( - const device::Result& result, - const char* operation) -{ - if (result.ok()) { - return ArmTeleopBackendResult::ok(); - } - std::string detail(operation); - detail += " failed"; - if (!result.message.empty()) { - detail += ": " + result.message; - } - return ArmTeleopBackendResult::failure( - toStatusCode(result.code), std::move(detail)); -} - -class RobotArmTeleopBackend final : public ArmTeleopBackend { -public: - RobotArmTeleopBackend( - std::shared_ptr arm, - config::ArmTeleopBackendConfig config) - : arm_(std::move(arm)), - config_(std::move(config)), - require_powered_( - !config_.has_require_powered() || - config_.require_powered()) - { - validateStaticConfiguration(); - } - - bool available() const noexcept override - { - return unavailable_reason_.empty(); - } - - std::string unavailableReason() const override - { - return unavailable_reason_; - } - - arm_teleop::RobotManifest manifest() const override - { - return manifest_; - } - - bool supportsForceFeedback() const noexcept override - { - // A non-zero effort vector is not enough. The RobotArm must explicitly - // identify a verified effort source. - return available() && arm_->jointEffortSource() != - device::JointEffortSource::Unspecified; - } - - ArmTeleopBackendResult open( - const arm_teleop::OpenSession& request) override - { - std::lock_guard operation_lock(operation_mutex_); - if (!available()) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - unavailable_reason_); - } - if (session_open_) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::ALREADY_EXISTS, - "RobotArm teleoperation servo mode is already open"); - } - - const auto state = arm_->getRobotState(); - cacheState(state); - const auto safety_result = validateSafety(state); - if (!safety_result.success) { - return safety_result; - } - if (!validMeasuredPosition(state.actual_joint_state)) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - "RobotArm initial joint position cache is invalid"); - } - if (request.requested_command_rate_hz() == 0) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::INVALID_ARGUMENT, - "requested_command_rate_hz must be non-zero"); - } - const double requested_period_s = - 1.0 / - static_cast(request.requested_command_rate_hz()); - // A client may request a slower command stream, but it may not claim a - // rate faster than the reviewed RobotArm servo period. - if (requested_period_s + 1e-12 < - config_.servo_period_s()) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - "requested command rate exceeds configured RobotArm servo rate"); - } - minimum_dispatch_period_ = - std::chrono::duration_cast( - std::chrono::duration( - std::max( - requested_period_s, - config_.servo_period_s()))); - - device::ServoOptions options; - options.period = config_.servo_period_s(); - const auto start = Clock::now(); - const auto result = arm_->startServoMode(options); - const auto elapsed = Clock::now() - start; - if (!result.ok()) { - return fromArmResult(result, "startServoMode"); - } - if (elapsed > std::chrono::microseconds( - config_.max_apply_duration_us())) { - bestEffortStop(); - return ArmTeleopBackendResult::failure( - grpc::StatusCode::DEADLINE_EXCEEDED, - "startServoMode exceeded max_apply_duration_us"); - } - - initial_position_ = state.actual_joint_state.position; - last_position_.clear(); - last_dispatch_time_ = Clock::time_point{}; - session_open_ = true; - return ArmTeleopBackendResult::ok(); - } - - ArmTeleopBackendResult applySetpoint( - const arm_teleop::JointSetpoint& setpoint, - const Clock::time_point deadline) override - { - std::lock_guard operation_lock(operation_mutex_); - if (Clock::now() >= deadline) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::DEADLINE_EXCEEDED, - "setpoint expired before RobotArm backend validation"); - } - if (!session_open_) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - "RobotArm teleoperation servo mode is not open"); - } - - const auto input_result = validateSetpointInput(setpoint); - if (!input_result.success) { - return input_result; - } - - const auto state = arm_->getRobotState(); - cacheState(state); - const auto safety_result = validateSafety(state); - if (!safety_result.success) { - return safety_result; - } - - const std::vector target( - setpoint.position_rad().begin(), - setpoint.position_rad().end()); - const auto& reference = - last_position_.empty() ? initial_position_ : last_position_; - const double allowed_step = - last_position_.empty() - ? config_.max_initial_position_step_rad() - : config_.max_position_step_rad(); - for (std::size_t index = 0; index < target.size(); ++index) { - const double delta = std::abs(target[index] - reference[index]); - if (delta > allowed_step) { - std::ostringstream detail; - detail << "joint " << model_.joint_names[index] - << " position step " << delta - << " exceeds configured limit " << allowed_step; - return ArmTeleopBackendResult::failure( - grpc::StatusCode::OUT_OF_RANGE, detail.str()); - } - // Once a target has been accepted, also enforce the RobotModel - // velocity limit on target-to-target motion. - if (!last_position_.empty() && - delta / - std::chrono::duration( - minimum_dispatch_period_) - .count() > - model_.joint_limits[index].max_velocity) { - std::ostringstream detail; - detail << "joint " << model_.joint_names[index] - << " target delta exceeds RobotModel velocity limit"; - return ArmTeleopBackendResult::failure( - grpc::StatusCode::OUT_OF_RANGE, detail.str()); - } - } - - const auto dispatch_time = Clock::now(); - if (last_dispatch_time_ != Clock::time_point{} && - dispatch_time - last_dispatch_time_ < - minimum_dispatch_period_) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::RESOURCE_EXHAUSTED, - "setpoint arrived before the negotiated RobotArm dispatch period"); - } - - device::JointPositionCommand command; - command.position = target; - // Robot state validation and command preparation may consume the - // remaining validity window. Re-check on the receiver's monotonic - // timeline at the last point before the RobotArm commit. - const auto start = Clock::now(); - if (start >= deadline) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::DEADLINE_EXCEEDED, - "setpoint expired before RobotArm command dispatch"); - } - if (deadline - start < - std::chrono::microseconds( - config_.max_apply_duration_us())) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::DEADLINE_EXCEEDED, - "setpoint lacks the configured RobotArm apply-time budget"); - } - last_dispatch_time_ = start; - const auto result = arm_->servoJ(command); - const auto elapsed = Clock::now() - start; - if (!result.ok()) { - return fromArmResult(result, "servoJ"); - } - - // servoJ has already accepted this target even when the local timing - // contract is exceeded; remember it before returning the failure so a - // caller can never treat an older target as the last applied command. - last_position_ = target; - if (elapsed > std::chrono::microseconds( - config_.max_apply_duration_us())) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::DEADLINE_EXCEEDED, - "servoJ exceeded max_apply_duration_us"); - } - return ArmTeleopBackendResult::ok(); - } - - ArmTeleopBackendResult stop( - const arm_teleop::StopReason, - const std::string&) override - { - std::lock_guard operation_lock(operation_mutex_); - const auto motion_result = arm_ ? arm_->stopMotion() - : device::Result::success(); - // This is deliberately called even when stopMotion fails. - const auto servo_result = arm_ ? arm_->stopServoMode() - : device::Result::success(); - session_open_ = false; - initial_position_.clear(); - last_position_.clear(); - last_dispatch_time_ = Clock::time_point{}; - minimum_dispatch_period_ = Clock::duration::zero(); - if (arm_) { - cacheState(arm_->getRobotState()); - } - if (!motion_result.ok()) { - return fromArmResult(motion_result, "stopMotion"); - } - return fromArmResult(servo_result, "stopServoMode"); - } - - ArmTeleopBackendSnapshot snapshot() const override - { - std::lock_guard cache_lock(cache_mutex_); - auto result = cached_snapshot_; - if (cache_time_ != Clock::time_point{}) { - const auto age = - std::chrono::duration_cast( - Clock::now() - cache_time_) - .count(); - result.joint_state.set_sample_age_us( - age > 0 ? static_cast(age) : 0U); - } - return result; - } - -private: - void validateStaticConfiguration() - { - if (!config_.enable()) { - unavailable_reason_ = - "RobotArm teleoperation backend is explicitly disabled"; - return; - } - if (!arm_) { - unavailable_reason_ = - "configured RobotArm device was not found"; - return; - } - if (config_.device_id().empty() || - config_.device_id() != arm_->id()) { - unavailable_reason_ = - "arm_teleop device_id must exactly match RobotArm.id"; - return; - } - if (!arm_->supportsTeleopGroupServo()) { - unavailable_reason_ = - "RobotArm teleop group-servo capability is not enabled"; - return; - } - if (!isDigest(config_.model_sha256()) || - !isDigest(config_.calibration_sha256())) { - unavailable_reason_ = - "arm_teleop model and calibration SHA256 values must be 64 hex characters"; - return; - } - if (config_.base_frame().empty() || - config_.tool_frame().empty()) { - unavailable_reason_ = - "arm_teleop base_frame and tool_frame are required"; - return; - } - if (!std::isfinite(config_.servo_period_s()) || - config_.servo_period_s() < kMinimumServoPeriodS || - config_.servo_period_s() > kMaximumServoPeriodS) { - unavailable_reason_ = - "arm_teleop servo_period_s must be in [0.0001, 0.1]"; - return; - } - const double period_us = - config_.servo_period_s() * 1000000.0; - if (config_.max_apply_duration_us() == 0 || - static_cast(config_.max_apply_duration_us()) > - period_us) { - unavailable_reason_ = - "arm_teleop max_apply_duration_us must be non-zero and no greater than one servo period"; - return; - } - if (!std::isfinite(config_.max_initial_position_step_rad()) || - config_.max_initial_position_step_rad() <= 0.0 || - !std::isfinite(config_.max_position_step_rad()) || - config_.max_position_step_rad() <= 0.0) { - unavailable_reason_ = - "arm_teleop position step limits must be finite and positive"; - return; - } - - model_ = arm_->getRobotModel(); - if (!model_.valid() || - model_.joint_limits.size() != model_.dof) { - unavailable_reason_ = - "RobotModel must contain one safety limit for every joint"; - return; - } - std::unordered_set names; - for (std::size_t index = 0; index < model_.dof; ++index) { - const auto& name = model_.joint_names[index]; - const auto& limit = model_.joint_limits[index]; - if (name.empty() || !names.insert(name).second || - !std::isfinite(limit.lower) || - !std::isfinite(limit.upper) || - !std::isfinite(limit.max_velocity) || - limit.lower >= limit.upper || - limit.max_velocity <= 0.0) { - unavailable_reason_ = - "RobotModel joint names and position/velocity limits are invalid"; - return; - } - } - - manifest_.set_robot_id(config_.device_id()); - manifest_.set_model_sha256(config_.model_sha256()); - manifest_.set_calibration_sha256( - config_.calibration_sha256()); - for (const auto& name : model_.joint_names) { - manifest_.add_joint_names(name); - } - manifest_.set_position_unit("rad"); - manifest_.set_velocity_unit("rad/s"); - manifest_.set_effort_unit("N*m"); - manifest_.set_base_frame(config_.base_frame()); - manifest_.set_tool_frame(config_.tool_frame()); - } - - ArmTeleopBackendResult validateSafety( - const device::ArmState& state) const - { - if (!state.connected) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - "RobotArm is not connected"); - } - if (require_powered_ && !state.powered_on) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - "RobotArm is not powered on"); - } - if (state.emergency_stopped || - state.safety_mode == device::SafetyMode::EmergencyStop || - state.safety_mode == device::SafetyMode::SystemEmergencyStop) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - "RobotArm emergency stop is active"); - } - if (state.protective_stopped || - state.safety_mode == device::SafetyMode::ProtectiveStop || - state.safety_mode == device::SafetyMode::SafeguardStop) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - "RobotArm protective stop is active"); - } - if (state.fault || - state.robot_mode == device::RobotMode::Fault || - state.safety_mode == device::SafetyMode::Fault) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::FAILED_PRECONDITION, - "RobotArm fault is active"); - } - return ArmTeleopBackendResult::ok(); - } - - bool validMeasuredPosition( - const device::JointGroupState& state) const - { - if (!state.position_valid || - state.position.size() != model_.dof) { - return false; - } - for (std::size_t index = 0; index < model_.dof; ++index) { - const double value = state.position[index]; - const auto& limit = model_.joint_limits[index]; - if (!std::isfinite(value) || - value < limit.lower || value > limit.upper) { - return false; - } - } - return true; - } - - ArmTeleopBackendResult validateSetpointInput( - const arm_teleop::JointSetpoint& setpoint) const - { - if (setpoint.position_rad_size() != - static_cast(model_.dof) || - setpoint.velocity_rad_s_size() != - static_cast(model_.dof)) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::INVALID_ARGUMENT, - "setpoint dimensions do not match RobotModel"); - } - for (std::size_t index = 0; index < model_.dof; ++index) { - const double position = - setpoint.position_rad(static_cast(index)); - const double velocity = - setpoint.velocity_rad_s(static_cast(index)); - const auto& limit = model_.joint_limits[index]; - if (!std::isfinite(position) || !std::isfinite(velocity)) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::INVALID_ARGUMENT, - "setpoint position and velocity must be finite"); - } - if (position < limit.lower || position > limit.upper) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::OUT_OF_RANGE, - "setpoint position exceeds RobotModel joint limit"); - } - if (std::abs(velocity) > limit.max_velocity) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::OUT_OF_RANGE, - "setpoint velocity exceeds RobotModel joint limit"); - } - } - return ArmTeleopBackendResult::ok(); - } - - void cacheState(const device::ArmState& state) const - { - ArmTeleopBackendSnapshot snapshot; - auto* joint = &snapshot.joint_state; - const auto& source = state.actual_joint_state; - joint->set_sample_sequence(source.sequence); - for (const double value : source.position) { - joint->add_position_rad(value); - } - for (const double value : source.velocity) { - joint->add_velocity_rad_s(value); - } - - const auto effort_source = - toProtoEffortSource(arm_->jointEffortSource()); - const bool effort_valid = - source.effort_valid && - source.effort.size() == model_.dof && - effort_source != arm_teleop::EFFORT_SOURCE_UNSPECIFIED && - std::all_of( - source.effort.begin(), source.effort.end(), - [](const double value) { return std::isfinite(value); }); - if (effort_valid) { - for (const double value : source.effort) { - joint->add_effort_nm(value); - } - } - joint->set_position_valid(validMeasuredPosition(source)); - joint->set_velocity_valid( - source.velocity_valid && - source.velocity.size() == model_.dof && - std::all_of( - source.velocity.begin(), source.velocity.end(), - [](const double value) { return std::isfinite(value); })); - joint->set_effort_valid(effort_valid); - joint->set_effort_source( - effort_valid ? effort_source - : arm_teleop::EFFORT_SOURCE_UNSPECIFIED); - - auto* safety = &snapshot.safety; - safety->set_connected(state.connected); - safety->set_powered_on(state.powered_on); - safety->set_protective_stopped(state.protective_stopped); - safety->set_emergency_stopped(state.emergency_stopped); - safety->set_fault( - state.fault || - state.robot_mode == device::RobotMode::Fault || - state.safety_mode == device::SafetyMode::Fault); - if (safety->fault()) { - safety->set_fault_detail("RobotArm reports a fault"); - } - - std::lock_guard cache_lock(cache_mutex_); - cached_snapshot_ = std::move(snapshot); - cache_time_ = Clock::now(); - } - - void bestEffortStop() noexcept - { - try { - arm_->stopMotion(); - } catch (...) { - } - try { - arm_->stopServoMode(); - } catch (...) { - } - session_open_ = false; - initial_position_.clear(); - last_position_.clear(); - last_dispatch_time_ = Clock::time_point{}; - minimum_dispatch_period_ = Clock::duration::zero(); - } - - std::shared_ptr arm_; - config::ArmTeleopBackendConfig config_; - bool require_powered_{true}; - device::RobotModel model_; - arm_teleop::RobotManifest manifest_; - std::string unavailable_reason_; - - mutable std::mutex operation_mutex_; - bool session_open_{false}; - std::vector initial_position_; - std::vector last_position_; - Clock::time_point last_dispatch_time_{}; - Clock::duration minimum_dispatch_period_{Clock::duration::zero()}; - - mutable std::mutex cache_mutex_; - mutable ArmTeleopBackendSnapshot cached_snapshot_; - mutable Clock::time_point cache_time_{}; -}; - -} // namespace - -std::shared_ptr makeRobotArmTeleopBackend( - std::shared_ptr arm, - const config::ArmTeleopBackendConfig& config) -{ - return std::make_shared( - std::move(arm), config); -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_safety_participants.cpp b/cmvr-es/service/grpc/server/src/grpc_safety_participants.cpp deleted file mode 100644 index a7e39b2e..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_safety_participants.cpp +++ /dev/null @@ -1,1139 +0,0 @@ -#include "service/grpc/server/include/grpc_safety_participants.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "manager/media_source_manager/include/device_media_source_adapter.h" -#include "manager/safety_manager/include/safety_manager.h" -#include "manager/task_manager/include/task_manager.h" -#include "service/grpc/action/include/action_queue_executor.h" -#include "service/grpc/server/include/camera_operational_activity_registry.h" -#include "service/grpc/server/include/camera_ptz_activity_registry.h" -#include "service/grpc/server/include/media_activity_coordinator.h" -#include "service/grpc/server/include/motor_activity_coordinator.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" -#include "service/grpc/stop_all/include/stop_operation_dispatcher.h" - -namespace cmvr::service { - -namespace { - -using safety::BarrierToken; -using safety::ParticipantDescriptor; -using safety::ParticipantPhase; -using safety::ParticipantResult; -using safety::RecoveryCheckResult; -using safety::RecoveryContext; -using safety::SafetyOperationContext; -using safety::SafetyParticipant; -using safety::SafetyReason; - -constexpr auto kParticipantTimeout = std::chrono::seconds(5); - -std::chrono::milliseconds remaining( - const safety::SafetyClock::time_point deadline) noexcept -{ - const auto now = safety::SafetyClock::now(); - if (now >= deadline) { - return std::chrono::milliseconds::zero(); - } - return std::chrono::duration_cast( - deadline - now); -} - -ParticipantResult success() -{ - return {true, SafetyReason::None, {}}; -} - -ParticipantResult failure( - const SafetyReason reason, - std::string detail) -{ - return {false, reason, std::move(detail)}; -} - -class TokenSequence { -protected: - BarrierToken tokenFor( - const std::string& participant_id, - const SafetyOperationContext& context) - { - return { - participant_id, - context.operation_id, - context.safety_epoch, - sequence_.fetch_add(1, std::memory_order_relaxed) + 1U}; - } - -private: - std::atomic sequence_{0}; -}; - -class IngressParticipant final : public SafetyParticipant, - private TokenSequence { -public: - ParticipantDescriptor descriptor() const override - { - return {"subsystem:ingress", ParticipantPhase::Ingress, true, - kParticipantTimeout}; - } - - BarrierToken beginBarrier( - const SafetyOperationContext& context) override - { - const auto token = tokenFor(descriptor().participant_id, context); - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - std::lock_guard lock(mutex_); - tickets_.emplace(token.generation, Entry{ticket, false}); - return token; - } - - ParticipantResult requestQuiesce( - const BarrierToken&, - const SafetyOperationContext&) override - { - return success(); - } - - ParticipantResult verifyQuiescent( - const BarrierToken&, - const SafetyOperationContext&) override - { - return globalStopAllAdmissionGate().lockAdmission().accepting() - ? failure( - SafetyReason::StopUnconfirmed, - "system ingress admission did not close") - : success(); - } - - RecoveryCheckResult recoverAdmission( - const BarrierToken& token, - const RecoveryContext&) override - { - bool rearm = false; - { - std::lock_guard lock(mutex_); - const auto found = tickets_.find(token.generation); - if (found == tickets_.end()) { - return { - false, - SafetyReason::StopUnconfirmed, - "system ingress recovery barrier is unavailable"}; - } - rearm = found->second.consumed; - } - if (rearm) { - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - if (!ticket.valid()) { - return { - false, - SafetyReason::StopUnconfirmed, - "system ingress could not begin a recovery round"}; - } - std::lock_guard lock(mutex_); - const auto found = tickets_.find(token.generation); - if (found == tickets_.end() || !found->second.consumed) { - (void)globalStopAllAdmissionGate().finishStopAll( - ticket, false); - return { - false, - SafetyReason::StopUnconfirmed, - "system ingress recovery barrier changed while rearming"}; - } - found->second.ticket = ticket; - found->second.consumed = false; - } - return {true, SafetyReason::None, {}}; - } - - ParticipantResult releaseBarrier( - const BarrierToken& token) noexcept override - { - try { - StopAllAdmissionGate::StopAllTicket ticket; - { - std::lock_guard lock(mutex_); - const auto found = tickets_.find(token.generation); - if (found == tickets_.end()) { - return failure( - SafetyReason::StopUnconfirmed, - "system ingress barrier is unavailable"); - } - ticket = found->second.ticket; - } - const auto committed = globalStopAllAdmissionGate() - .finishStopAllDetailed(ticket, true); - std::lock_guard lock(mutex_); - const auto found = tickets_.find(token.generation); - if (found != tickets_.end()) { - if (committed.admission_reopened) { - tickets_.erase(found); - } else if (committed.ticket_consumed) { - found->second.consumed = true; - } - } - return committed.admission_reopened - ? success() - : failure( - SafetyReason::StopUnconfirmed, - "could not safely resume system admission"); - } catch (...) { - return failure( - SafetyReason::InternalError, - "system ingress barrier release threw an exception"); - } - } - -private: - struct Entry { - StopAllAdmissionGate::StopAllTicket ticket; - bool consumed{false}; - }; - std::mutex mutex_; - std::unordered_map tickets_; -}; - -class ActionQueueParticipant final : public SafetyParticipant, - private TokenSequence { -public: - ActionQueueParticipant( - std::string participant_id, - std::shared_ptr executor) - : participant_id_(std::move(participant_id)), - executor_(std::move(executor)) - { - } - - ParticipantDescriptor descriptor() const override - { - return {participant_id_, ParticipantPhase::Scheduler, true, - kParticipantTimeout}; - } - - BarrierToken beginBarrier( - const SafetyOperationContext& context) override - { - const auto token = tokenFor(participant_id_, context); - const auto ticket = executor_->beginStopAll(true); - if (!ticket.valid()) { - return {}; - } - std::lock_guard lock(mutex_); - tickets_.emplace(token.generation, Entry{ticket, false}); - return token; - } - - ParticipantResult requestQuiesce( - const BarrierToken&, - const SafetyOperationContext&) override - { - return success(); - } - - ParticipantResult verifyQuiescent( - const BarrierToken&, - const SafetyOperationContext& context) override - { - return executor_->waitForIdle(remaining(context.deadline)) - ? success() - : failure( - SafetyReason::ParticipantTimeout, - "ActionQueue did not become idle before the deadline"); - } - - RecoveryCheckResult recoverAdmission( - const BarrierToken& token, - const RecoveryContext& context) override - { - bool rearm = false; - { - std::lock_guard lock(mutex_); - const auto found = tickets_.find(token.generation); - if (found == tickets_.end()) { - return { - false, - SafetyReason::StopUnconfirmed, - "ActionQueue recovery barrier is unavailable"}; - } - rearm = found->second.consumed; - } - if (rearm) { - const auto ticket = executor_->beginStopAll(true); - if (!ticket.valid()) { - return { - false, - SafetyReason::SystemStopping, - "ActionQueue is shutting down and cannot reopen"}; - } - std::lock_guard lock(mutex_); - const auto found = tickets_.find(token.generation); - if (found == tickets_.end() || !found->second.consumed) { - (void)executor_->finishStopAll(ticket, false); - return { - false, - SafetyReason::StopUnconfirmed, - "ActionQueue recovery barrier changed while rearming"}; - } - found->second.ticket = ticket; - found->second.consumed = false; - } - const bool idle = executor_->waitForIdle(remaining(context.deadline)); - return { - idle, - idle ? SafetyReason::None : SafetyReason::ParticipantTimeout, - idle ? std::string{} - : "ActionQueue remains active during recovery"}; - } - - ParticipantResult releaseBarrier( - const BarrierToken& token) noexcept override - { - try { - ActionQueueExecutor::StopAllTicket ticket; - { - std::lock_guard lock(mutex_); - const auto found = tickets_.find(token.generation); - if (found == tickets_.end()) { - return failure( - SafetyReason::StopUnconfirmed, - "ActionQueue barrier is unavailable"); - } - ticket = found->second.ticket; - } - const bool reopened = executor_->finishStopAll(ticket, true); - std::lock_guard lock(mutex_); - const auto found = tickets_.find(token.generation); - if (found != tickets_.end()) { - if (reopened) { - tickets_.erase(found); - } else { - found->second.consumed = true; - } - } - return reopened - ? success() - : failure( - SafetyReason::StopUnconfirmed, - "could not safely resume ActionQueue admission"); - } catch (...) { - return failure( - SafetyReason::InternalError, - "ActionQueue barrier release threw an exception"); - } - } - -private: - struct Entry { - ActionQueueExecutor::StopAllTicket ticket; - bool consumed{false}; - }; - std::string participant_id_; - std::shared_ptr executor_; - std::mutex mutex_; - std::unordered_map tickets_; -}; - -struct DispatchedBatch { - std::vector handles; -}; - -ParticipantResult waitForBatch( - const DispatchedBatch& batch, - const safety::SafetyClock::time_point deadline, - const std::string& description) -{ - for (const auto& handle : batch.handles) { - const auto result = handle.waitUntil(deadline); - if (!result.completed) { - return failure( - SafetyReason::ParticipantTimeout, - description + " did not complete before the deadline"); - } - if (!result.result) { - return failure( - SafetyReason::StopUnconfirmed, - result.detail.empty() - ? description + " did not confirm stop" - : result.detail); - } - } - return success(); -} - -StopOperationDispatcher::Handle submitDeferred( - StopOperationDispatcher& dispatcher, - std::string prefix, - DeferredStopOperation operation) -{ - auto callback = std::move(operation.operation); - return dispatcher.submit( - std::move(prefix) + operation.resource_key, - [callback = std::move(callback)]() mutable { - const auto result = callback(); - return StopOperationDispatcher::OperationResult{ - result.success, result.detail}; - }); -} - -class MediaSessionParticipant final : public SafetyParticipant, - private TokenSequence { -public: - explicit MediaSessionParticipant( - std::shared_ptr dispatcher) - : dispatcher_(std::move(dispatcher)) - { - } - - ParticipantDescriptor descriptor() const override - { - return {"subsystem:media-sessions", - ParticipantPhase::ControlSession, true, - kParticipantTimeout}; - } - - BarrierToken beginBarrier( - const SafetyOperationContext& context) override - { - const auto token = tokenFor(descriptor().participant_id, context); - Entry entry; - entry.ticket = globalMediaActivityCoordinator().beginStopAll(true); - if (!entry.ticket.valid()) { - return {}; - } - std::lock_guard lock(mutex_); - entries_.emplace(token.generation, std::move(entry)); - return token; - } - - ParticipantResult requestQuiesce( - const BarrierToken& token, - const SafetyOperationContext&) override - { - std::vector operations; - std::string error; - Entry* entry = nullptr; - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found == entries_.end()) { - return failure( - SafetyReason::StopUnconfirmed, - "media session barrier is unavailable"); - } - if (!globalMediaActivityCoordinator() - .collectCancellationOperations( - found->second.ticket, operations, &error)) { - return failure( - SafetyReason::StopUnconfirmed, - error.empty() - ? "could not collect media session cancellations" - : error); - } - entry = &found->second; - entry->batch.handles.reserve(operations.size()); - for (auto& operation : operations) { - entry->batch.handles.push_back(submitDeferred( - *dispatcher_, "safety:media-session:", - std::move(operation))); - } - } - (void)entry; - return success(); - } - - ParticipantResult verifyQuiescent( - const BarrierToken& token, - const SafetyOperationContext& context) override - { - Entry entry; - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found == entries_.end()) { - return failure( - SafetyReason::StopUnconfirmed, - "media session barrier is unavailable"); - } - entry = found->second; - } - const auto batch = waitForBatch( - entry.batch, context.deadline, "media cancellation"); - if (!batch.success) { - return batch; - } - return globalMediaActivityCoordinator().waitForStopped( - entry.ticket, remaining(context.deadline)) - ? success() - : failure( - SafetyReason::ParticipantTimeout, - "media sessions remained active before the deadline"); - } - - RecoveryCheckResult recoverAdmission( - const BarrierToken& token, - const RecoveryContext& context) override - { - bool rearm = false; - MediaActivityCoordinator::StopAllTicket ticket; - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found == entries_.end()) { - return { - false, SafetyReason::StopUnconfirmed, - "media recovery barrier is unavailable"}; - } - ticket = found->second.ticket; - rearm = found->second.consumed; - } - if (rearm) { - auto replacement = - globalMediaActivityCoordinator().beginStopAll(true); - if (!replacement.valid()) { - return { - false, SafetyReason::StopUnconfirmed, - "could not rearm media admission during recovery"}; - } - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found == entries_.end() || !found->second.consumed) { - (void)globalMediaActivityCoordinator() - .finishStopAllDetailed(replacement, false); - return { - false, SafetyReason::StopUnconfirmed, - "media recovery barrier changed while rearming"}; - } - found->second.ticket = replacement; - found->second.batch = {}; - found->second.consumed = false; - ticket = replacement; - } - - const SafetyOperationContext operation_context{ - context.recovery_id, - context.safety_epoch, - context.deadline}; - const auto requested = requestQuiesce(token, operation_context); - if (!requested.success) { - return { - false, requested.reason, std::move(requested.detail)}; - } - const auto verified = verifyQuiescent(token, operation_context); - return { - verified.success, - verified.reason, - std::move(verified.detail)}; - } - const bool stopped = globalMediaActivityCoordinator().waitForStopped( - ticket, remaining(context.deadline)); - return { - stopped, - stopped ? SafetyReason::None : SafetyReason::ParticipantTimeout, - stopped ? std::string{} - : "media sessions remain active during recovery"}; - } - - ParticipantResult releaseBarrier( - const BarrierToken& token) noexcept override - { - try { - MediaActivityCoordinator::StopAllTicket ticket; - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found == entries_.end()) { - return failure( - SafetyReason::StopUnconfirmed, - "media session barrier is unavailable"); - } - ticket = found->second.ticket; - } - const auto committed = globalMediaActivityCoordinator() - .finishStopAllDetailed(ticket, true); - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found != entries_.end() && - found->second.ticket.generation == ticket.generation && - found->second.ticket.ticket_id == ticket.ticket_id) { - if (committed.admission_resumed) { - entries_.erase(found); - } else if (committed.ticket_consumed) { - found->second.consumed = true; - } - } - } - return committed.participant_stopped && - committed.admission_resumed - ? success() - : failure( - SafetyReason::StopUnconfirmed, - committed.ticket_consumed - ? "media stopped but admission remained latched" - : "could not safely resume media session admission"); - } catch (...) { - return failure( - SafetyReason::InternalError, - "media session barrier release threw an exception"); - } - } - -private: - struct Entry { - MediaActivityCoordinator::StopAllTicket ticket; - DispatchedBatch batch; - bool consumed{false}; - }; - std::shared_ptr dispatcher_; - std::mutex mutex_; - std::unordered_map entries_; -}; - -class MotorSessionParticipant final : public SafetyParticipant, - private TokenSequence { -public: - explicit MotorSessionParticipant( - std::shared_ptr dispatcher) - : dispatcher_(std::move(dispatcher)) - { - } - - ParticipantDescriptor descriptor() const override - { - return {"subsystem:motor-sessions", - ParticipantPhase::ControlSession, true, - kParticipantTimeout}; - } - - BarrierToken beginBarrier( - const SafetyOperationContext& context) override - { - const auto token = tokenFor(descriptor().participant_id, context); - Entry entry; - entry.ticket = globalMotorActivityCoordinator().beginStopAll(true); - if (!entry.ticket.valid()) { - return {}; - } - std::lock_guard lock(mutex_); - entries_.emplace(token.generation, std::move(entry)); - return token; - } - - ParticipantResult requestQuiesce( - const BarrierToken& token, - const SafetyOperationContext&) override - { - std::vector operations; - std::string error; - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found == entries_.end()) { - return failure( - SafetyReason::StopUnconfirmed, - "motor session barrier is unavailable"); - } - if (!globalMotorActivityCoordinator().collectStopOperations( - found->second.ticket, operations, &error)) { - return failure( - SafetyReason::StopUnconfirmed, - error.empty() ? "could not collect motor stop operations" - : error); - } - found->second.batch.handles.reserve(operations.size()); - for (auto& operation : operations) { - found->second.batch.handles.push_back(submitDeferred( - *dispatcher_, "safety:motor-session:", - std::move(operation))); - } - return success(); - } - - ParticipantResult verifyQuiescent( - const BarrierToken& token, - const SafetyOperationContext& context) override - { - Entry entry; - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found == entries_.end()) { - return failure( - SafetyReason::StopUnconfirmed, - "motor session barrier is unavailable"); - } - entry = found->second; - } - const auto batch = waitForBatch( - entry.batch, context.deadline, "motor quick-stop"); - if (!batch.success) { - return batch; - } - std::string error; - if (!globalMotorActivityCoordinator().waitForStopped( - entry.ticket, remaining(context.deadline), &error)) { - return failure( - SafetyReason::ParticipantTimeout, - error.empty() - ? "motor sessions remained active before the deadline" - : error); - } - return success(); - } - - RecoveryCheckResult recoverAdmission( - const BarrierToken& token, - const RecoveryContext& context) override - { - bool rearm = false; - MotorActivityCoordinator::StopAllTicket ticket; - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found == entries_.end()) { - return { - false, SafetyReason::StopUnconfirmed, - "motor recovery barrier is unavailable"}; - } - ticket = found->second.ticket; - rearm = found->second.consumed; - } - if (rearm) { - auto replacement = - globalMotorActivityCoordinator().beginStopAll(true); - if (!replacement.valid()) { - return { - false, SafetyReason::StopUnconfirmed, - "could not rearm motor admission during recovery"}; - } - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found == entries_.end() || !found->second.consumed) { - (void)globalMotorActivityCoordinator() - .finishStopAllDetailed(replacement, false); - return { - false, SafetyReason::StopUnconfirmed, - "motor recovery barrier changed while rearming"}; - } - found->second.ticket = replacement; - found->second.batch = {}; - found->second.consumed = false; - ticket = replacement; - } - - const SafetyOperationContext operation_context{ - context.recovery_id, - context.safety_epoch, - context.deadline}; - const auto requested = requestQuiesce(token, operation_context); - if (!requested.success) { - return { - false, requested.reason, std::move(requested.detail)}; - } - const auto verified = verifyQuiescent(token, operation_context); - return { - verified.success, - verified.reason, - std::move(verified.detail)}; - } - std::string error; - const bool stopped = globalMotorActivityCoordinator().waitForStopped( - ticket, remaining(context.deadline), &error); - return { - stopped, - stopped ? SafetyReason::None : SafetyReason::ParticipantTimeout, - stopped ? std::string{} : error}; - } - - ParticipantResult releaseBarrier( - const BarrierToken& token) noexcept override - { - try { - MotorActivityCoordinator::StopAllTicket ticket; - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found == entries_.end()) { - return failure( - SafetyReason::StopUnconfirmed, - "motor session barrier is unavailable"); - } - ticket = found->second.ticket; - } - const auto committed = globalMotorActivityCoordinator() - .finishStopAllDetailed(ticket, true); - { - std::lock_guard lock(mutex_); - const auto found = entries_.find(token.generation); - if (found != entries_.end() && - found->second.ticket.generation == ticket.generation && - found->second.ticket.ticket_id == ticket.ticket_id) { - if (committed.admission_resumed) { - entries_.erase(found); - } else if (committed.ticket_consumed) { - found->second.consumed = true; - } - } - } - return committed.participant_stopped && - committed.admission_resumed - ? success() - : failure( - SafetyReason::StopUnconfirmed, - committed.ticket_consumed - ? "motors stopped but admission remained latched" - : "could not safely resume motor session admission"); - } catch (...) { - return failure( - SafetyReason::InternalError, - "motor session barrier release threw an exception"); - } - } - -private: - struct Entry { - MotorActivityCoordinator::StopAllTicket ticket; - DispatchedBatch batch; - bool consumed{false}; - }; - std::shared_ptr dispatcher_; - std::mutex mutex_; - std::unordered_map entries_; -}; - -using StopFactory = std::function()>; - -class DeferredBatchParticipant final : public SafetyParticipant, - private TokenSequence { -public: - DeferredBatchParticipant( - ParticipantDescriptor descriptor, - std::shared_ptr dispatcher, - StopFactory factory) - : descriptor_(std::move(descriptor)), - dispatcher_(std::move(dispatcher)), - factory_(std::move(factory)) - { - } - - ParticipantDescriptor descriptor() const override { return descriptor_; } - - BarrierToken beginBarrier( - const SafetyOperationContext& context) override - { - const auto token = tokenFor(descriptor_.participant_id, context); - std::lock_guard lock(mutex_); - batches_.try_emplace(token.generation); - return token; - } - - ParticipantResult requestQuiesce( - const BarrierToken& token, - const SafetyOperationContext&) override - { - auto operations = factory_(); - std::lock_guard lock(mutex_); - const auto found = batches_.find(token.generation); - if (found == batches_.end()) { - return failure( - SafetyReason::StopUnconfirmed, - descriptor_.participant_id + " barrier is unavailable"); - } - found->second.handles.clear(); - found->second.handles.reserve(operations.size()); - for (auto& operation : operations) { - found->second.handles.push_back(submitDeferred( - *dispatcher_, - "safety:" + descriptor_.participant_id + ':', - std::move(operation))); - } - return success(); - } - - ParticipantResult verifyQuiescent( - const BarrierToken& token, - const SafetyOperationContext& context) override - { - DispatchedBatch batch; - { - std::lock_guard lock(mutex_); - const auto found = batches_.find(token.generation); - if (found == batches_.end()) { - return failure( - SafetyReason::StopUnconfirmed, - descriptor_.participant_id + " barrier is unavailable"); - } - batch = found->second; - } - return waitForBatch( - batch, context.deadline, descriptor_.participant_id); - } - - RecoveryCheckResult recoverAdmission( - const BarrierToken& token, - const RecoveryContext& context) override - { - SafetyOperationContext operation{ - context.recovery_id, context.safety_epoch, context.deadline}; - const auto requested = requestQuiesce(token, operation); - if (!requested.success) { - return {false, requested.reason, requested.detail}; - } - const auto verified = verifyQuiescent(token, operation); - return {verified.success, verified.reason, verified.detail}; - } - - ParticipantResult releaseBarrier( - const BarrierToken& token) noexcept override - { - try { - std::lock_guard lock(mutex_); - batches_.erase(token.generation); - return success(); - } catch (...) { - return failure( - SafetyReason::InternalError, - descriptor_.participant_id + - " barrier release threw an exception"); - } - } - -private: - ParticipantDescriptor descriptor_; - std::shared_ptr dispatcher_; - StopFactory factory_; - std::mutex mutex_; - std::unordered_map batches_; -}; - -std::vector taskStopOperations() -{ - std::vector operations; - const auto tasks = task::TaskManager::activitySnapshotIfInitialized(); - operations.reserve(tasks.size()); - for (const auto& task : tasks) { - const auto id = task ? task->id() : std::string{"unknown"}; - operations.push_back({ - "task:" + id, - [task] { - if (!task) { - return DeferredStopResult{ - false, "task activity target is null"}; - } - return DeferredStopResult{ - task->stopActivity(), - "task " + task->id() + - " operational stop was not confirmed"}; - }}); - } - return operations; -} - -std::vector cameraPtzStopOperations() -{ - std::vector operations; - const auto ids = globalCameraPtzActivityRegistry().trackedDeviceIds(); - operations.reserve(ids.size()); - for (const auto& id : ids) { - operations.push_back({ - "camera-ptz:" + id, - [id] { - std::vector failures; - const bool stopped = globalCameraPtzActivityRegistry() - .stopActivitiesForDevice(id, &failures); - return DeferredStopResult{ - stopped, - failures.empty() ? std::string{} : failures.front()}; - }}); - } - return operations; -} - -std::vector mediaSourceStopOperations() -{ - std::unordered_set unique_ids; - const auto merge = [&unique_ids](const std::vector& ids) { - unique_ids.insert(ids.begin(), ids.end()); - }; - merge(media::globalMediaSourceManager().trackedSourceIds()); - merge(globalCameraOperationalActivityRegistry().trackedDeviceIds()); - - std::vector operations; - operations.reserve(unique_ids.size()); - for (const auto& id : unique_ids) { - operations.push_back({ - "media-source:" + id, - [id] { - std::vector failures; - bool stopped = media::globalMediaSourceManager() - .stopSourcesForDevice(id, &failures); - if (!globalCameraOperationalActivityRegistry() - .stopActivitiesForDevice(id, &failures)) { - stopped = false; - } - return DeferredStopResult{ - stopped, - failures.empty() ? std::string{} : failures.front()}; - }}); - } - return operations; -} - -struct SharedParticipants { - std::size_t owners{0}; - std::vector ids; -}; - -std::mutex shared_participants_mutex; -std::unordered_map - shared_participants; - -void acquireSharedParticipants( - safety::SafetyManager& coordinator, - const std::shared_ptr& dispatcher) -{ - std::lock_guard lock(shared_participants_mutex); - auto found = shared_participants.find(&coordinator); - if (found != shared_participants.end()) { - ++found->second.owners; - return; - } - - std::vector> participants; - participants.push_back(std::make_shared()); - participants.push_back( - std::make_shared(dispatcher)); - participants.push_back( - std::make_shared(dispatcher)); - participants.push_back(std::make_shared( - ParticipantDescriptor{ - "subsystem:tasks", ParticipantPhase::Scheduler, true, - kParticipantTimeout}, - dispatcher, - taskStopOperations)); - participants.push_back(std::make_shared( - ParticipantDescriptor{ - "subsystem:camera-ptz", ParticipantPhase::Actuator, true, - kParticipantTimeout}, - dispatcher, - cameraPtzStopOperations)); - participants.push_back(std::make_shared( - ParticipantDescriptor{ - "subsystem:media-sources", - ParticipantPhase::PeripheralActivity, - true, - kParticipantTimeout}, - dispatcher, - mediaSourceStopOperations)); - - SharedParticipants entry; - entry.owners = 1; - try { - for (const auto& participant : participants) { - const auto id = participant->descriptor().participant_id; - if (!coordinator.registerParticipant(participant)) { - throw std::runtime_error( - "failed to register safety participant: " + id); - } - entry.ids.push_back(id); - } - } catch (...) { - for (const auto& id : entry.ids) { - (void)coordinator.unregisterParticipant(id); - } - throw; - } - shared_participants.emplace(&coordinator, std::move(entry)); -} - -void releaseSharedParticipants(safety::SafetyManager& coordinator) noexcept -{ - try { - std::lock_guard lock(shared_participants_mutex); - const auto found = shared_participants.find(&coordinator); - if (found == shared_participants.end() || --found->second.owners != 0) { - return; - } - for (const auto& id : found->second.ids) { - (void)coordinator.unregisterParticipant(id); - } - // A retained barrier owns its participant independently. Never keep a - // raw coordinator key after the final service owner has gone. - shared_participants.erase(found); - } catch (...) { - } -} - -} // namespace - -struct GrpcSafetyParticipantRegistration::Impl { - safety::SafetyManager* coordinator{nullptr}; - std::string action_participant_id; - bool shared_acquired{false}; - - ~Impl() - { - if (!coordinator) { - return; - } - if (!action_participant_id.empty()) { - (void)coordinator->unregisterParticipant(action_participant_id); - } - if (shared_acquired) { - releaseSharedParticipants(*coordinator); - } - } -}; - -GrpcSafetyParticipantRegistration::GrpcSafetyParticipantRegistration( - std::unique_ptr impl) - : impl_(std::move(impl)) -{ -} - -GrpcSafetyParticipantRegistration::~GrpcSafetyParticipantRegistration() = - default; -GrpcSafetyParticipantRegistration::GrpcSafetyParticipantRegistration( - GrpcSafetyParticipantRegistration&&) noexcept = default; -GrpcSafetyParticipantRegistration& -GrpcSafetyParticipantRegistration::operator=( - GrpcSafetyParticipantRegistration&&) noexcept = default; - -std::unique_ptr -registerGrpcSafetyParticipants( - safety::SafetyManager& coordinator, - std::shared_ptr action_queue, - std::shared_ptr stop_dispatcher) -{ - if (!action_queue || !stop_dispatcher) { - throw std::invalid_argument( - "safety participant registration requires live executors"); - } - auto impl = std::make_unique(); - impl->coordinator = &coordinator; - acquireSharedParticipants(coordinator, stop_dispatcher); - impl->shared_acquired = true; - impl->action_participant_id = - "subsystem:action-queue:" + action_queue->instanceId(); - auto action_participant = std::make_shared( - impl->action_participant_id, std::move(action_queue)); - if (!coordinator.registerParticipant(std::move(action_participant))) { - throw std::runtime_error( - "failed to register ActionQueue safety participant"); - } - return std::unique_ptr( - new GrpcSafetyParticipantRegistration(std::move(impl))); -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_safety_proto.cpp b/cmvr-es/service/grpc/server/src/grpc_safety_proto.cpp deleted file mode 100644 index fc5347da..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_safety_proto.cpp +++ /dev/null @@ -1,397 +0,0 @@ -#include "service/grpc/server/include/grpc_safety_proto.h" - -#include - -namespace cmvr::service { - -namespace { - -const char* participantPhaseName( - const safety::ParticipantPhase phase) noexcept -{ - switch (phase) { - case safety::ParticipantPhase::Ingress: return "INGRESS"; - case safety::ParticipantPhase::Scheduler: return "SCHEDULER"; - case safety::ParticipantPhase::ControlSession: - return "CONTROL_SESSION"; - case safety::ParticipantPhase::Actuator: return "ACTUATOR"; - case safety::ParticipantPhase::PeripheralActivity: - return "PERIPHERAL_ACTIVITY"; - case safety::ParticipantPhase::Verification: return "VERIFICATION"; - } - return "UNKNOWN"; -} - -void populateParticipantResult( - const safety::ParticipantResultView& source, - api::SafetyParticipantResultInfo& destination) -{ - destination.set_recorded(source.recorded); - destination.set_success(source.success); - destination.set_reason_code(toApiSafetyReason(source.reason)); - destination.set_detail(source.detail); -} - -} // namespace - -api::CommandReasonCode toApiSafetyReason( - const safety::SafetyReason value) noexcept -{ - using safety::SafetyReason; - switch (value) { - case SafetyReason::None: return api::COMMAND_REASON_CODE_NONE; - case SafetyReason::InvalidArgument: - return api::COMMAND_REASON_CODE_INVALID_ARGUMENT; - case SafetyReason::Unauthenticated: - return api::COMMAND_REASON_CODE_UNAUTHENTICATED; - case SafetyReason::PermissionDenied: - return api::COMMAND_REASON_CODE_PERMISSION_DENIED; - case SafetyReason::RecoveryRpcDisabled: - return api::COMMAND_REASON_CODE_RECOVERY_RPC_DISABLED; - case SafetyReason::DeviceNotFound: - return api::COMMAND_REASON_CODE_DEVICE_NOT_FOUND; - case SafetyReason::DeviceUnavailable: - return api::COMMAND_REASON_CODE_DEVICE_UNAVAILABLE; - case SafetyReason::UnsupportedCommand: - return api::COMMAND_REASON_CODE_UNSUPPORTED_COMMAND; - case SafetyReason::SystemStarting: - return api::COMMAND_REASON_CODE_SYSTEM_STARTING; - case SafetyReason::SystemStopping: - return api::COMMAND_REASON_CODE_SYSTEM_STOPPING; - case SafetyReason::SafetyLatched: - return api::COMMAND_REASON_CODE_SAFETY_LATCHED; - case SafetyReason::SafetyStateMissing: - return api::COMMAND_REASON_CODE_SAFETY_STATE_MISSING; - case SafetyReason::SafetyStateStale: - return api::COMMAND_REASON_CODE_SAFETY_STATE_STALE; - case SafetyReason::HardwareUnsafe: - return api::COMMAND_REASON_CODE_HARDWARE_UNSAFE; - case SafetyReason::EmergencyStopActive: - return api::COMMAND_REASON_CODE_EMERGENCY_STOP_ACTIVE; - case SafetyReason::ProtectiveStopActive: - return api::COMMAND_REASON_CODE_PROTECTIVE_STOP_ACTIVE; - case SafetyReason::DeviceDisconnected: - return api::COMMAND_REASON_CODE_DEVICE_DISCONNECTED; - case SafetyReason::DeviceFault: - return api::COMMAND_REASON_CODE_DEVICE_FAULT; - case SafetyReason::DeviceNotReady: - return api::COMMAND_REASON_CODE_DEVICE_NOT_READY; - case SafetyReason::DeviceStillMoving: - return api::COMMAND_REASON_CODE_DEVICE_STILL_MOVING; - case SafetyReason::ControlBusy: - return api::COMMAND_REASON_CODE_CONTROL_BUSY; - case SafetyReason::GenerationMismatch: - return api::COMMAND_REASON_CODE_GENERATION_MISMATCH; - case SafetyReason::CommandIdRequired: - return api::COMMAND_REASON_CODE_COMMAND_ID_REQUIRED; - case SafetyReason::CommandIdConflict: - return api::COMMAND_REASON_CODE_COMMAND_ID_CONFLICT; - case SafetyReason::ResultEvicted: - return api::COMMAND_REASON_CODE_RESULT_EVICTED; - case SafetyReason::LedgerExhausted: - return api::COMMAND_REASON_CODE_LEDGER_EXHAUSTED; - case SafetyReason::Backpressure: - return api::COMMAND_REASON_CODE_BACKPRESSURE; - case SafetyReason::DeadlineExceededBeforeDispatch: - return api::COMMAND_REASON_CODE_DEADLINE_EXCEEDED_BEFORE_DISPATCH; - case SafetyReason::OutcomeUnknown: - return api::COMMAND_REASON_CODE_OUTCOME_UNKNOWN; - case SafetyReason::ParticipantTimeout: - return api::COMMAND_REASON_CODE_PARTICIPANT_TIMEOUT; - case SafetyReason::StopUnconfirmed: - return api::COMMAND_REASON_CODE_STOP_UNCONFIRMED; - case SafetyReason::RecoveryEpochMismatch: - return api::COMMAND_REASON_CODE_RECOVERY_EPOCH_MISMATCH; - case SafetyReason::RecoveryReasonRequired: - return api::COMMAND_REASON_CODE_RECOVERY_REASON_REQUIRED; - case SafetyReason::RecoveryAuditFailed: - return api::COMMAND_REASON_CODE_RECOVERY_AUDIT_FAILED; - case SafetyReason::InternalError: - return api::COMMAND_REASON_CODE_INTERNAL_ERROR; - } - return api::COMMAND_REASON_CODE_INTERNAL_ERROR; -} - -safety::SafetyReason fromApiSafetyReason( - const api::CommandReasonCode value) noexcept -{ - using safety::SafetyReason; - switch (value) { - case api::COMMAND_REASON_CODE_NONE: return SafetyReason::None; - case api::COMMAND_REASON_CODE_INVALID_ARGUMENT: - return SafetyReason::InvalidArgument; - case api::COMMAND_REASON_CODE_UNAUTHENTICATED: - return SafetyReason::Unauthenticated; - case api::COMMAND_REASON_CODE_PERMISSION_DENIED: - return SafetyReason::PermissionDenied; - case api::COMMAND_REASON_CODE_RECOVERY_RPC_DISABLED: - return SafetyReason::RecoveryRpcDisabled; - case api::COMMAND_REASON_CODE_DEVICE_NOT_FOUND: - return SafetyReason::DeviceNotFound; - case api::COMMAND_REASON_CODE_DEVICE_UNAVAILABLE: - return SafetyReason::DeviceUnavailable; - case api::COMMAND_REASON_CODE_UNSUPPORTED_COMMAND: - return SafetyReason::UnsupportedCommand; - case api::COMMAND_REASON_CODE_SYSTEM_STARTING: - return SafetyReason::SystemStarting; - case api::COMMAND_REASON_CODE_SYSTEM_STOPPING: - return SafetyReason::SystemStopping; - case api::COMMAND_REASON_CODE_SAFETY_LATCHED: - return SafetyReason::SafetyLatched; - case api::COMMAND_REASON_CODE_SAFETY_STATE_MISSING: - return SafetyReason::SafetyStateMissing; - case api::COMMAND_REASON_CODE_SAFETY_STATE_STALE: - return SafetyReason::SafetyStateStale; - case api::COMMAND_REASON_CODE_HARDWARE_UNSAFE: - return SafetyReason::HardwareUnsafe; - case api::COMMAND_REASON_CODE_EMERGENCY_STOP_ACTIVE: - return SafetyReason::EmergencyStopActive; - case api::COMMAND_REASON_CODE_PROTECTIVE_STOP_ACTIVE: - return SafetyReason::ProtectiveStopActive; - case api::COMMAND_REASON_CODE_DEVICE_DISCONNECTED: - return SafetyReason::DeviceDisconnected; - case api::COMMAND_REASON_CODE_DEVICE_FAULT: - return SafetyReason::DeviceFault; - case api::COMMAND_REASON_CODE_DEVICE_NOT_READY: - return SafetyReason::DeviceNotReady; - case api::COMMAND_REASON_CODE_DEVICE_STILL_MOVING: - return SafetyReason::DeviceStillMoving; - case api::COMMAND_REASON_CODE_CONTROL_BUSY: - return SafetyReason::ControlBusy; - case api::COMMAND_REASON_CODE_GENERATION_MISMATCH: - return SafetyReason::GenerationMismatch; - case api::COMMAND_REASON_CODE_COMMAND_ID_REQUIRED: - return SafetyReason::CommandIdRequired; - case api::COMMAND_REASON_CODE_COMMAND_ID_CONFLICT: - return SafetyReason::CommandIdConflict; - case api::COMMAND_REASON_CODE_RESULT_EVICTED: - return SafetyReason::ResultEvicted; - case api::COMMAND_REASON_CODE_LEDGER_EXHAUSTED: - return SafetyReason::LedgerExhausted; - case api::COMMAND_REASON_CODE_BACKPRESSURE: - return SafetyReason::Backpressure; - case api::COMMAND_REASON_CODE_DEADLINE_EXCEEDED_BEFORE_DISPATCH: - return SafetyReason::DeadlineExceededBeforeDispatch; - case api::COMMAND_REASON_CODE_OUTCOME_UNKNOWN: - return SafetyReason::OutcomeUnknown; - case api::COMMAND_REASON_CODE_PARTICIPANT_TIMEOUT: - return SafetyReason::ParticipantTimeout; - case api::COMMAND_REASON_CODE_STOP_UNCONFIRMED: - return SafetyReason::StopUnconfirmed; - case api::COMMAND_REASON_CODE_RECOVERY_EPOCH_MISMATCH: - return SafetyReason::RecoveryEpochMismatch; - case api::COMMAND_REASON_CODE_RECOVERY_REASON_REQUIRED: - return SafetyReason::RecoveryReasonRequired; - case api::COMMAND_REASON_CODE_RECOVERY_AUDIT_FAILED: - return SafetyReason::RecoveryAuditFailed; - case api::COMMAND_REASON_CODE_INTERNAL_ERROR: - case api::COMMAND_REASON_CODE_UNSPECIFIED: - return SafetyReason::InternalError; - } - return SafetyReason::InternalError; -} - -api::SafetyTriState toApiSafetyTriState( - const safety::TriState value) noexcept -{ - switch (value) { - case safety::TriState::False: return api::SAFETY_TRI_STATE_FALSE; - case safety::TriState::True: return api::SAFETY_TRI_STATE_TRUE; - case safety::TriState::Unknown: break; - } - return api::SAFETY_TRI_STATE_UNKNOWN; -} - -api::SafetyCondition toApiSafetyCondition( - const safety::SafetyCondition value) noexcept -{ - switch (value) { - case safety::SafetyCondition::Nominal: - return api::SAFETY_CONDITION_NOMINAL; - case safety::SafetyCondition::Restricted: - return api::SAFETY_CONDITION_RESTRICTED; - case safety::SafetyCondition::Unsafe: - return api::SAFETY_CONDITION_UNSAFE; - case safety::SafetyCondition::Unknown: break; - } - return api::SAFETY_CONDITION_UNKNOWN; -} - -api::SystemAdmissionState toApiSystemAdmissionState( - const safety::SystemAdmissionState value) noexcept -{ - switch (value) { - case safety::SystemAdmissionState::Starting: - return api::SYSTEM_ADMISSION_STATE_STARTING; - case safety::SystemAdmissionState::Open: - return api::SYSTEM_ADMISSION_STATE_OPEN; - case safety::SystemAdmissionState::Stopping: - return api::SYSTEM_ADMISSION_STATE_STOPPING; - case safety::SystemAdmissionState::Latched: - return api::SYSTEM_ADMISSION_STATE_LATCHED; - case safety::SystemAdmissionState::Recovering: - return api::SYSTEM_ADMISSION_STATE_RECOVERING; - case safety::SystemAdmissionState::ShuttingDown: - return api::SYSTEM_ADMISSION_STATE_SHUTTING_DOWN; - } - return api::SYSTEM_ADMISSION_STATE_UNSPECIFIED; -} - -api::DeviceAdmissionState toApiDeviceAdmissionState( - const safety::DeviceAdmissionState value) noexcept -{ - switch (value) { - case safety::DeviceAdmissionState::Observing: - return api::DEVICE_ADMISSION_STATE_OBSERVING; - case safety::DeviceAdmissionState::Open: - return api::DEVICE_ADMISSION_STATE_OPEN; - case safety::DeviceAdmissionState::Blocked: - return api::DEVICE_ADMISSION_STATE_BLOCKED; - case safety::DeviceAdmissionState::Quarantined: - return api::DEVICE_ADMISSION_STATE_QUARANTINED; - case safety::DeviceAdmissionState::Recovering: - return api::DEVICE_ADMISSION_STATE_RECOVERING; - case safety::DeviceAdmissionState::Removed: - return api::DEVICE_ADMISSION_STATE_REMOVED; - } - return api::DEVICE_ADMISSION_STATE_UNSPECIFIED; -} - -api::SafetyBlockerScope toApiSafetyBlockerScope( - const safety::BlockerScope value) noexcept -{ - return value == safety::BlockerScope::System - ? api::SAFETY_BLOCKER_SCOPE_SYSTEM - : api::SAFETY_BLOCKER_SCOPE_DEVICE; -} - -api::SafetyRecoveryRequirement toApiRecoveryRequirement( - const safety::RecoveryRequirement value) noexcept -{ - switch (value) { - case safety::RecoveryRequirement::RefreshOnly: - return api::SAFETY_RECOVERY_REQUIREMENT_REFRESH_ONLY; - case safety::RecoveryRequirement::ClearSoftwareLatch: - return api::SAFETY_RECOVERY_REQUIREMENT_CLEAR_SOFTWARE_LATCH; - case safety::RecoveryRequirement::HardwareReleaseRequired: - return api::SAFETY_RECOVERY_REQUIREMENT_HARDWARE_RELEASE_REQUIRED; - case safety::RecoveryRequirement::ManualInspectionRequired: - return api::SAFETY_RECOVERY_REQUIREMENT_MANUAL_INSPECTION_REQUIRED; - } - return api::SAFETY_RECOVERY_REQUIREMENT_UNSPECIFIED; -} - -api::SafetyOperationResult toApiRecoveryResult( - const safety::RecoveryResultCode value) noexcept -{ - switch (value) { - case safety::RecoveryResultCode::Recovered: - return api::SAFETY_OPERATION_RESULT_RECOVERED; - case safety::RecoveryResultCode::VerifiedButStillBlocked: - return api::SAFETY_OPERATION_RESULT_VERIFIED_BUT_STILL_BLOCKED; - case safety::RecoveryResultCode::BlockerRemains: - return api::SAFETY_OPERATION_RESULT_BLOCKER_REMAINS; - case safety::RecoveryResultCode::EpochMismatch: - return api::SAFETY_OPERATION_RESULT_EPOCH_MISMATCH; - case safety::RecoveryResultCode::NothingToRecover: - return api::SAFETY_OPERATION_RESULT_NOTHING_TO_RECOVER; - case safety::RecoveryResultCode::TimedOut: - return api::SAFETY_OPERATION_RESULT_TIMED_OUT; - case safety::RecoveryResultCode::Failed: - return api::SAFETY_OPERATION_RESULT_FAILED; - } - return api::SAFETY_OPERATION_RESULT_FAILED; -} - -void populateDeviceSafetyState( - const safety::DeviceSafetyStateView& source, - api::DeviceSafetyStateInfo& destination) -{ - const auto& snapshot = source.safety.snapshot; - destination.set_device_id(source.descriptor.device_id); - destination.set_device_kind(device::toString(source.descriptor.kind)); - destination.set_policy_family( - safety::toString(source.descriptor.default_policy)); - destination.set_lifecycle_state(device::toString(source.lifecycle)); - destination.set_health_state(device::toString(source.health.state)); - destination.set_admission_state( - toApiDeviceAdmissionState(source.admission_state)); - destination.set_condition(toApiSafetyCondition(snapshot.condition)); - destination.set_has_sample(source.safety.has_sample); - destination.set_snapshot_fresh(source.safety.fresh); - if (source.safety.has_sample) { - destination.set_sample_age_ms(static_cast( - source.safety.sample_age.count() < 0 - ? 0 - : source.safety.sample_age.count())); - } - destination.set_sample_sequence(snapshot.sample_sequence); - destination.set_observed_at_unix_ms(snapshot.observed_at_unix_ms); - destination.set_device_generation(snapshot.device_generation); - destination.set_connected(toApiSafetyTriState(snapshot.connected)); - destination.set_operational_ready( - toApiSafetyTriState(snapshot.operational_ready)); - destination.set_quiescent(toApiSafetyTriState(snapshot.quiescent)); - destination.set_motion_active( - toApiSafetyTriState(snapshot.motion_active)); - destination.set_actuator_enabled( - toApiSafetyTriState(snapshot.actuator_enabled)); - destination.set_emergency_stop_active( - toApiSafetyTriState(snapshot.emergency_stop_active)); - destination.set_protective_stop_active( - toApiSafetyTriState(snapshot.protective_stop_active)); - destination.set_fault_active( - toApiSafetyTriState(snapshot.fault_active)); - - for (const auto& blocker : source.blockers) { - auto* target = destination.add_blockers(); - target->set_reason_code(toApiSafetyReason(blocker.reason)); - target->set_scope(toApiSafetyBlockerScope(blocker.scope)); - target->set_recovery_requirement( - toApiRecoveryRequirement(blocker.recovery_requirement)); - target->set_source_id(blocker.source_id); - target->set_operation_id(blocker.operation_id); - target->set_first_observed_at_unix_ms( - blocker.first_observed_at_unix_ms); - target->set_last_observed_at_unix_ms( - blocker.last_observed_at_unix_ms); - } -} - -void populateSafetyTargetResult( - const safety::SafetyTargetResult& source, - api::SafetyOperationTargetResult& destination) -{ - destination.set_target_id(source.target_id); - destination.set_result( - source.success ? api::SAFETY_OPERATION_RESULT_SUCCEEDED - : api::SAFETY_OPERATION_RESULT_FAILED); - destination.set_reason_code(toApiSafetyReason(source.reason)); - destination.set_detail(source.detail); - destination.set_before_state( - toApiDeviceAdmissionState(source.before_state)); - destination.set_after_state( - toApiDeviceAdmissionState(source.after_state)); -} - -void populateSafetyParticipantState( - const safety::ParticipantSafetyStateView& source, - api::SafetyParticipantStateInfo& destination) -{ - destination.set_participant_id(source.descriptor.participant_id); - destination.set_phase(participantPhaseName(source.descriptor.phase)); - destination.set_required(source.descriptor.required); - destination.set_registered(source.registered); - destination.set_barrier_active(source.barrier_active); - destination.set_barrier_retained(source.barrier_retained); - destination.set_operation_id(source.operation_id); - destination.set_safety_epoch(source.safety_epoch); - populateParticipantResult( - source.last_request, *destination.mutable_last_request()); - populateParticipantResult( - source.last_verify, *destination.mutable_last_verify()); - populateParticipantResult( - source.last_release, *destination.mutable_last_release()); -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_security.cpp b/cmvr-es/service/grpc/server/src/grpc_security.cpp deleted file mode 100644 index 78593af9..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_security.cpp +++ /dev/null @@ -1,688 +0,0 @@ -#include "service/grpc/server/include/grpc_security.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::service { - -namespace { - -constexpr std::size_t kMaxCorrelationIdLength = 64; -std::atomic next_correlation_id{1}; - -bool isSafeCorrelationId(const std::string& value) -{ - if (value.empty() || value.size() > kMaxCorrelationIdLength) { - return false; - } - return std::all_of( - value.begin(), value.end(), [](const unsigned char character) { - return std::isalnum(character) || character == '-' || - character == '_' || character == '.' || character == ':'; - }); -} - -std::string makeCorrelationId() -{ - const auto sequence = - next_correlation_id.fetch_add(1, std::memory_order_relaxed); - const auto now = std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()).count(); - std::ostringstream output; - output << "grpc-" << std::hex << now << '-' << sequence; - return output.str(); -} - -std::string correlationIdFrom( - const std::multimap& metadata) -{ - const auto range = metadata.equal_range("x-correlation-id"); - if (range.first != range.second && - std::next(range.first) == range.second && - isSafeCorrelationId(range.first->second)) { - return range.first->second; - } - return makeCorrelationId(); -} - -bool roleAllows(const GrpcPrincipal& principal, const GrpcRole required) -{ - const auto rank = [](const GrpcRole role) { - switch (role) { - case GrpcRole::Anonymous: return 0; - case GrpcRole::Observer: return 1; - case GrpcRole::Operator: return 2; - case GrpcRole::SafetyAdmin: return 3; - } - return -1; - }; - return std::any_of( - principal.roles.begin(), principal.roles.end(), - [&](const GrpcRole role) { return rank(role) >= rank(required); }); -} - -GrpcCallFacts factsFrom( - grpc::ServerContext* context, - const GrpcMethodPolicy& method, - const bool encrypted) -{ - GrpcCallFacts facts; - facts.full_method_name = method.full_method_name; - facts.transport_encrypted = encrypted; - facts.received_at = std::chrono::steady_clock::now(); - facts.deadline = std::chrono::steady_clock::time_point::max(); - - if (context) { - facts.peer = context->peer(); - for (const auto& [key, value] : context->client_metadata()) { - facts.metadata.emplace( - std::string(key.data(), key.size()), - std::string(value.data(), value.size())); - } - - const auto deadline = context->deadline(); - if (deadline != std::chrono::system_clock::time_point::max()) { - const auto remaining = deadline - std::chrono::system_clock::now(); - facts.deadline = remaining <= decltype(remaining)::zero() - ? facts.received_at - : facts.received_at + - std::chrono::duration_cast< - std::chrono::steady_clock::duration>( - remaining); - } - } - - facts.local_peer = isLocalGrpcPeer(facts.peer); - facts.correlation_id = correlationIdFrom(facts.metadata); - return facts; -} - -} // namespace - -bool isLoopbackGrpcHost(const std::string& host) noexcept -{ - return host == "127.0.0.1" || host == "localhost" || host == "::1" || - host == "[::1]" || host.rfind("unix:", 0) == 0; -} - -bool isLocalGrpcPeer(const std::string& peer) noexcept -{ - return peer.rfind("unix:", 0) == 0 || - peer.rfind("ipv4:127.", 0) == 0 || - peer == "ipv6:[::1]" || peer == "ipv6:::1"; -} - -const char* toString(const GrpcTransportSecurity value) noexcept -{ - switch (value) { - case GrpcTransportSecurity::Insecure: return "insecure"; - case GrpcTransportSecurity::ServerTls: return "server_tls"; - case GrpcTransportSecurity::MutualTls: return "mutual_tls"; - } - return "unknown"; -} - -const char* toString(const GrpcAuthenticationMethod value) noexcept -{ - switch (value) { - case GrpcAuthenticationMethod::Disabled: return "disabled"; - case GrpcAuthenticationMethod::StaticToken: return "static_token"; - case GrpcAuthenticationMethod::Jwt: return "jwt"; - case GrpcAuthenticationMethod::TlsClientCertificate: - return "tls_client_certificate"; - } - return "unknown"; -} - -const char* toString(const GrpcRecoveryExposure value) noexcept -{ - switch (value) { - case GrpcRecoveryExposure::Disabled: return "disabled"; - case GrpcRecoveryExposure::LocalOnly: return "local_only"; - case GrpcRecoveryExposure::Authorized: return "authorized"; - } - return "unknown"; -} - -const char* toString(const GrpcRole value) noexcept -{ - switch (value) { - case GrpcRole::Anonymous: return "anonymous"; - case GrpcRole::Observer: return "observer"; - case GrpcRole::Operator: return "operator"; - case GrpcRole::SafetyAdmin: return "safety_admin"; - } - return "unknown"; -} - -GrpcSecurityConfigResult resolveGrpcSecurityConfig( - const config::GRPCServerConfig& server_config, - const std::string& effective_host) -{ - GrpcSecurityConfigResult result; - const bool loopback = isLoopbackGrpcHost(effective_host); - - if (!server_config.has_security()) { - result.valid = true; - result.config.transport = GrpcTransportSecurity::Insecure; - result.config.authentication = GrpcAuthenticationMethod::Disabled; - result.config.recovery_exposure = GrpcRecoveryExposure::Disabled; - result.config.allow_insecure_non_loopback = !loopback; - result.config.insecure_non_loopback = !loopback; - result.config.legacy_compatibility = true; - result.warnings.emplace_back( - "missing grpc security config: using one-release legacy " - "INSECURE/DISABLED compatibility with recovery disabled"); - return result; - } - - const auto& security = server_config.security(); - if (security.transport_mode() != config::GRPCSecurityConfig::INSECURE) { - result.error = - "only INSECURE gRPC transport is compiled in this release"; - return result; - } - result.config.transport = GrpcTransportSecurity::Insecure; - - if (security.authentication_mode() != - config::GRPCSecurityConfig::DISABLED) { - result.error = - "only DISABLED gRPC authentication is compiled in this release"; - return result; - } - result.config.authentication = GrpcAuthenticationMethod::Disabled; - - switch (security.recovery_exposure()) { - case config::GRPCSecurityConfig::RECOVERY_DISABLED: - result.config.recovery_exposure = GrpcRecoveryExposure::Disabled; - break; - case config::GRPCSecurityConfig::RECOVERY_LOCAL_ONLY: - result.config.recovery_exposure = GrpcRecoveryExposure::LocalOnly; - break; - case config::GRPCSecurityConfig::RECOVERY_AUTHORIZED: - result.error = - "RECOVERY_AUTHORIZED requires an implemented authentication " - "provider"; - return result; - case config::GRPCSecurityConfig::RECOVERY_EXPOSURE_UNSPECIFIED: - default: - result.error = "grpc recovery exposure must be explicit"; - return result; - } - result.config.recovery_audit_file = security.audit_file(); - if (result.config.recovery_exposure != - GrpcRecoveryExposure::Disabled && - result.config.recovery_audit_file.empty()) { - result.error = - "an enabled recovery RPC requires a persistent audit_file"; - return result; - } - - result.config.allow_insecure_non_loopback = - security.allow_insecure_non_loopback(); - result.config.insecure_non_loopback = !loopback; - if (!loopback && !security.allow_insecure_non_loopback()) { - result.error = - "INSECURE/DISABLED gRPC on a non-loopback host requires " - "allow_insecure_non_loopback=true"; - return result; - } - if (!loopback) { - result.warnings.emplace_back( - "gRPC is listening without transport encryption or client " - "authentication on a non-loopback host"); - } - - result.valid = true; - return result; -} - -GrpcAuthenticationResult DisabledGrpcAuthenticationProvider::authenticate( - const GrpcCallFacts&) const -{ - GrpcAuthenticationResult result; - result.principal.id = "anonymous"; - result.principal.method = GrpcAuthenticationMethod::Disabled; - result.principal.authenticated = false; - result.principal.roles = {GrpcRole::Anonymous}; - result.status = grpc::Status::OK; - return result; -} - -CompatibilityGrpcAuthorizationPolicy::CompatibilityGrpcAuthorizationPolicy( - const GrpcRecoveryExposure recovery_exposure) - : recovery_exposure_(recovery_exposure) -{ -} - -GrpcAuthorizationDecision CompatibilityGrpcAuthorizationPolicy::authorize( - const GrpcRequestContext& context, - const GrpcMethodPolicy& method) const -{ - if (method.access == GrpcAccessClass::Recover) { - switch (recovery_exposure_) { - case GrpcRecoveryExposure::Disabled: - return { - false, - grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "RECOVERY_RPC_DISABLED")}; - case GrpcRecoveryExposure::LocalOnly: - if (!context.local_peer) { - return { - false, - grpc::Status( - grpc::StatusCode::PERMISSION_DENIED, - "RecoverSafetyState is restricted to a local peer")}; - } - return {true, grpc::Status::OK}; - case GrpcRecoveryExposure::Authorized: - if (!context.principal.authenticated || - !roleAllows(context.principal, GrpcRole::SafetyAdmin)) { - return { - false, - grpc::Status( - context.principal.authenticated - ? grpc::StatusCode::PERMISSION_DENIED - : grpc::StatusCode::UNAUTHENTICATED, - "RecoverSafetyState requires SafetyAdmin")}; - } - return {true, grpc::Status::OK}; - } - } - - if (!context.principal.authenticated && - context.principal.method == GrpcAuthenticationMethod::Disabled) { - return {true, grpc::Status::OK}; - } - if (!context.principal.authenticated) { - return { - false, - grpc::Status( - grpc::StatusCode::UNAUTHENTICATED, - "gRPC caller authentication failed")}; - } - if (!roleAllows(context.principal, method.minimum_role)) { - return { - false, - grpc::Status( - grpc::StatusCode::PERMISSION_DENIED, - "gRPC caller does not have the required role")}; - } - return {true, grpc::Status::OK}; -} - -bool GrpcMethodPolicyRegistry::registerPolicy(GrpcMethodPolicy policy) -{ - if (policy.full_method_name.empty()) { - return false; - } - return policies_.emplace(policy.full_method_name, std::move(policy)).second; -} - -std::optional GrpcMethodPolicyRegistry::find( - const std::string& full_method_name) const -{ - const auto found = policies_.find(full_method_name); - if (found == policies_.end()) { - return std::nullopt; - } - return found->second; -} - -std::vector GrpcMethodPolicyRegistry::snapshot() const -{ - std::vector result; - result.reserve(policies_.size()); - for (const auto& [name, policy] : policies_) { - (void)name; - result.push_back(policy); - } - std::sort( - result.begin(), result.end(), - [](const auto& lhs, const auto& rhs) { - return lhs.full_method_name < rhs.full_method_name; - }); - return result; -} - -const GrpcMethodPolicyRegistry& defaultGrpcMethodPolicyRegistry() -{ - static const GrpcMethodPolicyRegistry registry = [] { - GrpcMethodPolicyRegistry result; - const auto add = [&result]( - const char* service, - const char* method, - const GrpcAccessClass access, - const safety::CommandIntent intent, - const safety::SafetyPolicyFamily family, - const bool mutating, - const bool safety_lane = false) { - GrpcRole role = GrpcRole::Observer; - if (access == GrpcAccessClass::Mutate || - access == GrpcAccessClass::Stop) { - role = GrpcRole::Operator; - } else if (access == GrpcAccessClass::Recover) { - role = GrpcRole::SafetyAdmin; - } - GrpcMethodPolicy policy; - policy.full_method_name = - std::string("/cmvr.api.") + service + '/' + method; - policy.access = access; - policy.minimum_role = role; - policy.command_intent = intent; - policy.policy_family = family; - policy.mutating = mutating; - policy.safety_lane = safety_lane; - if (!result.registerPolicy(std::move(policy))) { - throw std::logic_error( - std::string("duplicate gRPC method policy: ") + - service + '/' + method); - } - }; - const auto add_many = [&add]( - const char* service, - const std::initializer_list methods, - const GrpcAccessClass access, - const safety::CommandIntent intent, - const safety::SafetyPolicyFamily family, - const bool mutating, - const bool safety_lane = false) { - for (const auto* method : methods) { - add(service, method, access, intent, family, mutating, - safety_lane); - } - }; - - using safety::CommandIntent; - using safety::SafetyPolicyFamily; - add_many("SystemService", - {"GetSystemInfo", "GetSystemStatus", "GetDeviceList", - "GetSafetyState"}, - GrpcAccessClass::Read, CommandIntent::Observe, - SafetyPolicyFamily::Sensor, false); - add("SystemService", "UpdateParams", GrpcAccessClass::Mutate, - CommandIntent::Configure, SafetyPolicyFamily::Sensor, true); - add("SystemService", "StopAll", GrpcAccessClass::Stop, - CommandIntent::Stop, SafetyPolicyFamily::Control, true, true); - add("SystemService", "ExecuteActionQueue", GrpcAccessClass::Mutate, - CommandIntent::Actuate, SafetyPolicyFamily::Control, true); - add("SystemService", "RecoverSafetyState", GrpcAccessClass::Recover, - CommandIntent::RecoverAdmission, SafetyPolicyFamily::Control, - true, true); - - add_many("ArmService", - {"getJointState", "getPose", "getPoseMatrix", - "computeForwardKinematics"}, - GrpcAccessClass::Read, CommandIntent::Observe, - SafetyPolicyFamily::Control, false); - add_many("ArmService", {"torqueOff", "stopMotion"}, - GrpcAccessClass::Stop, CommandIntent::Stop, - SafetyPolicyFamily::Control, true, true); - add("ArmService", "clearFault", GrpcAccessClass::Mutate, - CommandIntent::ResetFault, SafetyPolicyFamily::Control, true); - add("ArmService", "torqueOn", GrpcAccessClass::Mutate, - CommandIntent::StartActivity, SafetyPolicyFamily::Control, true); - add_many("ArmService", - {"moveJ", "moveL", "speedJ", "speedL", "servoJ"}, - GrpcAccessClass::Mutate, CommandIntent::Actuate, - SafetyPolicyFamily::Control, true); - add_many("ArmService", {"calibrateZeroQ", "ExecuteJsonCommand"}, - GrpcAccessClass::Mutate, CommandIntent::Configure, - SafetyPolicyFamily::Control, true); - add("armteleop.v1.ArmTeleopService", "Teleoperate", - GrpcAccessClass::Mutate, - CommandIntent::Actuate, SafetyPolicyFamily::Control, true); - - add_many("AgvService", - {"getRuntimeState", "getNavigationStatus", "listMaps", - "listStations", "downloadMap", "streamMap"}, - GrpcAccessClass::Read, CommandIntent::Observe, - SafetyPolicyFamily::Control, false); - add_many("AgvService", - {"emergencyStop", "pauseNavigation", "cancelNavigation", - "stopVelocityControl", "stopMapping"}, - GrpcAccessClass::Stop, CommandIntent::Stop, - SafetyPolicyFamily::Control, true, true); - add("AgvService", "clearFault", GrpcAccessClass::Mutate, - CommandIntent::ResetFault, SafetyPolicyFamily::Control, true); - add("AgvService", "resumeNavigation", GrpcAccessClass::Mutate, - CommandIntent::StartActivity, SafetyPolicyFamily::Control, true); - add_many("AgvService", - {"navigateToPose", "navigateToStation", "followPath", - "setVelocity", "translate"}, - GrpcAccessClass::Mutate, CommandIntent::Actuate, - SafetyPolicyFamily::Control, true); - add_many("AgvService", - {"switchMap", "uploadMap", "startMapping"}, - GrpcAccessClass::Mutate, CommandIntent::Configure, - SafetyPolicyFamily::Control, true); - - add("MotorService", "getStatus", GrpcAccessClass::Read, - CommandIntent::Observe, SafetyPolicyFamily::Control, false); - add("MotorService", "emergencyStop", GrpcAccessClass::Stop, - CommandIntent::Stop, SafetyPolicyFamily::Control, true, true); - add_many("MotorService", - {"moveToZero", "profilePosition", "profileVelocity", - "streamCyclicPosition", "streamCyclicVelocity"}, - GrpcAccessClass::Mutate, CommandIntent::Actuate, - SafetyPolicyFamily::Control, true); - add_many("MotorService", {"setZero", "setEnabled"}, - GrpcAccessClass::Mutate, CommandIntent::Configure, - SafetyPolicyFamily::Control, true); - - add_many("DexHandService", - {"GetStatus", "GetSensorData", "GetSensorDataStream"}, - GrpcAccessClass::Read, CommandIntent::Observe, - SafetyPolicyFamily::Control, false); - add_many("DexHandService", - {"SetDexHandPos", "SetDexHandAngle", "SetDexHandForce", - "SetDexHandSpeed", "SetDexHandPresetAct"}, - GrpcAccessClass::Mutate, CommandIntent::Actuate, - SafetyPolicyFamily::Control, true); - - add_many("CameraService", - {"GetStatus", "GetRGBImage", "GetDepthImage", - "GetRGBDImages", "GetRGBImageStream", - "GetDepthImageStream", "GetRGBDImagesStream"}, - GrpcAccessClass::Read, CommandIntent::Observe, - SafetyPolicyFamily::Sensor, false); - add_many("CameraService", {"StartCamera", "StartRecording"}, - GrpcAccessClass::Mutate, CommandIntent::StartActivity, - SafetyPolicyFamily::Sensor, true); - add_many("CameraService", {"StopCamera", "StopRecording"}, - GrpcAccessClass::Stop, CommandIntent::Stop, - SafetyPolicyFamily::Sensor, true, true); - add("CameraService", "ControlPtz", GrpcAccessClass::Mutate, - CommandIntent::Actuate, SafetyPolicyFamily::Control, true); - - add_many("MicPhoneService", {"GetStatus", "StreamAudio", "GetVolume"}, - GrpcAccessClass::Read, CommandIntent::Observe, - SafetyPolicyFamily::Sensor, false); - add_many("MicPhoneService", {"StartRecord", "ResumeRecord"}, - GrpcAccessClass::Mutate, CommandIntent::StartActivity, - SafetyPolicyFamily::Sensor, true); - add_many("MicPhoneService", {"StopRecord", "PauseRecord"}, - GrpcAccessClass::Stop, CommandIntent::Stop, - SafetyPolicyFamily::Sensor, true, true); - add("MicPhoneService", "SetVolume", GrpcAccessClass::Mutate, - CommandIntent::Configure, SafetyPolicyFamily::Sensor, true); - - add_many("SpeakerService", {"GetStatus", "GetVolume"}, - GrpcAccessClass::Read, CommandIntent::Observe, - SafetyPolicyFamily::Sensor, false); - add_many("SpeakerService", - {"PlayAudio", "StreamAudio", "ResumePlayback"}, - GrpcAccessClass::Mutate, CommandIntent::StartActivity, - SafetyPolicyFamily::Sensor, true); - add_many("SpeakerService", {"StopPlayback", "PausePlayback"}, - GrpcAccessClass::Stop, CommandIntent::Stop, - SafetyPolicyFamily::Sensor, true, true); - add("SpeakerService", "SetVolume", GrpcAccessClass::Mutate, - CommandIntent::Configure, SafetyPolicyFamily::Sensor, true); - - add("BioHeadService", "GetSystemStatus", GrpcAccessClass::Read, - CommandIntent::Observe, SafetyPolicyFamily::Control, false); - add_many("BioHeadService", {"EmergencyStop", "SpeakStop"}, - GrpcAccessClass::Stop, CommandIntent::Stop, - SafetyPolicyFamily::Control, true, true); - add_many("BioHeadService", - {"SetExpression", "StreamExpression", "SpeakStart", - "Happy", "Surprise", "ExpressionTired", - "ExpressionAngry", "ExpressionSadness", - "ExpressionYawn"}, - GrpcAccessClass::Mutate, CommandIntent::Actuate, - SafetyPolicyFamily::Control, true); - - add("HlcService", "touch", GrpcAccessClass::Mutate, - CommandIntent::Actuate, SafetyPolicyFamily::Control, true); - add("TestService", "Call", GrpcAccessClass::Read, - CommandIntent::Observe, SafetyPolicyFamily::Sensor, false); - return result; - }(); - return registry; -} - -GrpcCallGuard::GrpcCallGuard( - GrpcRequestContext context, - GrpcAuthorizationDecision decision) - : context_(std::move(context)), decision_(std::move(decision)) -{ -} - -GrpcSecurityGateway::GrpcSecurityGateway( - GrpcSecurityRuntimeConfig config, - std::shared_ptr authentication, - std::shared_ptr authorization, - GrpcSecurityAuditSink audit_sink) - : config_(std::move(config)), - authentication_(std::move(authentication)), - authorization_(std::move(authorization)), - audit_sink_(std::move(audit_sink)) -{ -} - -GrpcCallGuard GrpcSecurityGateway::beginCall( - grpc::ServerContext* server_context, - const GrpcMethodPolicy& method) const -{ - return beginCall( - factsFrom( - server_context, method, - config_.transport != GrpcTransportSecurity::Insecure), - method); -} - -GrpcCallGuard GrpcSecurityGateway::beginCall( - GrpcCallFacts facts, - const GrpcMethodPolicy& method) const -{ - if (facts.full_method_name.empty()) { - facts.full_method_name = method.full_method_name; - } - if (facts.received_at == std::chrono::steady_clock::time_point{}) { - facts.received_at = std::chrono::steady_clock::now(); - } - if (facts.deadline == std::chrono::steady_clock::time_point{}) { - facts.deadline = std::chrono::steady_clock::time_point::max(); - } - if (facts.correlation_id.empty()) { - facts.correlation_id = correlationIdFrom(facts.metadata); - } - if (!facts.local_peer) { - facts.local_peer = isLocalGrpcPeer(facts.peer); - } - - const auto authentication = authentication_->authenticate(facts); - GrpcRequestContext context{ - facts.correlation_id, - facts.full_method_name, - facts.peer, - authentication.principal, - facts.transport_encrypted, - facts.local_peer, - facts.received_at, - facts.deadline}; - - GrpcAuthorizationDecision decision; - if (!authentication.ok()) { - decision = {false, authentication.status}; - } else { - decision = authorization_->authorize(context, method); - } - - if (audit_sink_) { - audit_sink_(GrpcSecurityAuditRecord{ - context.correlation_id, - context.full_method_name, - context.principal.id, - context.peer, - context.principal.method, - method.access, - context.principal.authenticated, - decision.allowed, - decision.status.error_code()}); - } - return GrpcCallGuard(std::move(context), std::move(decision)); -} - -std::shared_ptr makeGrpcSecurityGateway( - const GrpcSecurityRuntimeConfig& config, - GrpcSecurityAuditSink audit_sink) -{ - return std::make_shared( - config, - std::make_shared(), - std::make_shared( - config.recovery_exposure), - std::move(audit_sink)); -} - -std::shared_ptr makeDefaultGrpcSecurityGateway() -{ - GrpcSecurityRuntimeConfig config; - config.transport = GrpcTransportSecurity::Insecure; - config.authentication = GrpcAuthenticationMethod::Disabled; - config.recovery_exposure = GrpcRecoveryExposure::Disabled; - config.legacy_compatibility = true; - return makeGrpcSecurityGateway(config); -} - -GrpcCallGuard beginRegisteredGrpcCall( - const std::shared_ptr& gateway, - grpc::ServerContext* server_context, - const std::string& full_method_name) -{ - const auto policy = - defaultGrpcMethodPolicyRegistry().find(full_method_name); - if (!policy.has_value()) { - GrpcRequestContext context; - context.full_method_name = full_method_name; - context.received_at = std::chrono::steady_clock::now(); - context.deadline = std::chrono::steady_clock::time_point::max(); - if (server_context) { - context.peer = server_context->peer(); - context.local_peer = isLocalGrpcPeer(context.peer); - } - return GrpcCallGuard( - std::move(context), - {false, - grpc::Status( - grpc::StatusCode::INTERNAL, - "gRPC method has no registered security policy")}); - } - const auto active_gateway = - gateway ? gateway : makeDefaultGrpcSecurityGateway(); - return active_gateway->beginCall(server_context, *policy); -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/grpc_system_service.cpp b/cmvr-es/service/grpc/server/src/grpc_system_service.cpp deleted file mode 100644 index 211d2f37..00000000 --- a/cmvr-es/service/grpc/server/src/grpc_system_service.cpp +++ /dev/null @@ -1,2189 +0,0 @@ -// -// Created by xtkuang on 2025/6/6. -// - -#include "../include/grpc_system_service.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/base/logging/logger.h" -#include "devices/agv/abstract_agv.h" -#include "devices/arm/robot_arm.h" -#include "devices/biohead/abstract_biohead.h" -#include "devices/camera/abstract_camera.h" -#include "devices/dexhand/abstract_dexhand.h" -#include "devices/gripper/abstract_gripper.h" -#include "devices/microphone/abstract_microphone.h" -#include "devices/speaker/abstract_speaker.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" -#include "manager/media_source_manager/include/device_media_source_adapter.h" -#include "manager/task_manager/include/task_manager.h" -#include "service/grpc/action/include/action_queue_executor.h" -#include "service/grpc/server/include/camera_operational_activity_registry.h" -#include "service/grpc/server/include/camera_ptz_activity_registry.h" -#include "service/grpc/server/include/grpc_recovery_audit.h" -#include "service/grpc/server/include/grpc_safety_proto.h" -#include "service/grpc/server/include/grpc_safety_participants.h" -#include "service/grpc/server/include/grpc_security.h" -#include "service/grpc/server/include/media_activity_coordinator.h" -#include "service/grpc/server/include/motor_activity_coordinator.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" -#include "service/grpc/stop_all/include/stop_operation_dispatcher.h" - -using namespace cmvr::device; -using namespace cmvr::service; - -namespace { - -class ScopedControlBarrierSet final { -public: - ~ScopedControlBarrierSet() - { - auto& authority = - cmvr::control::ControlAuthorityManager::instance(); - for (const auto& barrier : barriers_) { - if (barrier.release_on_destroy) { - authority.release(barrier.token); - } else { - (void)authority.retireSafetyHolder(barrier.token); - } - } - } - - bool acquire(const std::string& device_id, std::string& detail) - { - cmvr::control::ControlLeaseToken ignored; - return acquire(device_id, detail, ignored); - } - - bool acquire( - const std::string& device_id, - std::string& detail, - cmvr::control::ControlLeaseToken& token) - { - static std::atomic sequence{0}; - const std::string owner = - "grpc-system:stop-all:" + - std::to_string( - sequence.fetch_add(1U, std::memory_order_relaxed) + 1U); - const auto result = - cmvr::control::ControlAuthorityManager::instance() - .preemptAcquire( - device_id, - owner, - std::chrono::duration_cast< - cmvr::control::ControlAuthorityManager::Duration>( - std::chrono::hours(24))); - if (!result.acquired) { - detail = result.detail; - return false; - } - // StopAll barriers default to fail-closed. If retaining the token in - // this local vector throws, retire the manager-side holder so a later - // successful StopAll can recover it without reopening control now. - try { - barriers_.push_back({result.token, false}); - } catch (...) { - (void)cmvr::control::ControlAuthorityManager::instance() - .retireSafetyHolder(result.token); - throw; - } - token = result.token; - return true; - } - - bool waitForPreemptedRelease( - const std::string& device_id, - const std::chrono::milliseconds timeout) - { - const auto* barrier = find_(device_id); - return barrier && - cmvr::control::ControlAuthorityManager::instance() - .waitForPreemptedRelease(barrier->token, timeout); - } - - bool recoverRetiredSafetyHolders(const std::string& device_id) - { - const auto* barrier = find_(device_id); - return barrier && - cmvr::control::ControlAuthorityManager::instance() - .recoverRetiredSafetyHolders(barrier->token); - } - - void quarantine(const std::string& device_id) - { - auto& authority = - cmvr::control::ControlAuthorityManager::instance(); - for (auto& barrier : barriers_) { - if (barrier.token.resource_id == device_id) { - (void)authority.retireSafetyHolder(barrier.token); - barrier.release_on_destroy = false; - } - } - } - - void quarantineAll() - { - auto& authority = - cmvr::control::ControlAuthorityManager::instance(); - for (auto& barrier : barriers_) { - (void)authority.retireSafetyHolder(barrier.token); - barrier.release_on_destroy = false; - } - } - - void confirmSafeToReleaseAll() - { - for (auto& barrier : barriers_) { - barrier.release_on_destroy = true; - } - } - - bool empty() const noexcept { return barriers_.empty(); } - - void recoverRetiredSafetyHoldersAll() - { - for (const auto& barrier : barriers_) { - if (!recoverRetiredSafetyHolders( - barrier.token.resource_id)) { - throw std::runtime_error( - "StopAll could not recover an earlier failed safety " - "barrier: " + barrier.token.resource_id); - } - } - } - - void releaseAll() noexcept - { - auto& authority = - cmvr::control::ControlAuthorityManager::instance(); - for (const auto& barrier : barriers_) { - authority.release(barrier.token); - } - barriers_.clear(); - } - -private: - struct Barrier { - cmvr::control::ControlLeaseToken token; - bool release_on_destroy{false}; - }; - - const Barrier* find_(const std::string& device_id) const noexcept - { - for (const auto& barrier : barriers_) { - if (barrier.token.resource_id == device_id) { - return &barrier; - } - } - return nullptr; - } - - std::vector barriers_; -}; - -class ScopedMotorStopAll final { -public: - ScopedMotorStopAll() - : coordinator_(globalMotorActivityCoordinator()), - ticket_(coordinator_.beginStopAll(true)) - { - } - - ~ScopedMotorStopAll() - { - if (ticket_.valid() && !completed_) { - (void)coordinator_.finishStopAll(ticket_, false); - } - } - - bool valid() const noexcept { return ticket_.valid(); } - - bool stopAndWait( - const std::chrono::milliseconds timeout, - std::string* error) - { - return ticket_.valid() && - coordinator_.stopAndWait(ticket_, timeout, error); - } - - bool requestStop(std::string* error) - { - return ticket_.valid() && coordinator_.requestStop(ticket_, error); - } - - bool collectStopOperations( - std::vector& operations, - std::string* error) - { - return ticket_.valid() && - coordinator_.collectStopOperations(ticket_, operations, error); - } - - bool waitForStopped( - const std::chrono::milliseconds timeout, - std::string* error) - { - return ticket_.valid() && - coordinator_.waitForStopped(ticket_, timeout, error); - } - - bool complete(const bool all_motors_stopped) - { - if (!ticket_.valid() || completed_) { - return false; - } - const bool completed = coordinator_.finishStopAll( - ticket_, all_motors_stopped); - completed_ = completed; - return completed; - } - -private: - MotorActivityCoordinator& coordinator_; - MotorActivityCoordinator::StopAllTicket ticket_; - bool completed_{false}; -}; - -class ScopedActionQueueStopAll final { -public: - explicit ScopedActionQueueStopAll( - cmvr::service::ActionQueueExecutor& executor) - : executor_(executor), ticket_(executor_.beginStopAll(true)) - { - } - - ~ScopedActionQueueStopAll() - { - if (ticket_.valid() && !completed_) { - (void)executor_.finishStopAll(ticket_, false); - } - } - - bool valid() const noexcept { return ticket_.valid(); } - - bool complete(const bool all_devices_stop_confirmed) - { - if (!ticket_.valid() || completed_) { - return false; - } - const bool completed = executor_.finishStopAll( - ticket_, all_devices_stop_confirmed); - completed_ = completed; - return completed; - } - -private: - cmvr::service::ActionQueueExecutor& executor_; - cmvr::service::ActionQueueExecutor::StopAllTicket ticket_; - bool completed_{false}; -}; - -class ScopedMediaStopAll final { -public: - ScopedMediaStopAll() - : coordinator_(cmvr::service::globalMediaActivityCoordinator()), - ticket_(coordinator_.beginStopAll(true)) - { - } - - ~ScopedMediaStopAll() - { - if (ticket_.valid() && !completed_) { - (void)coordinator_.finishStopAll(ticket_, false); - } - } - - bool valid() const noexcept { return ticket_.valid(); } - - bool waitForStopped(const std::chrono::milliseconds timeout) - { - return ticket_.valid() && - coordinator_.waitForStopped(ticket_, timeout); - } - - bool requestCancellation() - { - return ticket_.valid() && coordinator_.requestCancellation(ticket_); - } - - bool collectCancellationOperations( - std::vector& operations, - std::string* error) - { - return ticket_.valid() && - coordinator_.collectCancellationOperations( - ticket_, operations, error); - } - - bool complete(const bool all_media_stopped) - { - if (!ticket_.valid() || completed_) { - return false; - } - const bool completed = - coordinator_.finishStopAll(ticket_, all_media_stopped); - completed_ = completed; - return completed; - } - -private: - cmvr::service::MediaActivityCoordinator& coordinator_; - cmvr::service::MediaActivityCoordinator::StopAllTicket ticket_; - bool completed_{false}; -}; - -class ScopedAdmissionStopAll final { -public: - ScopedAdmissionStopAll() - : gate_(globalStopAllAdmissionGate()), - ticket_(gate_.beginStopAll()) - { - } - - ~ScopedAdmissionStopAll() - { - if (ticket_.valid() && !completed_) { - (void)gate_.finishStopAll(ticket_, false); - } - } - - bool valid() const noexcept { return ticket_.valid(); } - - bool complete(const bool all_domains_stop_confirmed) - { - if (!ticket_.valid() || completed_) { - return false; - } - const auto result = gate_.finishStopAllDetailed( - ticket_, all_domains_stop_confirmed); - completed_ = result.ticket_consumed; - return result.admission_reopened; - } - -private: - StopAllAdmissionGate& gate_; - StopAllAdmissionGate::StopAllTicket ticket_; - bool completed_{false}; -}; - -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()) - : 0U; -} - -std::chrono::milliseconds remainingStopBudget( - const std::chrono::steady_clock::time_point deadline) noexcept -{ - const auto now = std::chrono::steady_clock::now(); - if (now >= deadline) { - return std::chrono::milliseconds::zero(); - } - return std::chrono::duration_cast( - deadline - now); -} - -std::timed_mutex& processStopAllMutex() -{ - // SystemService is expected to be unique, but keeping the mutex at process - // scope also protects test, reload, and accidental multi-instance paths - // from issuing overlapping whole-device stop rounds. - static std::timed_mutex mutex; - return mutex; -} - -using StopDispatcher = cmvr::service::StopOperationDispatcher; -using StopHandle = StopDispatcher::Handle; -using StopOutcome = StopDispatcher::OperationResult; - -struct ProcessStopDispatcherRegistry final { - std::mutex mutex; - std::condition_variable available; - std::weak_ptr dispatcher; - bool destroying{false}; -}; - -ProcessStopDispatcherRegistry& processStopDispatcherRegistry() -{ - static ProcessStopDispatcherRegistry registry; - return registry; -} - -void acquireProcessStopDispatcher( - std::shared_ptr& owner) -{ - auto& registry = processStopDispatcherRegistry(); - std::unique_lock lock(registry.mutex); - registry.available.wait( - lock, [®istry] { return !registry.destroying; }); - if (auto existing = registry.dispatcher.lock()) { - owner = std::move(existing); - return; - } - - owner = std::make_shared(); - registry.dispatcher = owner; -} - -void releaseProcessStopDispatcher( - std::shared_ptr& owner) noexcept -{ - if (!owner) { - return; - } - - auto& registry = processStopDispatcherRegistry(); - std::unique_lock lock(registry.mutex); - if (owner.use_count() > 1) { - owner.reset(); - return; - } - - // Do not admit a replacement while the last dispatcher is joining a - // worker which may still be inside a device driver. - registry.destroying = true; - registry.dispatcher.reset(); - registry.available.notify_all(); - auto last_owner = std::move(owner); - lock.unlock(); - last_owner.reset(); - lock.lock(); - registry.destroying = false; - lock.unlock(); - registry.available.notify_all(); -} - -class StopDeadlineDetail final { -public: - void publishInitial(std::string detail) - { - std::lock_guard lock(mutex_); - detail_ = std::move(detail); - } - - void publishUntil( - const StopDispatcher::Deadline deadline, - std::string detail) - { - std::lock_guard lock(mutex_); - if (StopDispatcher::Clock::now() < deadline) { - detail_ = std::move(detail); - } - } - - std::string snapshot() const - { - std::lock_guard lock(mutex_); - return detail_; - } - -private: - mutable std::mutex mutex_; - std::string detail_; -}; - -struct StopHandleEntry final { - std::string device_id; - StopHandle handle; - bool control_resource{false}; - bool require_success{true}; -}; - -std::string stopResourceKey(const std::string& device_id) -{ - return "device:" + device_id; -} - -void appendFailure( - std::vector& failures, - const std::string& subject, - const std::string& detail) -{ - failures.push_back( - subject + ": " + - (detail.empty() ? "operational stop was not confirmed" : detail)); -} - -bool waitForStopOperations( - const std::vector& operations, - const StopDispatcher::Deadline deadline, - ScopedControlBarrierSet& control_barriers, - std::vector& failures, - std::unordered_set* completed_devices = nullptr) -{ - bool all_succeeded = true; - for (const auto& operation : operations) { - const auto outcome = operation.handle.waitUntil(deadline); - if (outcome.completed && completed_devices) { - completed_devices->emplace(operation.device_id); - } - if (outcome.completed && outcome.result) { - continue; - } - if (outcome.completed && !operation.require_success) { - CMVR_LOG(WARNING) - << "[gRPCSystemServiceImpl] (StopAll): initial stop was not " - "confirmed and will be retried, id=" - << operation.device_id << ", detail=" << outcome.detail; - continue; - } - - all_succeeded = false; - if (operation.control_resource) { - control_barriers.quarantine(operation.device_id); - } - appendFailure( - failures, - operation.device_id, - outcome.detail.empty() - ? "stop operation did not complete before the deadline" - : outcome.detail); - } - return all_succeeded; -} - -StopOutcome stopArm( - const std::shared_ptr& arm, - const bool final_confirmation) -{ - const auto result = arm->stopMotion(); - if (!result.ok()) { - return { - false, - std::string(final_confirmation ? "final " : "initial ") + - "RobotArm stop was not confirmed" + - (result.message.empty() ? "" : ": " + result.message)}; - } - if (final_confirmation) { - try { - if (arm->busy()) { - return {false, "RobotArm remained busy after final stop"}; - } - } catch (const std::exception& error) { - return { - false, - std::string("RobotArm idle confirmation threw: ") + - error.what()}; - } catch (...) { - return { - false, - "RobotArm idle confirmation threw an unknown exception"}; - } - } - return {true, {}}; -} - -bool agvStopResultAccepted(const cmvr::device::AgvResult& result) noexcept -{ - return result.ok() || - result.code == cmvr::device::AgvErrorCode::UnsupportedCommand; -} - -StopOutcome stopAgv( - const std::shared_ptr& agv, - const bool final_confirmation) -{ - const auto cancel = agv->cancelNavigation(); - const auto velocity = agv->stopVelocityControl(); - const auto mapping = agv->stopMapping(); - const auto stopped = agv->confirmMotionStopped(); - - if (final_confirmation && - (!agvStopResultAccepted(cancel) || - !agvStopResultAccepted(velocity) || - !agvStopResultAccepted(mapping) || !stopped.ok())) { - return {false, "final AGV operational stop was not confirmed"}; - } - if (!final_confirmation && !stopped.ok()) { - return { - false, - "initial AGV stopped state was not confirmed" + - (stopped.message.empty() ? "" : ": " + stopped.message)}; - } - return {true, {}}; -} - -StopOutcome stopDexHand( - const std::shared_ptr& hand, - const bool final_confirmation) -{ - if (!hand->stopOperationalActivity()) { - return { - false, - std::string(final_confirmation ? "final " : "initial ") + - "DexHand operational stop was not confirmed"}; - } - return {true, {}}; -} - -template -StopOutcome invokeStopOperation( - Operation&& operation, - const std::string& description) noexcept -{ - try { - return std::forward(operation)(); - } catch (const std::exception& error) { - return { - false, - description + " threw: " + error.what()}; - } catch (...) { - return { - false, - description + " threw an unknown exception"}; - } -} - -template -StopOutcome stopControlWithFence( - const cmvr::control::ControlLeaseToken& barrier, - const StopDispatcher::Deadline deadline, - InitialStop&& initial_stop, - FinalStop&& final_stop, - const std::string& description, - const std::shared_ptr& deadline_detail) -{ - deadline_detail->publishUntil( - deadline, - "initial " + description + - " stop did not complete before the StopAll deadline"); - const auto initial = invokeStopOperation( - std::forward(initial_stop), - "initial " + description + " stop"); - if (!initial.success) { - CMVR_LOG(WARNING) - << "[gRPCSystemServiceImpl] (StopAll): " << initial.detail; - } - - if (!barrier.valid()) { - return { - false, - description + - " safety barrier was unavailable after the initial stop"}; - } - - const std::string handler_timeout_detail = - "timed out waiting for the preempted " + description + - " control handler to exit"; - deadline_detail->publishUntil(deadline, handler_timeout_detail); - const bool handler_drained = - cmvr::control::ControlAuthorityManager::instance() - .waitForPreemptedRelease( - barrier, remainingStopBudget(deadline)); - if (handler_drained) { - deadline_detail->publishUntil( - deadline, - "final " + description + - " stop did not complete before the StopAll deadline"); - } - // This final typed stop remains mandatory even when the handler wait used - // the entire RPC budget. The dispatcher owns this worker past the RPC - // deadline, closing the race where an old handler resumes after the first - // stop while the safety barrier remains fail-closed. - const auto final = invokeStopOperation( - std::forward(final_stop), - "final " + description + " stop"); - if (!handler_drained) { - std::string detail = handler_timeout_detail; - if (!final.success) { - detail += "; " + - (final.detail.empty() - ? "final " + description + - " stop was not confirmed" - : final.detail); - } - return {false, std::move(detail)}; - } - return final; -} - -StopOutcome stopCameraActivities( - const std::string& device_id, - const std::shared_ptr& camera) -{ - std::vector failures; - bool stopped = cmvr::media::globalMediaSourceManager() - .stopSourcesForDevice(device_id, &failures); - if (!globalCameraPtzActivityRegistry().stopActivitiesForDevice( - device_id, &failures)) { - stopped = false; - } - - try { - cmvr::device::CameraState state{}; - if (camera) { - camera->getState(state); - } - if (camera && state.is_recording) { - camera->stopRecording(); - camera->getState(state); - if (state.is_recording) { - stopped = false; - failures.push_back( - device_id + ": camera recording did not stop"); - } - } - } catch (const std::exception& error) { - stopped = false; - failures.push_back( - device_id + ": camera recording stop threw: " + error.what()); - } catch (...) { - stopped = false; - failures.push_back( - device_id + - ": camera recording stop threw an unknown exception"); - } - - if (!globalCameraOperationalActivityRegistry().stopActivitiesForDevice( - device_id, camera, &failures)) { - stopped = false; - } - return { - stopped, - failures.empty() - ? std::string{} - : "camera activities were not fully stopped: " + - failures.front()}; -} - -StopOutcome stopMicrophone( - const std::string& device_id, - const std::shared_ptr& microphone) -{ - std::vector failures; - bool stopped = cmvr::media::globalMediaSourceManager() - .stopSourcesForDevice(device_id, &failures); - try { - cmvr::device::MicrophoneState state{}; - microphone->getState(state); - if (state.is_recording) { - microphone->stopRecording(); - microphone->getState(state); - if (state.is_recording) { - stopped = false; - failures.push_back( - device_id + ": microphone recording did not stop"); - } - } - } catch (const std::exception& error) { - stopped = false; - failures.push_back( - device_id + ": microphone recording stop threw: " + - error.what()); - } catch (...) { - stopped = false; - failures.push_back( - device_id + - ": microphone recording stop threw an unknown exception"); - } - return { - stopped, - failures.empty() - ? std::string{} - : "microphone activities were not fully stopped: " + - failures.front()}; -} - -StopOutcome stopSpeaker( - const std::shared_ptr& speaker) -{ - return speaker->stopPlayback() - ? StopOutcome{true, {}} - : StopOutcome{false, "speaker playback did not stop"}; -} - -StopOutcome stopTrackedMediaActivities(const std::string& device_id) -{ - std::vector failures; - bool stopped = cmvr::media::globalMediaSourceManager() - .stopSourcesForDevice(device_id, &failures); - if (!globalCameraPtzActivityRegistry().stopActivitiesForDevice( - device_id, &failures)) { - stopped = false; - } - if (!globalCameraOperationalActivityRegistry().stopActivitiesForDevice( - device_id, &failures)) { - stopped = false; - } - return { - stopped, - failures.empty() - ? std::string{} - : "tracked media activities were not fully stopped: " + - failures.front()}; -} - -void mergeStopOutcome( - const StopOutcome& outcome, - bool& all_stopped, - std::vector& failures) -{ - if (outcome.success) { - return; - } - all_stopped = false; - failures.push_back( - outcome.detail.empty() - ? "operational stop was not confirmed" - : outcome.detail); -} - -template -StopOutcome stopOperationalDevice( - const std::shared_ptr& device, - const char* description); - -StopOutcome stopOtherActivities( - const std::string& device_id, - const std::shared_ptr& camera, - const std::shared_ptr& microphone, - const std::shared_ptr& speaker, - const std::shared_ptr& head, - const std::shared_ptr& gripper, - const bool tracked_media) -{ - bool all_stopped = true; - bool media_stopped_by_typed_device = false; - std::vector failures; - - if (camera) { - mergeStopOutcome( - invokeStopOperation( - [&] { return stopCameraActivities(device_id, camera); }, - "camera activity stop"), - all_stopped, failures); - media_stopped_by_typed_device = true; - } - if (microphone) { - mergeStopOutcome( - invokeStopOperation( - [&] { return stopMicrophone(device_id, microphone); }, - "microphone activity stop"), - all_stopped, failures); - media_stopped_by_typed_device = true; - } - if (tracked_media && !media_stopped_by_typed_device) { - mergeStopOutcome( - invokeStopOperation( - [&] { return stopTrackedMediaActivities(device_id); }, - "tracked media activity stop"), - all_stopped, failures); - } - if (speaker) { - mergeStopOutcome( - invokeStopOperation( - [&] { return stopSpeaker(speaker); }, - "speaker activity stop"), - all_stopped, failures); - } - if (head) { - mergeStopOutcome( - invokeStopOperation( - [&] { return stopOperationalDevice(head, "BioHead"); }, - "BioHead activity stop"), - all_stopped, failures); - } - if (gripper) { - mergeStopOutcome( - invokeStopOperation( - [&] { return stopOperationalDevice(gripper, "gripper"); }, - "gripper activity stop"), - all_stopped, failures); - } - - return { - all_stopped, - failures.empty() ? std::string{} : failures.front()}; -} - -StopOutcome stopTaskActivity( - const std::shared_ptr& task) -{ - if (!task) { - return {false, "task activity target was null"}; - } - return task->stopActivity() - ? StopOutcome{true, {}} - : StopOutcome{ - false, - "task " + task->id() + - " operational stop was not confirmed"}; -} - -void submitDeferredOperations( - StopDispatcher& dispatcher, - std::vector operations, - std::vector& handles) -{ - handles.reserve(handles.size() + operations.size()); - for (auto& operation : operations) { - const auto subject = operation.resource_key; - auto callback = std::move(operation.operation); - auto handle = dispatcher.submit( - operation.resource_key, - [callback = std::move(callback)]() mutable { - const auto result = callback(); - return StopOutcome{result.success, result.detail}; - }); - handles.push_back( - {subject, std::move(handle), false, true}); - } -} - -template -StopOutcome stopOperationalDevice( - const std::shared_ptr& device, - const char* description) -{ - return device->stopOperationalActivity() - ? StopOutcome{true, {}} - : StopOutcome{ - false, - std::string(description) + - " operational stop was not confirmed"}; -} - -cmvr::api::SystemDeviceType toApiDeviceType( - const cmvr::device::DeviceKind kind) noexcept -{ - switch (kind) { - case cmvr::device::DeviceKind::AGV: - return cmvr::api::SYSTEM_DEVICE_TYPE_AGV; - case cmvr::device::DeviceKind::Arm: - return cmvr::api::SYSTEM_DEVICE_TYPE_ARM; - case cmvr::device::DeviceKind::Battery: - return cmvr::api::SYSTEM_DEVICE_TYPE_BATTERY; - case cmvr::device::DeviceKind::BioHead: - return cmvr::api::SYSTEM_DEVICE_TYPE_BIO_HEAD; - case cmvr::device::DeviceKind::Camera: - return cmvr::api::SYSTEM_DEVICE_TYPE_CAMERA; - case cmvr::device::DeviceKind::CanBus: - return cmvr::api::SYSTEM_DEVICE_TYPE_CAN_BUS; - case cmvr::device::DeviceKind::DexHand: - return cmvr::api::SYSTEM_DEVICE_TYPE_DEX_HAND; - case cmvr::device::DeviceKind::Gripper: - return cmvr::api::SYSTEM_DEVICE_TYPE_GRIPPER; - case cmvr::device::DeviceKind::Microphone: - return cmvr::api::SYSTEM_DEVICE_TYPE_MICROPHONE; - case cmvr::device::DeviceKind::Motor: - return cmvr::api::SYSTEM_DEVICE_TYPE_MOTOR; - case cmvr::device::DeviceKind::MotorSystem: - return cmvr::api::SYSTEM_DEVICE_TYPE_MOTOR_SYSTEM; - case cmvr::device::DeviceKind::MujocoViewer: - return cmvr::api::SYSTEM_DEVICE_TYPE_MUJOCO_VIEWER; - case cmvr::device::DeviceKind::MujocoWorld: - return cmvr::api::SYSTEM_DEVICE_TYPE_MUJOCO_WORLD; - case cmvr::device::DeviceKind::Robot: - return cmvr::api::SYSTEM_DEVICE_TYPE_ROBOT; - case cmvr::device::DeviceKind::Speaker: - return cmvr::api::SYSTEM_DEVICE_TYPE_SPEAKER; - case cmvr::device::DeviceKind::Unknown: - break; - } - return cmvr::api::SYSTEM_DEVICE_TYPE_UNSPECIFIED; -} - -cmvr::api::SystemDeviceState toApiDeviceState( - const cmvr::device::ManagedDeviceState state) noexcept -{ - switch (state) { - case cmvr::device::ManagedDeviceState::Disabled: - return cmvr::api::SYSTEM_DEVICE_STATE_DISABLED; - case cmvr::device::ManagedDeviceState::Initializing: - return cmvr::api::SYSTEM_DEVICE_STATE_INITIALIZING; - case cmvr::device::ManagedDeviceState::Registered: - return cmvr::api::SYSTEM_DEVICE_STATE_REGISTERED; - case cmvr::device::ManagedDeviceState::Ready: - return cmvr::api::SYSTEM_DEVICE_STATE_READY; - case cmvr::device::ManagedDeviceState::Running: - return cmvr::api::SYSTEM_DEVICE_STATE_RUNNING; - case cmvr::device::ManagedDeviceState::Stopped: - return cmvr::api::SYSTEM_DEVICE_STATE_STOPPED; - case cmvr::device::ManagedDeviceState::Error: - return cmvr::api::SYSTEM_DEVICE_STATE_ERROR; - case cmvr::device::ManagedDeviceState::Unknown: - break; - } - return cmvr::api::SYSTEM_DEVICE_STATE_UNSPECIFIED; -} - -cmvr::api::SystemDeviceHealth toApiDeviceHealth( - const cmvr::device::DeviceHealthState state) noexcept -{ - switch (state) { - case cmvr::device::DeviceHealthState::Healthy: - return cmvr::api::SYSTEM_DEVICE_HEALTH_HEALTHY; - case cmvr::device::DeviceHealthState::Degraded: - return cmvr::api::SYSTEM_DEVICE_HEALTH_DEGRADED; - case cmvr::device::DeviceHealthState::Fault: - return cmvr::api::SYSTEM_DEVICE_HEALTH_FAULT; - case cmvr::device::DeviceHealthState::Unknown: - break; - } - return cmvr::api::SYSTEM_DEVICE_HEALTH_UNSPECIFIED; -} - -struct ParsedSafetyScope final { - bool valid{false}; - bool all_devices{false}; - std::vector device_ids; - std::string error; -}; - -ParsedSafetyScope parseSafetyScope( - const cmvr::api::SafetyScope& scope, - const bool default_to_all) -{ - ParsedSafetyScope result; - switch (scope.target_case()) { - case cmvr::api::SafetyScope::kAllDevices: - if (!scope.all_devices()) { - result.error = "all_devices must be explicitly true"; - return result; - } - result.valid = true; - result.all_devices = true; - return result; - case cmvr::api::SafetyScope::kDevices: { - if (scope.devices().device_ids().empty()) { - result.error = "device scope must contain at least one device ID"; - return result; - } - std::unordered_set unique; - result.device_ids.reserve(scope.devices().device_ids_size()); - for (const auto& id : scope.devices().device_ids()) { - if (id.empty() || !unique.insert(id).second) { - result.error = - "device scope IDs must be non-empty and unique"; - return result; - } - result.device_ids.push_back(id); - } - result.valid = true; - return result; - } - case cmvr::api::SafetyScope::TARGET_NOT_SET: - if (default_to_all) { - result.valid = true; - result.all_devices = true; - } else { - result.error = "an explicit recovery scope is required"; - } - return result; - } - result.error = "invalid safety scope"; - return result; -} - -void setSafetyHeaderFailure( - cmvr::api::CommandHeader_Feedback* header, - const cmvr::safety::SafetyReason reason, - const std::string& detail) -{ - header->set_success(false); - header->set_reason_code(toApiSafetyReason(reason)); - header->set_error_message(detail); - header->set_execution_state( - cmvr::api::COMMAND_EXECUTION_STATE_REJECTED_BEFORE_DISPATCH); - setCurrentTimestamp(header->mutable_timestamp()); -} - -bool recoveryCompletedAsRequested( - const cmvr::safety::RecoveryResultCode result) noexcept -{ - return result == cmvr::safety::RecoveryResultCode::Recovered || - result == - cmvr::safety::RecoveryResultCode::VerifiedButStillBlocked || - result == cmvr::safety::RecoveryResultCode::NothingToRecover; -} - -} // namespace - -gRPCSystemServiceImpl::gRPCSystemServiceImpl() - : gRPCSystemServiceImpl( - std::chrono::seconds(15), makeDefaultGrpcSecurityGateway()) -{ -} - -gRPCSystemServiceImpl::gRPCSystemServiceImpl( - const std::chrono::milliseconds stop_timeout) - : gRPCSystemServiceImpl(stop_timeout, makeDefaultGrpcSecurityGateway()) -{ -} - -gRPCSystemServiceImpl::gRPCSystemServiceImpl( - std::shared_ptr security_gateway) - : gRPCSystemServiceImpl( - std::chrono::seconds(15), std::move(security_gateway)) -{ -} - -gRPCSystemServiceImpl::gRPCSystemServiceImpl( - const std::chrono::milliseconds stop_timeout, - std::shared_ptr security_gateway) - : gRPCSystemServiceImpl( - stop_timeout, std::move(security_gateway), nullptr) -{ -} - -gRPCSystemServiceImpl::gRPCSystemServiceImpl( - const std::chrono::milliseconds stop_timeout, - std::shared_ptr security_gateway, - std::shared_ptr recovery_audit_sink) - : dmgr_(DeviceManager::getInstance()), - stop_timeout_( - stop_timeout > std::chrono::milliseconds::zero() - ? stop_timeout - : std::chrono::seconds(15)), - security_gateway_( - security_gateway ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()), - recovery_audit_sink_(std::move(recovery_audit_sink)), - action_queue_(std::make_shared(dmgr_)) -{ - // Acquire only after ActionQueue construction succeeds. This ensures an - // exception cannot release the last dispatcher outside the registry. - acquireProcessStopDispatcher(stop_dispatcher_); - try { - safety_participant_registration_ = registerGrpcSafetyParticipants( - dmgr_.safetyManager(), action_queue_, stop_dispatcher_); - } catch (...) { - releaseProcessStopDispatcher(stop_dispatcher_); - throw; - } -} - -gRPCSystemServiceImpl::~gRPCSystemServiceImpl() -{ - // The dispatcher intentionally outlives ActionQueue, then joins any - // deadline-overrunning stop workers before the last service disappears. - safety_participant_registration_.reset(); - action_queue_.reset(); - releaseProcessStopDispatcher(stop_dispatcher_); -} - -bool gRPCSystemServiceImpl::waitForStopDispatcherDestructionForTesting( - const std::chrono::milliseconds timeout) -{ - auto& registry = processStopDispatcherRegistry(); - std::unique_lock lock(registry.mutex); - return registry.available.wait_for( - lock, - timeout, - [®istry] { return registry.destroying; }); -} - -void gRPCSystemServiceImpl::prepareForShutdown() -{ - // Do not wait for a business StopAll round here. StopAll may be blocked in - // a device backend, while server shutdown must still invalidate queued and - // active ActionQueue work promptly. ActionQueueExecutor owns the state lock - // and generation fence needed to make this transition race-safe; an - // in-flight StopAll ticket then becomes stale and fails closed. - (void)action_queue_->disableForShutdown(); -} - -grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, - const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.SystemService/GetSystemInfo"); - try { - response->set_version(dmgr_.version()); - response->set_system_name(dmgr_.name()); - response->set_action_service_instance_id( - action_queue_->instanceId()); - const auto& security = security_gateway_->config(); - response->set_grpc_transport_security(toString(security.transport)); - response->set_grpc_authentication(toString(security.authentication)); - response->set_grpc_recovery_exposure( - toString(security.recovery_exposure)); - response->set_grpc_insecure_non_loopback( - security.insecure_non_loopback); - const auto safety = dmgr_.safetyManager().snapshot(); - response->set_control_service_instance_id( - safety.service_instance_id); - response->set_safety_enforcement_mode( - cmvr::safety::toString(safety.enforcement_mode)); - response->set_safety_schema_version(1); - 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) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.SystemService/GetSystemStatus"); - 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::GetDeviceList( - grpc::ServerContext* context, - const api::GetDeviceListCommand_Request* request, - api::GetDeviceListCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.SystemService/GetDeviceList"); - (void)request; - try { - const auto snapshot = dmgr_.snapshot(); - response->set_manager_name(snapshot.name); - response->set_manager_version(snapshot.version); - response->set_manager_description(snapshot.description); - response->set_sampled_at_unix_ms(unixTimeMs()); - - for (const auto& source : snapshot.devices) { - // The public inventory contains enabled entries only. Keep enabled - // devices visible even when their lifecycle or health is in error. - if (!source.enabled) { - continue; - } - - auto* destination = response->add_device_list(); - destination->set_device_id(source.id); - destination->set_device_type(toApiDeviceType(source.kind)); - destination->set_type_name(source.type_name); - destination->set_enabled(true); - destination->set_manager_state(toApiDeviceState(source.state)); - destination->set_health(toApiDeviceHealth(source.health.state)); - destination->set_has_error(source.abnormal); - destination->set_error_message(source.error_message); - destination->set_status_updated_at_unix_ms( - source.status_updated_at_unix_ms); - } - - response->mutable_header()->set_success(true); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetDeviceList): success, devices=" - << response->device_list_size(); - return grpc::Status::OK; - } - catch (const std::exception& e) { - response->Clear(); - 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::GetSafetyState( - grpc::ServerContext* context, - const cmvr::api::GetSafetyStateCommand_Request* request, - cmvr::api::GetSafetyStateCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.SystemService/GetSafetyState"); - if (!request || !response) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "GetSafetyState request and response are required"); - } - - const auto scope = parseSafetyScope(request->scope(), true); - if (!scope.valid) { - setSafetyHeaderFailure( - response->mutable_header(), - cmvr::safety::SafetyReason::InvalidArgument, - scope.error); - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, scope.error); - } - - const auto snapshot = dmgr_.safetyManager().snapshot(); - response->set_system_state( - toApiSystemAdmissionState(snapshot.system_state)); - response->set_safety_epoch(snapshot.safety_epoch); - response->set_control_service_instance_id(snapshot.service_instance_id); - response->set_enforcement_mode( - cmvr::safety::toString(snapshot.enforcement_mode)); - response->set_active_operation_id(snapshot.active_operation_id); - response->set_active_operation_phase(snapshot.active_operation_phase); - response->set_sampled_at_unix_ms(unixTimeMs()); - - std::unordered_set requested_ids( - scope.device_ids.begin(), scope.device_ids.end()); - for (const auto& device : snapshot.devices) { - if (!scope.all_devices && - requested_ids.erase(device.descriptor.device_id) == 0U) { - continue; - } - populateDeviceSafetyState(device, *response->add_devices()); - } - for (const auto& participant : snapshot.participants) { - populateSafetyParticipantState( - participant, *response->add_participants()); - } - if (!requested_ids.empty()) { - const auto detail = - "safety device is not registered: " + *requested_ids.begin(); - response->Clear(); - setSafetyHeaderFailure( - response->mutable_header(), - cmvr::safety::SafetyReason::DeviceNotFound, - detail); - return grpc::Status(grpc::StatusCode::NOT_FOUND, detail); - } - - response->mutable_header()->set_success(true); - response->mutable_header()->set_reason_code( - cmvr::api::COMMAND_REASON_CODE_NONE); - response->mutable_header()->set_service_instance_id( - snapshot.service_instance_id); - response->mutable_header()->set_safety_epoch(snapshot.safety_epoch); - response->mutable_header()->set_execution_state( - cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; -} - -grpc::Status gRPCSystemServiceImpl::RecoverSafetyState( - grpc::ServerContext* context, - const cmvr::api::RecoverSafetyStateCommand_Request* request, - cmvr::api::RecoverSafetyStateCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.SystemService/RecoverSafetyState"); - if (!request || !response) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "RecoverSafetyState request and response are required"); - } - - const auto scope = parseSafetyScope(request->scope(), false); - if (!scope.valid || request->recovery_id().empty() || - request->reason().empty() || request->expected_safety_epoch() == 0 || - request->mode() == - cmvr::api::RecoverSafetyStateCommand::MODE_UNSPECIFIED) { - std::string detail = scope.valid - ? "recovery_id, reason, expected_safety_epoch, and mode are required" - : scope.error; - setSafetyHeaderFailure( - response->mutable_header(), - request->reason().empty() - ? cmvr::safety::SafetyReason::RecoveryReasonRequired - : cmvr::safety::SafetyReason::InvalidArgument, - detail); - return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, detail); - } - if (!recovery_audit_sink_) { - const std::string detail = - "persistent recovery audit is not configured"; - setSafetyHeaderFailure( - response->mutable_header(), - cmvr::safety::SafetyReason::RecoveryAuditFailed, - detail); - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "RECOVERY_AUDIT_FAILED"); - } - - const bool verify_only = request->mode() == - cmvr::api::RecoverSafetyStateCommand::VERIFY_ONLY; - RecoveryAuditRecord audit; - audit.occurred_at_unix_ms = unixTimeMs(); - audit.stage = "accepted"; - audit.correlation_id = cmvr_grpc_call_guard.context().correlation_id; - audit.principal_id = cmvr_grpc_call_guard.context().principal.id; - audit.peer = cmvr_grpc_call_guard.context().peer; - audit.recovery_id = request->recovery_id(); - audit.reason = request->reason(); - audit.mode = verify_only ? "verify_only" : "clear_software_latch"; - audit.all_devices = scope.all_devices; - audit.device_ids = scope.device_ids; - audit.expected_safety_epoch = request->expected_safety_epoch(); - audit.result = "pending"; - std::string audit_error; - if (!recovery_audit_sink_->append(audit, &audit_error)) { - const auto detail = audit_error.empty() - ? std::string("persistent recovery audit write failed") - : audit_error; - setSafetyHeaderFailure( - response->mutable_header(), - cmvr::safety::SafetyReason::RecoveryAuditFailed, - detail); - return grpc::Status( - grpc::StatusCode::FAILED_PRECONDITION, - "RECOVERY_AUDIT_FAILED"); - } - - auto deadline = cmvr_grpc_call_guard.context().deadline; - const auto configured_deadline = - cmvr::safety::SafetyClock::now() + - dmgr_.safetyManager().config().recovery_timeout; - if (deadline == cmvr::safety::SafetyClock::time_point::max() || - configured_deadline < deadline) { - deadline = configured_deadline; - } - if (request->timeout_ms() != 0) { - deadline = std::min( - deadline, - cmvr::safety::SafetyClock::now() + - std::chrono::milliseconds(request->timeout_ms())); - } - - cmvr::safety::RecoveryRequest coordinator_request; - coordinator_request.recovery_id = request->recovery_id(); - coordinator_request.device_ids = scope.device_ids; - coordinator_request.all_devices = scope.all_devices; - coordinator_request.expected_safety_epoch = - request->expected_safety_epoch(); - coordinator_request.verify_only = verify_only; - coordinator_request.reason = request->reason(); - coordinator_request.deadline = deadline; - if (!verify_only) { - auto commit_audit = audit; - commit_audit.stage = "clear_commit"; - commit_audit.result = "authorized"; - const auto sink = recovery_audit_sink_; - coordinator_request.authorize_clear = - [sink, commit_audit = std::move(commit_audit)]() mutable { - commit_audit.occurred_at_unix_ms = unixTimeMs(); - std::string error; - const bool persisted = sink->append(commit_audit, &error); - if (!persisted) { - CMVR_LOG(ERROR) - << "[gRPCSystemServiceImpl] Recovery clear audit " - "failed: " - << error; - } - return persisted; - }; - } - - const auto result = - dmgr_.safetyManager().recover(coordinator_request); - response->set_recovery_id(result.recovery_id); - response->set_result(toApiRecoveryResult(result.result)); - response->set_previous_safety_epoch(result.previous_safety_epoch); - response->set_current_safety_epoch(result.current_safety_epoch); - response->set_system_state( - toApiSystemAdmissionState(result.system_state)); - for (const auto& target : result.targets) { - populateSafetyTargetResult(target, *response->add_targets()); - } - - const bool success = recoveryCompletedAsRequested(result.result); - auto* header = response->mutable_header(); - header->set_success(success); - header->set_command_id(request->recovery_id()); - header->set_service_instance_id( - dmgr_.safetyManager().serviceInstanceId()); - header->set_safety_epoch(result.current_safety_epoch); - header->set_execution_state( - success ? cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED - : cmvr::api::COMMAND_EXECUTION_STATE_FAILED); - if (success) { - header->set_reason_code(cmvr::api::COMMAND_REASON_CODE_NONE); - } else if (!result.targets.empty()) { - header->set_reason_code( - toApiSafetyReason(result.targets.front().reason)); - header->set_error_message(result.targets.front().detail); - } else { - header->set_reason_code( - cmvr::api::COMMAND_REASON_CODE_INTERNAL_ERROR); - header->set_error_message("recovery failed without a target result"); - } - setCurrentTimestamp(header->mutable_timestamp()); - - audit.occurred_at_unix_ms = unixTimeMs(); - audit.stage = "completed"; - audit.previous_safety_epoch = result.previous_safety_epoch; - audit.current_safety_epoch = result.current_safety_epoch; - audit.result = cmvr::safety::toString(result.result); - if (!recovery_audit_sink_->append(audit, &audit_error)) { - CMVR_LOG(ERROR) - << "[gRPCSystemServiceImpl] Recovery completion audit failed: " - << audit_error; - } - return grpc::Status::OK; -} - -grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.SystemService/UpdateParams"); - 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::StopAll(grpc::ServerContext* context, - const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.SystemService/StopAll"); - if (!request || !response) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "StopAll request and response are required"); - } - if (dmgr_.safetyManager().config().enforcement_mode != - cmvr::safety::EnforcementMode::Legacy) { - auto deadline = cmvr_grpc_call_guard.context().deadline; - const auto configured_deadline = - cmvr::safety::SafetyClock::now() + stop_timeout_; - if (deadline == cmvr::safety::SafetyClock::time_point::max() || - configured_deadline < deadline) { - deadline = configured_deadline; - } - if (request->timeout_ms() != 0) { - deadline = std::min( - deadline, - cmvr::safety::SafetyClock::now() + - std::chrono::milliseconds(request->timeout_ms())); - } - - std::string operation_id = request->operation_id(); - if (operation_id.empty() && request->has_header()) { - operation_id = request->header().command_id(); - } - const auto result = dmgr_.safetyManager().stopAll( - std::move(operation_id), deadline); - response->set_operation_id(result.operation_id); - response->set_previous_safety_epoch( - result.previous_safety_epoch); - response->set_current_safety_epoch( - result.current_safety_epoch); - response->set_system_state( - toApiSystemAdmissionState(result.system_state)); - for (const auto& target : result.targets) { - populateSafetyTargetResult(target, *response->add_targets()); - } - - auto* header = response->mutable_header(); - header->set_success(result.success); - header->set_command_id(result.operation_id); - header->set_service_instance_id( - dmgr_.safetyManager().serviceInstanceId()); - header->set_safety_epoch(result.current_safety_epoch); - header->set_execution_state( - result.success - ? cmvr::api::COMMAND_EXECUTION_STATE_COMPLETED - : cmvr::api::COMMAND_EXECUTION_STATE_FAILED); - if (result.success) { - header->set_reason_code(cmvr::api::COMMAND_REASON_CODE_NONE); - } else if (!result.targets.empty()) { - const auto failed = std::find_if( - result.targets.begin(), result.targets.end(), - [](const auto& target) { return !target.success; }); - if (failed != result.targets.end()) { - header->set_reason_code(toApiSafetyReason(failed->reason)); - header->set_error_message(failed->detail); - } - } else { - header->set_reason_code( - cmvr::api::COMMAND_REASON_CODE_STOP_UNCONFIRMED); - header->set_error_message( - "StopAll did not produce a participant result"); - } - setCurrentTimestamp(header->mutable_timestamp()); - return grpc::Status::OK; - } - (void)request; - const auto stop_deadline = - std::chrono::steady_clock::now() + stop_timeout_; - - std::unique_lock stop_all_lock( - processStopAllMutex(), std::defer_lock); - while (!stop_all_lock.try_lock_for(std::min( - std::chrono::milliseconds(50), - remainingStopBudget(stop_deadline)))) { - if (context && context->IsCancelled()) { - return grpc::Status( - grpc::StatusCode::CANCELLED, - "StopAll was cancelled while waiting for another StopAll round"); - } - if (remainingStopBudget(stop_deadline) == - std::chrono::milliseconds::zero()) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message( - "StopAll timed out waiting for another StopAll round"); - setCurrentTimestamp( - response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - } - - try { - // Close every admission domain before resolving devices or issuing - // stops. The process remains alive; only new operational work pauses. - ScopedAdmissionStopAll admission_stop; - ScopedMediaStopAll media_stop; - ScopedMotorStopAll motor_stop; - ScopedActionQueueStopAll action_stop(*action_queue_); - if (!admission_stop.valid()) { - throw std::runtime_error( - "StopAll could not establish the system admission barrier"); - } - if (!media_stop.valid()) { - throw std::runtime_error( - "StopAll could not establish the media activity barrier"); - } - if (!action_stop.valid()) { - throw std::runtime_error( - "StopAll cannot start because the service is shutting down"); - } - if (!motor_stop.valid()) { - throw std::runtime_error( - "StopAll could not establish the motor activity barrier"); - } - - // This snapshot is intentionally metadata-only. A wedged health query - // must not prevent physical stop requests from being dispatched. - const auto inventory = dmgr_.inventorySnapshot(); - ScopedControlBarrierSet control_barriers; - std::vector unconfirmed_devices; - - struct ControlTarget final { - std::string id; - cmvr::device::DeviceKind kind{ - cmvr::device::DeviceKind::Unknown}; - std::shared_ptr arm; - std::shared_ptr agv; - std::shared_ptr hand; - cmvr::control::ControlLeaseToken barrier; - }; - std::vector control_targets; - control_targets.reserve(inventory.size()); - - struct OtherTarget final { - std::string id; - std::shared_ptr camera; - std::shared_ptr microphone; - std::shared_ptr speaker; - std::shared_ptr head; - std::shared_ptr gripper; - bool tracked_media{false}; - }; - std::unordered_map other_targets; - other_targets.reserve(inventory.size()); - - for (const auto& device : inventory) { - const bool is_control = - device.kind == cmvr::device::DeviceKind::Arm || - device.kind == cmvr::device::DeviceKind::AGV || - device.kind == cmvr::device::DeviceKind::DexHand; - if (!is_control) { - OtherTarget target; - target.id = device.id; - switch (device.kind) { - case cmvr::device::DeviceKind::Camera: - target.camera = std::dynamic_pointer_cast< - cmvr::device::AbstractCamera>(device.device); - break; - case cmvr::device::DeviceKind::Microphone: - target.microphone = std::dynamic_pointer_cast< - cmvr::device::AbstractMicrophone>(device.device); - break; - case cmvr::device::DeviceKind::Speaker: - target.speaker = std::dynamic_pointer_cast< - cmvr::device::AbstractSpeaker>(device.device); - break; - case cmvr::device::DeviceKind::BioHead: - target.head = std::dynamic_pointer_cast< - cmvr::device::AbstractBiohead>(device.device); - break; - case cmvr::device::DeviceKind::Gripper: - target.gripper = std::dynamic_pointer_cast< - cmvr::device::AbstractGripper>(device.device); - break; - default: - continue; - } - if (!target.camera && !target.microphone && - !target.speaker && !target.head && !target.gripper) { - appendFailure( - unconfirmed_devices, device.id, - "inventory type did not resolve to its typed device"); - continue; - } - other_targets.emplace(device.id, std::move(target)); - continue; - } - - ControlTarget target; - target.id = device.id; - target.kind = device.kind; - if (device.kind == cmvr::device::DeviceKind::Arm) { - target.arm = std::dynamic_pointer_cast< - cmvr::device::RobotArm>(device.device); - } else if (device.kind == cmvr::device::DeviceKind::AGV) { - target.agv = std::dynamic_pointer_cast< - cmvr::device::AbstractAGV>(device.device); - } else { - target.hand = std::dynamic_pointer_cast< - cmvr::device::AbstractDexHand>(device.device); - } - if (!target.arm && !target.agv && !target.hand) { - appendFailure( - unconfirmed_devices, device.id, - "inventory type did not resolve to the typed control device"); - continue; - } - - std::string detail; - bool acquired = false; - try { - acquired = control_barriers.acquire( - device.id, detail, target.barrier); - } catch (const std::exception& error) { - detail = error.what(); - } catch (...) { - detail = "unknown exception"; - } - if (!acquired) { - appendFailure( - unconfirmed_devices, device.id, - "could not establish the device safety barrier" + - (detail.empty() ? "" : ": " + detail)); - continue; - } - control_targets.push_back(std::move(target)); - } - - std::unordered_set tracked_media_ids; - const auto merge_tracked_ids = [&tracked_media_ids]( - const std::vector& ids) { - tracked_media_ids.insert(ids.begin(), ids.end()); - }; - merge_tracked_ids( - cmvr::media::globalMediaSourceManager().trackedSourceIds()); - merge_tracked_ids( - globalCameraPtzActivityRegistry().trackedDeviceIds()); - merge_tracked_ids( - globalCameraOperationalActivityRegistry().trackedDeviceIds()); - for (const auto& id : tracked_media_ids) { - auto [target, inserted] = other_targets.try_emplace(id); - if (inserted) { - target->second.id = id; - } - target->second.tracked_media = true; - } - - // Every independent stop is submitted before waiting for any result. - // A blocked backend therefore cannot delay peer motion or media stops. - std::vector control_stops; - control_stops.reserve(control_targets.size()); - for (const auto& target : control_targets) { - StopHandle handle; - auto deadline_detail = std::make_shared(); - if (target.arm) { - const auto arm = target.arm; - const auto barrier = target.barrier; - deadline_detail->publishInitial( - "initial RobotArm stop did not complete before the " - "StopAll deadline"); - handle = stop_dispatcher_->submit( - "control:" + stopResourceKey(target.id), - [arm, barrier, stop_deadline, deadline_detail] { - return stopControlWithFence( - barrier, stop_deadline, - [arm] { return stopArm(arm, false); }, - [arm] { return stopArm(arm, true); }, - "RobotArm", deadline_detail); - }, - [deadline_detail] { - return deadline_detail->snapshot(); - }); - } else if (target.agv) { - const auto agv = target.agv; - const auto barrier = target.barrier; - deadline_detail->publishInitial( - "initial AGV stop did not complete before the StopAll " - "deadline"); - handle = stop_dispatcher_->submit( - "control:" + stopResourceKey(target.id), - [agv, barrier, stop_deadline, deadline_detail] { - return stopControlWithFence( - barrier, stop_deadline, - [agv] { return stopAgv(agv, false); }, - [agv] { return stopAgv(agv, true); }, - "AGV", deadline_detail); - }, - [deadline_detail] { - return deadline_detail->snapshot(); - }); - } else { - const auto hand = target.hand; - const auto barrier = target.barrier; - deadline_detail->publishInitial( - "initial DexHand stop did not complete before the " - "StopAll deadline"); - handle = stop_dispatcher_->submit( - "control:" + stopResourceKey(target.id), - [hand, barrier, stop_deadline, deadline_detail] { - return stopControlWithFence( - barrier, stop_deadline, - [hand] { return stopDexHand(hand, false); }, - [hand] { return stopDexHand(hand, true); }, - "DexHand", deadline_detail); - }, - [deadline_detail] { - return deadline_detail->snapshot(); - }); - } - control_stops.push_back( - {target.id, std::move(handle), true, true}); - } - - std::vector motor_operations; - std::vector - deferred_motor_operations; - std::string motor_collection_error; - if (!motor_stop.collectStopOperations( - deferred_motor_operations, &motor_collection_error)) { - appendFailure( - unconfirmed_devices, "motors", - motor_collection_error.empty() - ? "could not collect registered motor stop operations" - : motor_collection_error); - } else { - submitDeferredOperations( - *stop_dispatcher_, std::move(deferred_motor_operations), - motor_operations); - } - - std::vector media_cancellations; - std::vector - deferred_media_cancellations; - std::string media_collection_error; - if (!media_stop.collectCancellationOperations( - deferred_media_cancellations, &media_collection_error)) { - appendFailure( - unconfirmed_devices, "media", - media_collection_error.empty() - ? "could not collect active media cancellations" - : media_collection_error); - } else { - submitDeferredOperations( - *stop_dispatcher_, std::move(deferred_media_cancellations), - media_cancellations); - } - - std::vector task_stops; - const auto task_targets = - cmvr::task::TaskManager::activitySnapshotIfInitialized(); - task_stops.reserve(task_targets.size()); - for (const auto& task : task_targets) { - const auto task_id = task ? task->id() : std::string{"unknown"}; - auto handle = stop_dispatcher_->submit( - "task:" + task_id, - [task] { - return invokeStopOperation( - [task] { return stopTaskActivity(task); }, - "task activity stop"); - }); - task_stops.push_back( - {task_id, std::move(handle), false, true}); - } - - std::vector other_stops; - other_stops.reserve(other_targets.size()); - for (const auto& [id, target] : other_targets) { - const auto camera = target.camera; - const auto microphone = target.microphone; - const auto speaker = target.speaker; - const auto head = target.head; - const auto gripper = target.gripper; - const bool tracked_media = target.tracked_media; - auto handle = stop_dispatcher_->submit( - "activity:" + stopResourceKey(id), - [id, camera, microphone, speaker, head, gripper, - tracked_media] { - return stopOtherActivities( - id, camera, microphone, speaker, head, gripper, - tracked_media); - }); - other_stops.push_back( - {id, std::move(handle), false, true}); - } - - std::unordered_set completed_control_stops; - (void)waitForStopOperations( - control_stops, stop_deadline, control_barriers, - unconfirmed_devices, &completed_control_stops); - (void)waitForStopOperations( - motor_operations, stop_deadline, control_barriers, - unconfirmed_devices); - (void)waitForStopOperations( - media_cancellations, stop_deadline, control_barriers, - unconfirmed_devices); - (void)waitForStopOperations( - task_stops, stop_deadline, control_barriers, - unconfirmed_devices); - std::unordered_set completed_other_stops; - (void)waitForStopOperations( - other_stops, stop_deadline, control_barriers, - unconfirmed_devices, &completed_other_stops); - - std::string motor_error; - const bool motors_stopped = motor_stop.waitForStopped( - remainingStopBudget(stop_deadline), &motor_error); - if (!motors_stopped) { - unconfirmed_devices.push_back( - "motors: " + (motor_error.empty() - ? "operational stop was not confirmed" - : motor_error)); - } - - const bool media_stopped = media_stop.waitForStopped( - remainingStopBudget(stop_deadline)); - if (!media_stopped) { - unconfirmed_devices.push_back( - "media: timed out waiting for active RPCs to stop"); - } else { - // The first device stop runs in parallel with media cancellation so - // motion stops are never delayed by a blocked media handler. A - // handler already inside runIfCurrent(), however, can finish a - // start/resume after that first stop. Once every old media session - // has drained, repeat the typed operational stops while all - // admission gates are still closed. Do not retry a device whose - // first stop is still running; StopOperationDispatcher would only - // join that old job and a concurrent driver stop would be unsafe. - std::vector final_media_stops; - final_media_stops.reserve(completed_other_stops.size()); - for (const auto& [id, target] : other_targets) { - const bool needs_final_media_stop = - target.camera || target.microphone || target.speaker || - target.tracked_media; - if (!needs_final_media_stop || - completed_other_stops.count(id) == 0U) { - continue; - } - - const auto camera = target.camera; - const auto microphone = target.microphone; - const auto speaker = target.speaker; - const bool tracked_media = target.tracked_media; - auto handle = stop_dispatcher_->submit( - "activity:" + stopResourceKey(id), - [id, camera, microphone, speaker, tracked_media] { - return stopOtherActivities( - id, camera, microphone, speaker, nullptr, nullptr, - tracked_media); - }); - final_media_stops.push_back( - {id, std::move(handle), false, true}); - } - - // DexHand sensor RPCs also belong to the media coordinator and can - // race their resumeOperationalActivity() with the initial control - // stop. Their typed safety barrier is still held here, so a final - // operational stop cannot admit a new hand command. - for (const auto& target : control_targets) { - if (!target.hand || - completed_control_stops.count(target.id) == 0U) { - continue; - } - const auto hand = target.hand; - auto handle = stop_dispatcher_->submit( - "final-media-control:" + stopResourceKey(target.id), - [hand] { - return invokeStopOperation( - [hand] { return stopDexHand(hand, true); }, - "final DexHand media activity stop"); - }); - final_media_stops.push_back( - {target.id, std::move(handle), true, true}); - } - (void)waitForStopOperations( - final_media_stops, stop_deadline, control_barriers, - unconfirmed_devices); - } - - if (!action_queue_->waitForIdle( - remainingStopBudget(stop_deadline))) { - unconfirmed_devices.push_back( - "ActionQueue: timed out waiting for the execution queue to " - "become idle"); - } - if (!unconfirmed_devices.empty()) { - control_barriers.quarantineAll(); - throw std::runtime_error( - "StopAll could not confirm that every device stopped; " - "control remains paused and affected resources remain " - "quarantined: " + - unconfirmed_devices.front()); - } - control_barriers.recoverRetiredSafetyHoldersAll(); - if (!action_stop.complete(true)) { - throw std::runtime_error( - "StopAll stopped all devices but could not safely resume " - "ActionQueue admission"); - } - if (!media_stop.complete(true)) { - throw std::runtime_error( - "StopAll stopped all media but could not safely resume " - "media activity admission"); - } - if (!motor_stop.complete(true)) { - throw std::runtime_error( - "StopAll stopped all motors but could not safely resume " - "motor command admission"); - } - // Release the current round's typed control barriers while the global - // gate is still closed. Retired barriers from earlier failed rounds - // were recovered above; unrelated active safety holders are preserved. - control_barriers.releaseAll(); - if (!admission_stop.complete(true)) { - throw std::runtime_error( - "StopAll stopped all activities but could not safely resume " - "system admission"); - } - 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; - } - catch (...) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message( - "StopAll failed with an unknown exception"); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - -grpc::Status gRPCSystemServiceImpl::ExecuteActionQueue( - grpc::ServerContext* context, - const cmvr::api::ActionQueueCommand_Request* request, - cmvr::api::ActionQueueCommand_Feedback* response) -{ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.SystemService/ExecuteActionQueue"); - if (!request || !response) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "ActionQueue request and response are required"); - } - try { - cmvr::safety::CommandActor actor; - actor.principal_id = - cmvr_grpc_call_guard.context().principal.id; - actor.authenticated = - cmvr_grpc_call_guard.context().principal.authenticated; - for (const auto role : - cmvr_grpc_call_guard.context().principal.roles) { - actor.roles.emplace_back(cmvr::service::toString(role)); - } - // The callback is consumed only on this synchronous handler stack. It - // is never retained by the worker-owned action Record, so returning the - // RPC cannot leave a dangling ServerContext reference. - const auto wait_result = action_queue_->submitAndWait( - *request, - *response, - [context]() { - return context && context->IsCancelled(); - }, - std::move(actor)); - if (wait_result == - ActionQueueExecutor::WaitResult::CanceledBeforeAdmission) { - return grpc::Status( - grpc::StatusCode::CANCELLED, - "ActionQueue RPC was canceled before admission"); - } - if (wait_result == - ActionQueueExecutor::WaitResult::CanceledAfterAdmission) { - return grpc::Status( - grpc::StatusCode::CANCELLED, - "ActionQueue RPC waiter was canceled after admission; the edge action continues and its result can be retrieved with the same action_id"); - } - return grpc::Status::OK; - } catch (const std::exception& error) { - response->Clear(); - response->set_action_id(request->action_id()); - response->set_service_instance_id(action_queue_->instanceId()); - response->set_result(api::ACTION_RESULT_CODE_FAILED); - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(error.what()); - setCurrentTimestamp( - response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} diff --git a/cmvr-es/service/grpc/server/src/media_activity_coordinator.cpp b/cmvr-es/service/grpc/server/src/media_activity_coordinator.cpp deleted file mode 100644 index 2af439ea..00000000 --- a/cmvr-es/service/grpc/server/src/media_activity_coordinator.cpp +++ /dev/null @@ -1,459 +0,0 @@ -#include "service/grpc/server/include/media_activity_coordinator.h" - -#include "common/base/logging/logger.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::service { - -struct MediaActivityCoordinator::SessionState final { - SessionState( - const std::uint64_t session_id_value, - const std::uint64_t generation_value, - CancelCallback callback) - : session_id(session_id_value), - generation(generation_value), - cancel_callback(std::move(callback)) {} - - void markCancelled() noexcept - { - is_cancelled.store(true, std::memory_order_release); - } - - DeferredStopResult requestCancel() - { - markCancelled(); - - CancelCallback callback; - { - std::unique_lock lock(callback_mutex); - if (callback_in_flight) { - callback_condition.wait( - lock, [this] { return !callback_in_flight; }); - } - if (callback_invoked) { - return cancel_result; - } - if (released || !cancel_callback) { - callback_invoked = true; - cancel_result = {true, {}}; - return cancel_result; - } - callback_invoked = true; - callback_in_flight = true; - callback = cancel_callback; - } - - try { - DeferredStopResult result{true, {}}; - try { - callback(); - } catch (const std::exception& error) { - result = { - false, - std::string("media cancellation callback threw: ") + - error.what()}; - CMVR_LOG(ERROR) - << "[MediaActivityCoordinator] " << result.detail; - } catch (...) { - result = { - false, - "media cancellation callback threw an unknown exception"}; - CMVR_LOG(ERROR) - << "[MediaActivityCoordinator] " << result.detail; - } - - { - std::lock_guard lock(callback_mutex); - cancel_result = result; - callback_in_flight = false; - } - callback_condition.notify_all(); - return result; - } catch (...) { - { - std::lock_guard lock(callback_mutex); - callback_in_flight = false; - } - callback_condition.notify_all(); - throw; - } - } - - void prepareRelease() noexcept - { - std::unique_lock lock(callback_mutex); - released = true; - cancel_callback = {}; - callback_condition.wait(lock, [this] { return !callback_in_flight; }); - } - - const std::uint64_t session_id; - const std::uint64_t generation; - std::atomic is_cancelled{false}; - std::mutex callback_mutex; - std::condition_variable callback_condition; - CancelCallback cancel_callback; - bool callback_invoked{false}; - bool callback_in_flight{false}; - bool released{false}; - DeferredStopResult cancel_result; - std::size_t dispatches_in_flight{0U}; - std::unordered_set exclusive_resources; -}; - -struct MediaActivityCoordinator::Impl final { - bool hasSessionsBefore(const std::uint64_t generation) const - { - for (const auto& [session_id, session] : sessions) { - (void)session_id; - if (session->generation < generation) { - return true; - } - } - return false; - } - - mutable std::mutex mutex; - std::condition_variable condition; - std::unordered_map> sessions; - std::unordered_map exclusive_resources; - std::unordered_set stop_all_tickets; - std::uint64_t generation{1U}; - std::uint64_t next_session_id{0U}; - std::uint64_t next_ticket_id{0U}; - bool accepting{true}; - bool stop_all_failed{false}; -}; - -MediaActivityCoordinator::Session::Session( - std::shared_ptr impl, - std::shared_ptr state) - : impl_(std::move(impl)), state_(std::move(state)) {} - -MediaActivityCoordinator::Session::~Session() -{ - reset(); -} - -MediaActivityCoordinator::Session::Session(Session&& other) noexcept - : impl_(std::move(other.impl_)), state_(std::move(other.state_)) {} - -MediaActivityCoordinator::Session& -MediaActivityCoordinator::Session::operator=(Session&& other) noexcept -{ - if (this != &other) { - reset(); - impl_ = std::move(other.impl_); - state_ = std::move(other.state_); - } - return *this; -} - -MediaActivityCoordinator::Session::operator bool() const noexcept -{ - return impl_ && state_; -} - -bool MediaActivityCoordinator::Session::cancelled() const noexcept -{ - return !state_ || state_->is_cancelled.load(std::memory_order_acquire); -} - -bool MediaActivityCoordinator::Session::runIfCurrent( - const std::function& operation) const -{ - if (!impl_ || !state_ || !operation) { - return false; - } - - { - std::lock_guard lock(impl_->mutex); - const auto active = impl_->sessions.find(state_->session_id); - if (!impl_->accepting || - state_->generation != impl_->generation || - state_->is_cancelled.load(std::memory_order_acquire) || - active == impl_->sessions.end() || - active->second != state_) { - return false; - } - ++state_->dispatches_in_flight; - } - - const auto finish_dispatch = [impl = impl_, state = state_]() noexcept { - try { - { - std::lock_guard lock(impl->mutex); - if (state->dispatches_in_flight > 0U) { - --state->dispatches_in_flight; - } - } - impl->condition.notify_all(); - } catch (...) { - } - }; - - try { - operation(); - } catch (...) { - finish_dispatch(); - throw; - } - finish_dispatch(); - return true; -} - -bool MediaActivityCoordinator::Session::claimExclusiveResource( - const std::string& resource_key) -{ - if (!impl_ || !state_ || resource_key.empty()) { - return false; - } - - std::lock_guard lock(impl_->mutex); - const auto active = impl_->sessions.find(state_->session_id); - if (!impl_->accepting || - state_->generation != impl_->generation || - state_->is_cancelled.load(std::memory_order_acquire) || - active == impl_->sessions.end() || - active->second != state_) { - return false; - } - - const auto owner = impl_->exclusive_resources.find(resource_key); - if (owner != impl_->exclusive_resources.end()) { - return owner->second == state_->session_id; - } - impl_->exclusive_resources.emplace(resource_key, state_->session_id); - state_->exclusive_resources.emplace(resource_key); - return true; -} - -void MediaActivityCoordinator::Session::reset() noexcept -{ - auto impl = std::move(impl_); - auto state = std::move(state_); - if (!impl || !state) { - return; - } - - state->prepareRelease(); - { - std::unique_lock lock(impl->mutex); - state->markCancelled(); - impl->condition.wait(lock, [&state] { - return state->dispatches_in_flight == 0U; - }); - const auto active = impl->sessions.find(state->session_id); - if (active != impl->sessions.end() && active->second == state) { - impl->sessions.erase(active); - } - for (const auto& resource_key : state->exclusive_resources) { - const auto owner = impl->exclusive_resources.find(resource_key); - if (owner != impl->exclusive_resources.end() && - owner->second == state->session_id) { - impl->exclusive_resources.erase(owner); - } - } - } - impl->condition.notify_all(); -} - -MediaActivityCoordinator::MediaActivityCoordinator() - : impl_(std::make_shared()) {} - -MediaActivityCoordinator::Session MediaActivityCoordinator::beginSession( - CancelCallback cancel) -{ - auto system_admission = - globalStopAllAdmissionGate().lockAdmission(); - std::lock_guard lock(impl_->mutex); - if (!system_admission.accepting() || !impl_->accepting) { - return {}; - } - - const auto session_id = ++impl_->next_session_id; - auto state = std::make_shared( - session_id, impl_->generation, std::move(cancel)); - impl_->sessions.emplace(session_id, state); - return Session(impl_, std::move(state)); -} - -MediaActivityCoordinator::StopAllTicket -MediaActivityCoordinator::beginStopAll(const bool defer_cancellation) -{ - StopAllTicket ticket; - std::vector> sessions; - { - std::lock_guard lock(impl_->mutex); - if (impl_->accepting || impl_->stop_all_tickets.empty()) { - impl_->accepting = false; - ++impl_->generation; - impl_->stop_all_failed = false; - } - - ticket.generation = impl_->generation; - ticket.ticket_id = ++impl_->next_ticket_id; - impl_->stop_all_tickets.emplace(ticket.ticket_id); - - sessions.reserve(impl_->sessions.size()); - for (const auto& [session_id, session] : impl_->sessions) { - (void)session_id; - if (session->generation < ticket.generation) { - session->markCancelled(); - sessions.push_back(session); - } - } - } - - if (!defer_cancellation) { - for (const auto& session : sessions) { - (void)session->requestCancel(); - } - } - return ticket; -} - -bool MediaActivityCoordinator::collectCancellationOperations( - const StopAllTicket& ticket, - std::vector& operations, - std::string* error) const -{ - operations.clear(); - if (error) { - error->clear(); - } - if (!ticket.valid()) { - if (error) { - *error = "invalid media StopAll ticket"; - } - return false; - } - - { - std::lock_guard lock(impl_->mutex); - if (impl_->accepting || ticket.generation != impl_->generation || - impl_->stop_all_tickets.count(ticket.ticket_id) == 0U) { - if (error) { - *error = "media StopAll ticket is no longer current"; - } - return false; - } - operations.reserve(impl_->sessions.size()); - for (const auto& [session_id, session] : impl_->sessions) { - if (session->generation < ticket.generation) { - operations.push_back({ - "media-session:" + std::to_string(session_id), - [session] { return session->requestCancel(); }}); - } - } - } - std::sort( - operations.begin(), operations.end(), - [](const auto& lhs, const auto& rhs) { - return lhs.resource_key < rhs.resource_key; - }); - return true; -} - -bool MediaActivityCoordinator::requestCancellation( - const StopAllTicket& ticket) -{ - std::vector operations; - if (!collectCancellationOperations(ticket, operations)) { - return false; - } - - bool all_cancelled = true; - for (const auto& operation : operations) { - try { - if (!operation.operation().success) { - all_cancelled = false; - } - } catch (...) { - all_cancelled = false; - } - } - return all_cancelled; -} - -bool MediaActivityCoordinator::waitForStopped( - const StopAllTicket& ticket, - const std::chrono::milliseconds timeout) -{ - if (!ticket.valid() || timeout < std::chrono::milliseconds::zero()) { - return false; - } - - std::unique_lock lock(impl_->mutex); - if (ticket.generation != impl_->generation || - impl_->stop_all_tickets.count(ticket.ticket_id) == 0U) { - return false; - } - return impl_->condition.wait_for(lock, timeout, [this, &ticket] { - return ticket.generation == impl_->generation && - !impl_->hasSessionsBefore(ticket.generation); - }); -} - -bool MediaActivityCoordinator::finishStopAll( - const StopAllTicket& ticket, - const bool all_media_stopped) -{ - return finishStopAllDetailed(ticket, all_media_stopped) - .participant_stopped; -} - -MediaActivityCoordinator::FinishStopAllResult -MediaActivityCoordinator::finishStopAllDetailed( - const StopAllTicket& ticket, - const bool all_media_stopped) -{ - std::lock_guard lock(impl_->mutex); - if (!ticket.valid() || impl_->accepting || - ticket.generation != impl_->generation || - impl_->stop_all_tickets.erase(ticket.ticket_id) == 0U) { - return {}; - } - - const bool caller_succeeded = - all_media_stopped && !impl_->hasSessionsBefore(ticket.generation); - if (!caller_succeeded) { - impl_->stop_all_failed = true; - } - if (impl_->stop_all_tickets.empty() && !impl_->stop_all_failed && - !impl_->hasSessionsBefore(ticket.generation)) { - impl_->accepting = true; - } - return {true, caller_succeeded, impl_->accepting}; -} - -void MediaActivityCoordinator::clearForTesting() noexcept -{ - try { - std::lock_guard lock(impl_->mutex); - impl_->accepting = true; - impl_->stop_all_failed = false; - ++impl_->generation; - impl_->stop_all_tickets.clear(); - } catch (...) { - } - impl_->condition.notify_all(); -} - -MediaActivityCoordinator& globalMediaActivityCoordinator() -{ - static MediaActivityCoordinator coordinator; - return coordinator; -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/src/motor_activity_coordinator.cpp b/cmvr-es/service/grpc/server/src/motor_activity_coordinator.cpp deleted file mode 100644 index 9bab0d38..00000000 --- a/cmvr-es/service/grpc/server/src/motor_activity_coordinator.cpp +++ /dev/null @@ -1,497 +0,0 @@ -#include "service/grpc/server/include/motor_activity_coordinator.h" - -#include -#include -#include -#include -#include -#include -#include - -#include "common/base/logging/logger.h" - -namespace cmvr::service { - -namespace { - -struct ControlCallbacks final { - MotorActivityCoordinator::CancelCallback cancel; - MotorActivityCoordinator::QuickStopCallback quick_stop; - MotorActivityCoordinator::IdleCallback idle; - std::string description; -}; - -struct MotorStopOperationState final { - explicit MotorStopOperationState(ControlCallbacks callbacks_value) - : callbacks(std::move(callbacks_value)) - { - } - - DeferredStopResult cancel() - { - { - std::unique_lock lock(mutex); - condition.wait(lock, [this] { return !cancel_running; }); - if (cancel_completed) { - return cancel_result; - } - cancel_running = true; - } - - try { - DeferredStopResult result{true, {}}; - try { - callbacks.cancel(); - } catch (const std::exception& error) { - result = { - false, - std::string("motor cancellation callback threw: ") + - error.what()}; - } catch (...) { - result = { - false, - "motor cancellation callback threw an unknown exception"}; - } - - { - std::lock_guard lock(mutex); - cancel_result = result; - cancel_completed = true; - cancel_running = false; - } - condition.notify_all(); - return result; - } catch (...) { - { - std::lock_guard lock(mutex); - cancel_running = false; - } - condition.notify_all(); - throw; - } - } - - DeferredStopResult run() - { - { - std::unique_lock lock(mutex); - condition.wait(lock, [this] { return !stop_running; }); - if (stop_completed) { - return stop_result; - } - stop_running = true; - } - - try { - DeferredStopResult result = cancel(); - bool quick_stopped = false; - std::string quick_stop_error; - try { - quick_stopped = callbacks.quick_stop(); - if (!quick_stopped) { - quick_stop_error = callbacks.description.empty() - ? "a registered motor did not confirm quick-stop" - : callbacks.description + - ": quick-stop was not confirmed"; - } - } catch (const std::exception& error) { - quick_stop_error = - std::string("motor quick-stop callback threw: ") + - error.what(); - } catch (...) { - quick_stop_error = - "motor quick-stop callback threw an unknown exception"; - } - if (!quick_stopped) { - if (result.success) { - result = {false, std::move(quick_stop_error)}; - } else if (!quick_stop_error.empty()) { - result.detail += "; " + quick_stop_error; - } - } - - { - std::lock_guard lock(mutex); - stop_result = result; - stop_completed = true; - stop_running = false; - } - condition.notify_all(); - return result; - } catch (...) { - { - std::lock_guard lock(mutex); - stop_running = false; - } - condition.notify_all(); - throw; - } - } - - ControlCallbacks callbacks; - std::mutex mutex; - std::condition_variable condition; - bool cancel_running{false}; - bool cancel_completed{false}; - bool stop_running{false}; - bool stop_completed{false}; - DeferredStopResult cancel_result; - DeferredStopResult stop_result; -}; - -} // namespace - -struct MotorActivityCoordinator::Impl final { - bool targetsIdle() const - { - for (const auto& [id, stop_state] : round_targets) { - (void)stop_state; - const auto entry = controls.find(id); - if (entry == controls.end()) { - continue; - } - try { - if (!entry->second.idle || !entry->second.idle()) { - return false; - } - } catch (...) { - return false; - } - } - return true; - } - - std::mutex mutex; - std::condition_variable condition; - std::unordered_map controls; - std::unordered_map< - std::uint64_t, std::shared_ptr> round_targets; - std::unordered_set stop_all_tickets; - std::uint64_t generation{1U}; - std::uint64_t next_control_id{0U}; - std::uint64_t next_ticket_id{0U}; - bool accepting{true}; - bool stop_all_failed{false}; -}; - -MotorActivityCoordinator::Registration::Registration( - std::shared_ptr impl, - const std::uint64_t id) noexcept - : impl_(std::move(impl)), id_(id) -{ -} - -MotorActivityCoordinator::Registration::~Registration() -{ - reset(); -} - -MotorActivityCoordinator::Registration::Registration( - Registration&& other) noexcept - : impl_(std::move(other.impl_)), id_(std::exchange(other.id_, 0U)) -{ -} - -MotorActivityCoordinator::Registration& -MotorActivityCoordinator::Registration::operator=(Registration&& other) noexcept -{ - if (this != &other) { - reset(); - impl_ = std::move(other.impl_); - id_ = std::exchange(other.id_, 0U); - } - return *this; -} - -MotorActivityCoordinator::Registration::operator bool() const noexcept -{ - return impl_ && id_ != 0U; -} - -void MotorActivityCoordinator::Registration::reset() noexcept -{ - auto impl = std::move(impl_); - const auto id = std::exchange(id_, 0U); - if (!impl || id == 0U) { - return; - } - try { - { - std::lock_guard lock(impl->mutex); - impl->controls.erase(id); - } - impl->condition.notify_all(); - } catch (...) { - } -} - -MotorActivityCoordinator::AdmissionGuard::AdmissionGuard( - std::unique_lock&& lock, - const bool accepting) noexcept - : lock_(std::move(lock)), accepting_(accepting) -{ -} - -MotorActivityCoordinator::MotorActivityCoordinator() - : impl_(std::make_shared()) -{ -} - -MotorActivityCoordinator::Registration -MotorActivityCoordinator::registerControl( - CancelCallback cancel, - QuickStopCallback quick_stop, - IdleCallback idle, - std::string description) -{ - if (!cancel || !quick_stop || !idle) { - return {}; - } - - std::lock_guard lock(impl_->mutex); - const auto id = ++impl_->next_control_id; - impl_->controls.emplace( - id, - ControlCallbacks{ - std::move(cancel), std::move(quick_stop), std::move(idle), - std::move(description)}); - return Registration(impl_, id); -} - -MotorActivityCoordinator::AdmissionGuard -MotorActivityCoordinator::lockAdmission() -{ - std::unique_lock lock(impl_->mutex); - return AdmissionGuard(std::move(lock), impl_->accepting); -} - -MotorActivityCoordinator::StopAllTicket -MotorActivityCoordinator::beginStopAll(const bool defer_callbacks) -{ - StopAllTicket ticket; - std::vector> targets; - { - std::lock_guard lock(impl_->mutex); - if (impl_->accepting || impl_->stop_all_tickets.empty()) { - impl_->accepting = false; - impl_->stop_all_failed = false; - ++impl_->generation; - impl_->round_targets.clear(); - targets.reserve(impl_->controls.size()); - for (const auto& [id, callbacks] : impl_->controls) { - auto state = - std::make_shared(callbacks); - impl_->round_targets.emplace(id, state); - targets.push_back(std::move(state)); - } - } - - ticket.generation = impl_->generation; - ticket.ticket_id = ++impl_->next_ticket_id; - impl_->stop_all_tickets.emplace(ticket.ticket_id); - if (targets.empty()) { - targets.reserve(impl_->round_targets.size()); - for (const auto& [id, state] : impl_->round_targets) { - (void)id; - targets.push_back(state); - } - } - } - - if (!defer_callbacks) { - for (const auto& target : targets) { - const auto result = target->cancel(); - if (!result.success) { - CMVR_LOG(ERROR) - << "[MotorActivityCoordinator] " << result.detail; - } - } - } - impl_->condition.notify_all(); - return ticket; -} - -bool MotorActivityCoordinator::collectStopOperations( - const StopAllTicket& ticket, - std::vector& operations, - std::string* error) const -{ - operations.clear(); - if (error) { - error->clear(); - } - if (!ticket.valid()) { - if (error) { - *error = "invalid motor StopAll ticket"; - } - return false; - } - - { - std::lock_guard lock(impl_->mutex); - if (impl_->accepting || ticket.generation != impl_->generation || - impl_->stop_all_tickets.count(ticket.ticket_id) == 0U) { - if (error) { - *error = "motor StopAll ticket is no longer current"; - } - return false; - } - operations.reserve(impl_->round_targets.size()); - for (const auto& [id, state] : impl_->round_targets) { - operations.push_back({ - "motor-control:" + std::to_string(id), - [state] { return state->run(); }}); - } - } - std::sort( - operations.begin(), operations.end(), - [](const auto& lhs, const auto& rhs) { - return lhs.resource_key < rhs.resource_key; - }); - return true; -} - -bool MotorActivityCoordinator::requestStop( - const StopAllTicket& ticket, - std::string* error) -{ - std::vector operations; - if (!collectStopOperations(ticket, operations, error)) { - return false; - } - - bool all_stopped = true; - for (const auto& operation : operations) { - DeferredStopResult result; - try { - result = operation.operation(); - } catch (const std::exception& exception) { - result = { - false, - std::string("motor stop operation threw: ") + - exception.what()}; - } catch (...) { - result = { - false, "motor stop operation threw an unknown exception"}; - } - if (!result.success) { - all_stopped = false; - if (error && error->empty()) { - *error = result.detail.empty() - ? "a registered motor did not confirm quick-stop" - : result.detail; - } - CMVR_LOG(ERROR) - << "[MotorActivityCoordinator] " - << (result.detail.empty() - ? "motor stop operation was not confirmed" - : result.detail); - } - } - return all_stopped; -} - -bool MotorActivityCoordinator::waitForStopped( - const StopAllTicket& ticket, - const std::chrono::milliseconds timeout, - std::string* error) -{ - if (!ticket.valid() || timeout < std::chrono::milliseconds::zero()) { - if (error && error->empty()) { - *error = "invalid motor StopAll ticket or timeout"; - } - return false; - } - - const auto deadline = std::chrono::steady_clock::now() + timeout; - std::unique_lock lock(impl_->mutex); - const bool idle = impl_->condition.wait_until(lock, deadline, [&] { - return ticket.generation != impl_->generation || - impl_->stop_all_tickets.count(ticket.ticket_id) == 0U || - impl_->targetsIdle(); - }); - const bool ticket_current = - !impl_->accepting && ticket.generation == impl_->generation && - impl_->stop_all_tickets.count(ticket.ticket_id) != 0U; - const bool targets_idle = ticket_current && impl_->targetsIdle(); - if ((!idle || !targets_idle) && error && error->empty()) { - *error = "timed out waiting for active motor RPCs to stop"; - } - return idle && targets_idle; -} - -bool MotorActivityCoordinator::stopAndWait( - const StopAllTicket& ticket, - const std::chrono::milliseconds timeout, - std::string* error) -{ - if (error) { - error->clear(); - } - const bool stop_requested = requestStop(ticket, error); - const bool stopped = waitForStopped(ticket, timeout, error); - return stop_requested && stopped; -} - -bool MotorActivityCoordinator::finishStopAll( - const StopAllTicket& ticket, - const bool all_motors_stopped) -{ - return finishStopAllDetailed(ticket, all_motors_stopped) - .participant_stopped; -} - -MotorActivityCoordinator::FinishStopAllResult -MotorActivityCoordinator::finishStopAllDetailed( - const StopAllTicket& ticket, - const bool all_motors_stopped) -{ - std::lock_guard lock(impl_->mutex); - if (!ticket.valid() || impl_->accepting || - ticket.generation != impl_->generation || - impl_->stop_all_tickets.erase(ticket.ticket_id) == 0U) { - return {}; - } - - const bool caller_succeeded = - all_motors_stopped && impl_->targetsIdle(); - if (!caller_succeeded) { - impl_->stop_all_failed = true; - } - if (impl_->stop_all_tickets.empty() && !impl_->stop_all_failed && - impl_->targetsIdle()) { - impl_->accepting = true; - impl_->round_targets.clear(); - } - return {true, caller_succeeded, impl_->accepting}; -} - -void MotorActivityCoordinator::notifyStateChanged() noexcept -{ - try { - impl_->condition.notify_all(); - } catch (...) { - } -} - -void MotorActivityCoordinator::clearForTesting() noexcept -{ - try { - std::lock_guard lock(impl_->mutex); - impl_->accepting = true; - impl_->stop_all_failed = false; - ++impl_->generation; - impl_->round_targets.clear(); - impl_->stop_all_tickets.clear(); - } catch (...) { - } - notifyStateChanged(); -} - -MotorActivityCoordinator& globalMotorActivityCoordinator() -{ - static MotorActivityCoordinator coordinator; - return coordinator; -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/camera_operational_activity_registry_test.cpp b/cmvr-es/service/grpc/server/tests/camera_operational_activity_registry_test.cpp deleted file mode 100644 index 973fdd9e..00000000 --- a/cmvr-es/service/grpc/server/tests/camera_operational_activity_registry_test.cpp +++ /dev/null @@ -1,491 +0,0 @@ -#include "service/grpc/server/include/camera_operational_activity_registry.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { -namespace { - -class TestCamera final : public device::AbstractCamera { -public: - explicit TestCamera(std::string id) - { - id_ = std::move(id); - state_.is_initialized = true; - } - - std::string typeName() const override { return "TestCamera"; } - - void getState(device::CameraState& state) override - { - std::lock_guard lock(mutex_); - state = state_; - } - - bool start() override - { - ++lifecycle_start_calls_; - return true; - } - - bool stop() override - { - ++lifecycle_stop_calls_; - std::lock_guard lock(mutex_); - operational_active_ = false; - state_.is_opened = false; - return !fail_lifecycle_stop_; - } - - bool startOperationalActivity() override - { - ++operational_start_calls_; - { - std::unique_lock lock(mutex_); - start_entered_ = true; - condition_.notify_all(); - condition_.wait(lock, [this] { return !block_start_; }); - if (fail_operational_start_) { - return false; - } - operational_active_ = true; - state_.is_opened = true; - } - return true; - } - - bool stopOperationalActivity() override - { - ++operational_stop_calls_; - std::unique_lock lock(mutex_); - stop_entered_ = true; - condition_.notify_all(); - condition_.wait(lock, [this] { return !block_stop_; }); - if (fail_operational_stop_) { - return false; - } - operational_active_ = false; - state_.is_opened = false; - return true; - } - - void setFailOperationalStart(const bool fail) - { - fail_operational_start_ = fail; - } - - void setFailOperationalStop(const bool fail) - { - fail_operational_stop_ = fail; - } - - void blockStop() - { - std::lock_guard lock(mutex_); - block_stop_ = true; - stop_entered_ = false; - } - - void waitForStopEntered() - { - std::unique_lock lock(mutex_); - condition_.wait(lock, [this] { return stop_entered_; }); - } - - void releaseStop() - { - { - std::lock_guard lock(mutex_); - block_stop_ = false; - } - condition_.notify_all(); - } - - void blockStart() - { - std::lock_guard lock(mutex_); - block_start_ = true; - start_entered_ = false; - } - - void waitForStartEntered() - { - std::unique_lock lock(mutex_); - condition_.wait(lock, [this] { return start_entered_; }); - } - - void releaseStart() - { - { - std::lock_guard lock(mutex_); - block_start_ = false; - } - condition_.notify_all(); - } - - bool operationalActive() const - { - std::lock_guard lock(mutex_); - return operational_active_; - } - - int lifecycleStartCalls() const { return lifecycle_start_calls_; } - int lifecycleStopCalls() const { return lifecycle_stop_calls_; } - int operationalStartCalls() const { return operational_start_calls_; } - int operationalStopCalls() const { return operational_stop_calls_; } - -private: - mutable std::mutex mutex_; - std::condition_variable condition_; - bool operational_active_{false}; - bool fail_operational_start_{false}; - bool fail_operational_stop_{false}; - bool fail_lifecycle_stop_{false}; - bool block_start_{false}; - bool start_entered_{false}; - bool block_stop_{false}; - bool stop_entered_{false}; - std::atomic lifecycle_start_calls_{0}; - std::atomic lifecycle_stop_calls_{0}; - std::atomic operational_start_calls_{0}; - std::atomic operational_stop_calls_{0}; -}; - -class CameraOperationalActivityRegistryTest : public ::testing::Test { -protected: - void SetUp() override - { - globalStopAllAdmissionGate().clearForTesting(); - } - - void TearDown() override - { - globalStopAllAdmissionGate().clearForTesting(); - } -}; - -TEST_F(CameraOperationalActivityRegistryTest, - StopAllStopsTrackedActivityWithoutLifecycleStopAndCanRestart) -{ - CameraOperationalActivityRegistry registry; - auto camera = std::make_shared("camera"); - - EXPECT_EQ( - registry.start(camera->id(), camera), - CameraOperationalActivityRegistry::DispatchResult::Success); - EXPECT_TRUE(camera->operationalActive()); - EXPECT_EQ(registry.activeCameraCount(), 1U); - - auto ticket = globalStopAllAdmissionGate().beginStopAll(); - ASSERT_TRUE(ticket.valid()); - EXPECT_TRUE(registry.stopAllActivities()); - EXPECT_FALSE(camera->operationalActive()); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - EXPECT_EQ(camera->operationalStopCalls(), 1); - EXPECT_EQ(registry.activeCameraCount(), 0U); - ASSERT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); - - EXPECT_EQ( - registry.start(camera->id(), camera), - CameraOperationalActivityRegistry::DispatchResult::Success); - EXPECT_TRUE(camera->operationalActive()); - EXPECT_EQ(camera->operationalStartCalls(), 2); - EXPECT_EQ(camera->lifecycleStartCalls(), 0); -} - -TEST_F(CameraOperationalActivityRegistryTest, - FailedOperationalStopRemainsTrackedAndKeepsAdmissionClosed) -{ - CameraOperationalActivityRegistry registry; - auto camera = std::make_shared("camera"); - ASSERT_EQ( - registry.start(camera->id(), camera), - CameraOperationalActivityRegistry::DispatchResult::Success); - - camera->setFailOperationalStop(true); - auto ticket = globalStopAllAdmissionGate().beginStopAll(); - std::vector failures; - EXPECT_FALSE(registry.stopAllActivities(&failures)); - EXPECT_EQ(registry.activeCameraCount(), 1U); - ASSERT_EQ(failures.size(), 1U); - EXPECT_EQ( - registry.start(camera->id(), camera), - CameraOperationalActivityRegistry::DispatchResult::RejectedByStopAll); - EXPECT_FALSE(globalStopAllAdmissionGate().finishStopAll(ticket, false)); - - camera->setFailOperationalStop(false); - ticket = globalStopAllAdmissionGate().beginStopAll(); - EXPECT_TRUE(registry.stopAllActivities()); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); - EXPECT_EQ(registry.activeCameraCount(), 0U); -} - -TEST_F(CameraOperationalActivityRegistryTest, - StopAllWaitsForRacingStartThenStopsTheStartedActivity) -{ - CameraOperationalActivityRegistry registry; - auto camera = std::make_shared("camera"); - camera->blockStart(); - - CameraOperationalActivityRegistry::DispatchResult start_result = - CameraOperationalActivityRegistry::DispatchResult::DeviceFailure; - std::thread start_thread([&] { - start_result = registry.start(camera->id(), camera); - }); - camera->waitForStartEntered(); - - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - std::atomic stop_returned{false}; - bool stop_result = false; - std::thread stop_thread([&] { - stop_result = registry.stopAllActivities(); - stop_returned = true; - }); - - std::this_thread::yield(); - EXPECT_FALSE(stop_returned.load()); - camera->releaseStart(); - start_thread.join(); - stop_thread.join(); - - EXPECT_EQ( - start_result, - CameraOperationalActivityRegistry::DispatchResult::Success); - EXPECT_TRUE(stop_result); - EXPECT_FALSE(camera->operationalActive()); - EXPECT_EQ(camera->operationalStopCalls(), 1); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); -} - -TEST_F(CameraOperationalActivityRegistryTest, - TrackedIdsExposeAStartThatIsStillInsideDriverIo) -{ - CameraOperationalActivityRegistry registry; - auto camera = std::make_shared("camera.pending"); - camera->blockStart(); - - CameraOperationalActivityRegistry::DispatchResult start_result = - CameraOperationalActivityRegistry::DispatchResult::DeviceFailure; - std::thread start_thread([&] { - start_result = registry.start(camera->id(), camera); - }); - camera->waitForStartEntered(); - - const auto snapshot_started = std::chrono::steady_clock::now(); - EXPECT_EQ( - registry.trackedDeviceIds(), - (std::vector{"camera.pending"})); - EXPECT_LT( - std::chrono::steady_clock::now() - snapshot_started, - std::chrono::milliseconds(100)); - - camera->releaseStart(); - start_thread.join(); - EXPECT_EQ( - start_result, - CameraOperationalActivityRegistry::DispatchResult::Success); -} - -TEST_F(CameraOperationalActivityRegistryTest, - DefaultOperationalStopFailsClosed) -{ - class UnsupportedCamera final : public device::AbstractCamera { - public: - UnsupportedCamera() { id_ = "unsupported"; } - std::string typeName() const override { return "UnsupportedCamera"; } - void getState(device::CameraState& state) override { state = {}; } - }; - - CameraOperationalActivityRegistry registry; - auto camera = std::make_shared(); - ASSERT_EQ( - registry.start(camera->id(), camera), - CameraOperationalActivityRegistry::DispatchResult::Success); - - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - EXPECT_FALSE(registry.stopAllActivities()); - EXPECT_EQ(registry.activeCameraCount(), 1U); - EXPECT_FALSE(globalStopAllAdmissionGate().finishStopAll(ticket, false)); -} - -TEST_F(CameraOperationalActivityRegistryTest, - StopForDeviceIsSelectiveAndActiveIdsReflectFailures) -{ - CameraOperationalActivityRegistry registry; - auto first = std::make_shared("camera.first"); - auto second = std::make_shared("camera.second"); - ASSERT_EQ( - registry.start(first->id(), first), - CameraOperationalActivityRegistry::DispatchResult::Success); - ASSERT_EQ( - registry.start(second->id(), second), - CameraOperationalActivityRegistry::DispatchResult::Success); - EXPECT_EQ( - registry.activeDeviceIds(), - (std::vector{"camera.first", "camera.second"})); - EXPECT_EQ( - registry.trackedDeviceIds(), - (std::vector{"camera.first", "camera.second"})); - - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - first->setFailOperationalStop(true); - std::vector failures; - EXPECT_FALSE(registry.stopActivitiesForDevice(first->id(), &failures)); - EXPECT_EQ(failures.size(), 1U); - EXPECT_TRUE(first->operationalActive()); - EXPECT_TRUE(second->operationalActive()); - EXPECT_EQ(second->operationalStopCalls(), 0); - - first->setFailOperationalStop(false); - EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); - EXPECT_EQ( - registry.activeDeviceIds(), - (std::vector{"camera.second"})); - EXPECT_TRUE(registry.stopActivitiesForDevice("missing")); - EXPECT_TRUE(registry.stopAllActivities()); - EXPECT_TRUE(registry.activeDeviceIds().empty()); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); - EXPECT_EQ(first->lifecycleStopCalls(), 0); - EXPECT_EQ(second->lifecycleStopCalls(), 0); -} - -TEST_F(CameraOperationalActivityRegistryTest, - InventoryFallbackStopsOncePerStopAllRoundAndCanStopAgainLater) -{ - CameraOperationalActivityRegistry registry; - auto camera = std::make_shared("inventory-camera"); - ASSERT_TRUE(camera->startOperationalActivity()); - - auto ticket = globalStopAllAdmissionGate().beginStopAll(); - ASSERT_TRUE(ticket.valid()); - EXPECT_TRUE(registry.stopActivitiesForDevice(camera->id(), camera)); - EXPECT_TRUE(registry.stopActivitiesForDevice(camera->id(), camera)); - EXPECT_FALSE(camera->operationalActive()); - EXPECT_EQ(camera->operationalStopCalls(), 1); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - ASSERT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); - - ASSERT_TRUE(camera->startOperationalActivity()); - ticket = globalStopAllAdmissionGate().beginStopAll(); - ASSERT_TRUE(ticket.valid()); - EXPECT_TRUE(registry.stopActivitiesForDevice(camera->id(), camera)); - EXPECT_FALSE(camera->operationalActive()); - EXPECT_EQ(camera->operationalStopCalls(), 2); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); -} - -TEST_F(CameraOperationalActivityRegistryTest, - TrackedCameraTakesPrecedenceOverInventoryFallback) -{ - CameraOperationalActivityRegistry registry; - auto tracked = std::make_shared("camera"); - auto fallback = std::make_shared("camera"); - ASSERT_EQ( - registry.start(tracked->id(), tracked), - CameraOperationalActivityRegistry::DispatchResult::Success); - ASSERT_TRUE(fallback->startOperationalActivity()); - - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - ASSERT_TRUE(ticket.valid()); - EXPECT_TRUE(registry.stopActivitiesForDevice( - tracked->id(), fallback)); - EXPECT_FALSE(tracked->operationalActive()); - EXPECT_EQ(tracked->operationalStopCalls(), 1); - EXPECT_TRUE(fallback->operationalActive()); - EXPECT_EQ(fallback->operationalStopCalls(), 0); - EXPECT_EQ(tracked->lifecycleStopCalls(), 0); - EXPECT_EQ(fallback->lifecycleStopCalls(), 0); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); -} - -TEST_F(CameraOperationalActivityRegistryTest, - ActiveDeviceIdCannotBeReboundToAnotherCameraInstance) -{ - CameraOperationalActivityRegistry registry; - auto original = std::make_shared("camera"); - auto replacement = std::make_shared("camera"); - ASSERT_EQ( - registry.start(original->id(), original), - CameraOperationalActivityRegistry::DispatchResult::Success); - - CameraOperationalActivityRegistry::ActivityToken replacement_token; - EXPECT_EQ( - registry.start( - replacement->id(), replacement, &replacement_token), - CameraOperationalActivityRegistry::DispatchResult::DeviceFailure); - EXPECT_FALSE(replacement_token.valid()); - EXPECT_TRUE(original->operationalActive()); - EXPECT_FALSE(replacement->operationalActive()); - EXPECT_EQ(replacement->operationalStartCalls(), 0); - - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - ASSERT_TRUE(ticket.valid()); - EXPECT_TRUE(registry.stopAllActivities()); - EXPECT_FALSE(original->operationalActive()); - EXPECT_EQ(original->operationalStopCalls(), 1); - EXPECT_EQ(replacement->operationalStopCalls(), 0); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); -} - -TEST_F(CameraOperationalActivityRegistryTest, - DifferentDevicesStopConcurrentlyAndSameDeviceStopIsSerialized) -{ - CameraOperationalActivityRegistry registry; - auto first = std::make_shared("camera.first"); - auto second = std::make_shared("camera.second"); - ASSERT_EQ( - registry.start(first->id(), first), - CameraOperationalActivityRegistry::DispatchResult::Success); - ASSERT_EQ( - registry.start(second->id(), second), - CameraOperationalActivityRegistry::DispatchResult::Success); - first->blockStop(); - second->blockStop(); - - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - std::thread first_stop([&] { - EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); - }); - first->waitForStopEntered(); - const auto snapshot_started = std::chrono::steady_clock::now(); - EXPECT_EQ( - registry.trackedDeviceIds(), - (std::vector{"camera.first", "camera.second"})); - EXPECT_LT( - std::chrono::steady_clock::now() - snapshot_started, - std::chrono::milliseconds(100)); - std::thread second_stop([&] { - EXPECT_TRUE(registry.stopActivitiesForDevice(second->id())); - }); - second->waitForStopEntered(); - - std::atomic matching_stop_returned{false}; - std::thread matching_stop([&] { - EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); - matching_stop_returned = true; - }); - std::this_thread::yield(); - EXPECT_FALSE(matching_stop_returned.load()); - - second->releaseStop(); - second_stop.join(); - first->releaseStop(); - first_stop.join(); - matching_stop.join(); - EXPECT_TRUE(matching_stop_returned.load()); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/camera_ptz_activity_registry_test.cpp b/cmvr-es/service/grpc/server/tests/camera_ptz_activity_registry_test.cpp deleted file mode 100644 index eec9ebe2..00000000 --- a/cmvr-es/service/grpc/server/tests/camera_ptz_activity_registry_test.cpp +++ /dev/null @@ -1,349 +0,0 @@ -#include "service/grpc/server/include/camera_ptz_activity_registry.h" - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { -namespace { - -class TestCamera final : public device::AbstractCamera { -public: - struct Call { - device::PtzCommand command; - bool stop; - int speed; - }; - - explicit TestCamera(std::string id) - { - id_ = std::move(id); - } - - std::string typeName() const override { return "TestCamera"; } - void getState(device::CameraState& state) override { state = {}; } - - bool controlPtz( - const device::PtzCommand command, - const bool stop, - const int speed) override - { - std::unique_lock lock(mutex_); - calls_.push_back({command, stop, speed}); - if (stop) { - stop_entered_ = true; - condition_.notify_all(); - condition_.wait(lock, [this] { return !block_stop_; }); - } - return !(stop && fail_stop_); - } - - bool stop() override - { - ++lifecycle_stop_calls_; - return true; - } - - std::vector calls() const - { - std::lock_guard lock(mutex_); - return calls_; - } - - void setFailStop(const bool fail) { fail_stop_ = fail; } - void blockStop() - { - std::lock_guard lock(mutex_); - block_stop_ = true; - stop_entered_ = false; - } - void waitForStopEntered() - { - std::unique_lock lock(mutex_); - condition_.wait(lock, [this] { return stop_entered_; }); - } - void releaseStop() - { - { - std::lock_guard lock(mutex_); - block_stop_ = false; - } - condition_.notify_all(); - } - int lifecycleStopCalls() const { return lifecycle_stop_calls_; } - -private: - mutable std::mutex mutex_; - std::condition_variable condition_; - std::vector calls_; - bool fail_stop_{false}; - bool block_stop_{false}; - bool stop_entered_{false}; - int lifecycle_stop_calls_{0}; -}; - -class CameraPtzActivityRegistryTest : public ::testing::Test { -protected: - void SetUp() override - { - globalStopAllAdmissionGate().clearForTesting(); - } - - void TearDown() override - { - globalStopAllAdmissionGate().clearForTesting(); - } -}; - -TEST_F(CameraPtzActivityRegistryTest, - StopAllMatchesEveryActiveStartAndNeverStopsLifecycle) -{ - CameraPtzActivityRegistry registry; - auto camera = std::make_shared("camera"); - - EXPECT_EQ( - registry.control( - camera->id(), camera, device::PtzCommand::PanLeft, false, 4), - CameraPtzActivityRegistry::DispatchResult::Success); - EXPECT_EQ( - registry.control( - camera->id(), camera, device::PtzCommand::ZoomIn, false, 7), - CameraPtzActivityRegistry::DispatchResult::Success); - - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - ASSERT_TRUE(ticket.valid()); - EXPECT_TRUE(registry.stopAllActivities()); - EXPECT_EQ(registry.activeCommandCount(), 0U); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - ASSERT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); - - const auto calls = camera->calls(); - ASSERT_EQ(calls.size(), 4U); - EXPECT_FALSE(calls[0].stop); - EXPECT_FALSE(calls[1].stop); - EXPECT_TRUE(calls[2].stop); - EXPECT_TRUE(calls[3].stop); -} - -TEST_F(CameraPtzActivityRegistryTest, - FailedStopRemainsTrackedForRetryAndAdmissionRecovers) -{ - CameraPtzActivityRegistry registry; - auto camera = std::make_shared("camera"); - ASSERT_EQ( - registry.control( - camera->id(), camera, device::PtzCommand::TiltUp, false, 3), - CameraPtzActivityRegistry::DispatchResult::Success); - - auto ticket = globalStopAllAdmissionGate().beginStopAll(); - camera->setFailStop(true); - std::vector failures; - EXPECT_FALSE(registry.stopAllActivities(&failures)); - EXPECT_EQ(registry.activeCommandCount(), 1U); - ASSERT_FALSE(failures.empty()); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, false) == false); - - EXPECT_EQ( - registry.control( - camera->id(), camera, device::PtzCommand::ZoomOut, false, 5), - CameraPtzActivityRegistry::DispatchResult::RejectedByStopAll); - - ticket = globalStopAllAdmissionGate().beginStopAll(); - camera->setFailStop(false); - EXPECT_TRUE(registry.stopAllActivities()); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); - EXPECT_EQ( - registry.control( - camera->id(), camera, device::PtzCommand::ZoomOut, false, 5), - CameraPtzActivityRegistry::DispatchResult::Success); -} - -TEST_F(CameraPtzActivityRegistryTest, - ExplicitStopRemainsAvailableWhileStartAdmissionIsLatched) -{ - CameraPtzActivityRegistry registry; - auto camera = std::make_shared("camera"); - ASSERT_EQ( - registry.control( - camera->id(), camera, device::PtzCommand::PanLeft, false, 4), - CameraPtzActivityRegistry::DispatchResult::Success); - - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - ASSERT_TRUE(ticket.valid()); - EXPECT_FALSE(globalStopAllAdmissionGate().finishStopAll(ticket, false)); - EXPECT_EQ( - registry.control( - camera->id(), camera, device::PtzCommand::ZoomIn, false, 4), - CameraPtzActivityRegistry::DispatchResult::RejectedByStopAll); - EXPECT_EQ( - registry.control( - camera->id(), camera, device::PtzCommand::PanLeft, true, 4), - CameraPtzActivityRegistry::DispatchResult::Success); - EXPECT_EQ(registry.activeCommandCount(), 0U); - - const auto calls = camera->calls(); - ASSERT_EQ(calls.size(), 2U); - EXPECT_FALSE(calls.front().stop); - EXPECT_TRUE(calls.back().stop); -} - -TEST_F(CameraPtzActivityRegistryTest, - StopForDeviceIsSelectiveAndActiveIdsReflectFailures) -{ - CameraPtzActivityRegistry registry; - auto first = std::make_shared("camera.first"); - auto second = std::make_shared("camera.second"); - ASSERT_EQ( - registry.control( - first->id(), first, device::PtzCommand::PanLeft, false, 4), - CameraPtzActivityRegistry::DispatchResult::Success); - ASSERT_EQ( - registry.control( - second->id(), second, device::PtzCommand::ZoomIn, false, 7), - CameraPtzActivityRegistry::DispatchResult::Success); - EXPECT_EQ( - registry.activeDeviceIds(), - (std::vector{"camera.first", "camera.second"})); - EXPECT_EQ( - registry.trackedDeviceIds(), - (std::vector{"camera.first", "camera.second"})); - - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - first->setFailStop(true); - std::vector failures; - EXPECT_FALSE(registry.stopActivitiesForDevice(first->id(), &failures)); - EXPECT_EQ(registry.activeCommandCount(), 2U); - EXPECT_EQ(failures.size(), 1U); - EXPECT_EQ(second->calls().size(), 1U); - - first->setFailStop(false); - EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); - EXPECT_EQ(registry.activeCommandCount(), 1U); - EXPECT_EQ( - registry.activeDeviceIds(), - (std::vector{"camera.second"})); - EXPECT_TRUE(registry.stopActivitiesForDevice("missing")); - EXPECT_TRUE(registry.stopAllActivities()); - EXPECT_TRUE(registry.activeDeviceIds().empty()); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); - EXPECT_EQ(first->lifecycleStopCalls(), 0); - EXPECT_EQ(second->lifecycleStopCalls(), 0); -} - -TEST_F(CameraPtzActivityRegistryTest, - DifferentDevicesStopConcurrentlyButMatchingControlIsSerialized) -{ - CameraPtzActivityRegistry registry; - auto first = std::make_shared("camera.first"); - auto second = std::make_shared("camera.second"); - ASSERT_EQ( - registry.control( - first->id(), first, device::PtzCommand::PanLeft, false, 4), - CameraPtzActivityRegistry::DispatchResult::Success); - ASSERT_EQ( - registry.control( - second->id(), second, device::PtzCommand::ZoomIn, false, 7), - CameraPtzActivityRegistry::DispatchResult::Success); - first->blockStop(); - second->blockStop(); - - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - std::thread first_stop([&] { - EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); - }); - first->waitForStopEntered(); - const auto snapshot_started = std::chrono::steady_clock::now(); - EXPECT_EQ( - registry.trackedDeviceIds(), - (std::vector{"camera.first", "camera.second"})); - EXPECT_LT( - std::chrono::steady_clock::now() - snapshot_started, - std::chrono::milliseconds(100)); - std::thread second_stop([&] { - EXPECT_TRUE(registry.stopActivitiesForDevice(second->id())); - }); - second->waitForStopEntered(); - - std::atomic matching_stop_returned{false}; - std::thread matching_stop([&] { - EXPECT_TRUE(registry.stopActivitiesForDevice(first->id())); - matching_stop_returned = true; - }); - std::this_thread::yield(); - EXPECT_FALSE(matching_stop_returned.load()); - - second->releaseStop(); - second_stop.join(); - first->releaseStop(); - first_stop.join(); - matching_stop.join(); - EXPECT_TRUE(matching_stop_returned.load()); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); -} - -TEST_F(CameraPtzActivityRegistryTest, - DispatchFenceRunsInsidePerDeviceQueueBeforeHardwareMutation) -{ - CameraPtzActivityRegistry registry; - auto camera = std::make_shared("camera"); - std::mutex mutex; - std::condition_variable condition; - bool first_fence_entered = false; - bool release_first_fence = false; - std::atomic second_fence_entered{false}; - - std::thread first([&] { - EXPECT_EQ( - registry.control( - camera->id(), camera, device::PtzCommand::PanLeft, false, 4, - [&] { - std::unique_lock lock(mutex); - first_fence_entered = true; - condition.notify_all(); - condition.wait(lock, [&] { return release_first_fence; }); - return true; - }), - CameraPtzActivityRegistry::DispatchResult::Success); - }); - { - std::unique_lock lock(mutex); - condition.wait(lock, [&] { return first_fence_entered; }); - } - - std::thread second([&] { - EXPECT_EQ( - registry.control( - camera->id(), camera, device::PtzCommand::ZoomIn, false, 7, - [&] { - second_fence_entered = true; - return false; - }), - CameraPtzActivityRegistry::DispatchResult::RejectedByDispatchFence); - }); - std::this_thread::yield(); - EXPECT_FALSE(second_fence_entered.load()); - - { - std::lock_guard lock(mutex); - release_first_fence = true; - } - condition.notify_all(); - first.join(); - second.join(); - - EXPECT_TRUE(second_fence_entered.load()); - const auto calls = camera->calls(); - ASSERT_EQ(calls.size(), 1U); - EXPECT_EQ(calls.front().command, device::PtzCommand::PanLeft); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_agv_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_agv_service_test.cpp deleted file mode 100644 index a59e2e13..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_agv_service_test.cpp +++ /dev/null @@ -1,806 +0,0 @@ -#include "service/grpc/server/include/grpc_agv_service.h" - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" -#include "manager/device_manager/include/device_manager.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { -namespace { - -constexpr char kNativeErrorMessage[] = - "SEER Robokit command failed: ret_code=41200, err_msg=speed_illegal"; -constexpr char kNativeNavigationErrorMessage[] = - "SEER Robokit command failed: ret_code=43051, err_msg=planner_rejected_pose"; - -class FakeAgv final : public device::AbstractAGV { -public: - FakeAgv() - { - id_ = "test-agv"; - } - - std::string typeName() const override { return "FakeAgv"; } - - device::AgvResult emergencyStop() override - { - ++emergency_stop_calls_; - emergency_stop_barrier_observed_ = normalLeaseIsBlocked_( - "test-probe:emergencyStop"); - return device::AgvResult::success(); - } - - device::AgvResult navigateToPose( - const math::Pose2d& pose, - const device::AgvMotionOptions& options, - const device::AgvAdapterParams&) override - { - ++navigate_pose_calls_; - pose_cancellation_bound_ = - static_cast(options.cancellation_requested); - pose_cancellation_requested_during_call_ = - pose_cancellation_bound_ && options.cancellation_requested(); - if (pose_cancellation_requested_during_call_) { - return device::AgvResult::failure( - device::AgvErrorCode::TaskCanceled, - "navigation canceled before fake device dispatch"); - } - pose_ = pose; - pose_options_ = options; - pose_options_.cancellation_requested = {}; - return pose_result_; - } - - device::AgvResult navigateToStation( - const std::string& station_id, - const device::AgvMotionOptions& options, - const device::AgvAdapterParams& adapter_params) override - { - station_id_ = station_id; - station_options_ = options; - station_adapter_params_ = adapter_params; - station_cancellation_bound_ = - static_cast(options.cancellation_requested); - station_cancellation_requested_during_call_ = - station_cancellation_bound_ && options.cancellation_requested(); - station_options_.cancellation_requested = {}; - return device::AgvResult::success(); - } - - device::AgvResult followPath( - const std::vector& path, - const device::AgvMotionOptions& options) override - { - path_ = path; - path_options_ = options; - path_cancellation_bound_ = - static_cast(options.cancellation_requested); - path_cancellation_requested_during_call_ = - path_cancellation_bound_ && options.cancellation_requested(); - path_options_.cancellation_requested = {}; - return device::AgvResult::success(); - } - - device::AgvResult setVelocity(const device::AgvVelocity&) override - { - ++set_velocity_calls_; - return device::AgvResult::failure( - device::AgvErrorCode::CommandFailed, - kNativeErrorMessage); - } - - device::AgvResult cancelNavigation() override - { - ++cancel_navigation_calls_; - cancel_navigation_barrier_observed_ = normalLeaseIsBlocked_( - "test-probe:cancelNavigation"); - return device::AgvResult::success(); - } - - device::AgvResult stopVelocityControl() override - { - ++stop_velocity_calls_; - stop_velocity_barrier_observed_ = normalLeaseIsBlocked_( - "test-probe:stopVelocityControl"); - return device::AgvResult::success(); - } - - device::AgvResult confirmMotionStopped() override - { - ++confirm_stopped_calls_; - confirm_stopped_barrier_observed_ = normalLeaseIsBlocked_( - "test-probe:confirmMotionStopped"); - return confirm_stopped_result_; - } - - bool normalLeaseIsBlocked_(const std::string& owner) - { - auto& authority = control::ControlAuthorityManager::instance(); - const auto probe = authority.tryAcquire( - id_, owner, std::chrono::hours(1)); - if (probe.acquired) { - authority.release(probe.token); - } - return !probe.acquired; - } - - math::Pose2d pose_{}; - device::AgvMotionOptions pose_options_; - device::AgvResult pose_result_{device::AgvResult::success()}; - std::string station_id_; - device::AgvMotionOptions station_options_; - device::AgvAdapterParams station_adapter_params_; - std::vector path_; - device::AgvMotionOptions path_options_; - bool pose_cancellation_bound_{false}; - bool pose_cancellation_requested_during_call_{false}; - bool station_cancellation_bound_{false}; - bool station_cancellation_requested_during_call_{false}; - bool path_cancellation_bound_{false}; - bool path_cancellation_requested_during_call_{false}; - int emergency_stop_calls_{0}; - int cancel_navigation_calls_{0}; - int stop_velocity_calls_{0}; - int confirm_stopped_calls_{0}; - int set_velocity_calls_{0}; - int navigate_pose_calls_{0}; - bool emergency_stop_barrier_observed_{false}; - bool cancel_navigation_barrier_observed_{false}; - bool stop_velocity_barrier_observed_{false}; - bool confirm_stopped_barrier_observed_{false}; - device::AgvResult confirm_stopped_result_{ - device::AgvResult::success()}; -}; - -class LegacyFollowPathAgv final : public device::AbstractAGV { -public: - std::string typeName() const override { return "LegacyFollowPathAgv"; } - - device::AgvResult followPath( - const std::vector& path) override - { - path_ = path; - return device::AgvResult::success(); - } - - std::vector path_; -}; - -class GrpcAgvServiceTest : public ::testing::Test { -protected: - void SetUp() override - { - control::ControlAuthorityManager::instance().clear(); - globalStopAllAdmissionGate().clearForTesting(); - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - agv_ = std::make_shared(); - manager.registerDevice(agv_); - service_ = std::make_unique(); - } - - void TearDown() override - { - service_.reset(); - agv_.reset(); - device::DeviceManager::destroyInstance(); - control::ControlAuthorityManager::instance().clear(); - globalStopAllAdmissionGate().clearForTesting(); - } - - std::shared_ptr agv_; - std::unique_ptr service_; -}; - -void setMotionOptions(msgs::AgvMotionOptions* options) -{ - options->set_max_speed(0.4); - options->set_max_angular_speed(0.5); - options->set_max_acceleration(0.6); - options->set_max_angular_acceleration(0.7); - options->set_reach_distance(0.08); - options->set_reach_angle(0.09); - options->set_wait_timeout_ms(1234); - options->set_poll_interval_ms(55); -} - -void expectMotionOptions(const device::AgvMotionOptions& options) -{ - EXPECT_DOUBLE_EQ(options.max_speed, 0.4); - EXPECT_DOUBLE_EQ(options.max_angular_speed, 0.5); - EXPECT_DOUBLE_EQ(options.max_acceleration, 0.6); - EXPECT_DOUBLE_EQ(options.max_angular_acceleration, 0.7); - EXPECT_DOUBLE_EQ(options.reach_distance, 0.08); - EXPECT_DOUBLE_EQ(options.reach_angle, 0.09); - EXPECT_EQ(options.wait_timeout_ms, 1234); - EXPECT_EQ(options.poll_interval_ms, 55); - EXPECT_FALSE(options.asynchronous); -} - -TEST_F(GrpcAgvServiceTest, NavigationRpcsForwardSpeedAndAccelerationOptions) -{ - api::AgvNavigateToPoseCommand_Request pose_request; - pose_request.mutable_header()->set_device_id("test-agv"); - pose_request.mutable_pose()->set_x(1.0); - pose_request.mutable_pose()->set_y(2.0); - pose_request.mutable_pose()->set_theta(0.5); - setMotionOptions(pose_request.mutable_options()); - api::AgvNavigateToPoseCommand_Feedback pose_response; - grpc::ServerContext pose_context; - - const auto pose_status = service_->navigateToPose( - &pose_context, - &pose_request, - &pose_response); - - ASSERT_TRUE(pose_status.ok()) << pose_status.error_message(); - EXPECT_TRUE(pose_response.header().success()); - EXPECT_DOUBLE_EQ(agv_->pose_.x, 1.0); - EXPECT_DOUBLE_EQ(agv_->pose_.y, 2.0); - EXPECT_DOUBLE_EQ(agv_->pose_.theta, 0.5); - expectMotionOptions(agv_->pose_options_); - EXPECT_TRUE(agv_->pose_cancellation_bound_); - EXPECT_FALSE(agv_->pose_cancellation_requested_during_call_); - - api::AgvNavigateToStationCommand_Request station_request; - station_request.mutable_header()->set_device_id("test-agv"); - station_request.set_station_id("station-1"); - setMotionOptions(station_request.mutable_options()); - api::AgvNavigateToStationCommand_Feedback station_response; - grpc::ServerContext station_context; - - const auto station_status = service_->navigateToStation( - &station_context, - &station_request, - &station_response); - - ASSERT_TRUE(station_status.ok()) << station_status.error_message(); - EXPECT_TRUE(station_response.header().success()); - EXPECT_EQ(agv_->station_id_, "station-1"); - expectMotionOptions(agv_->station_options_); - EXPECT_TRUE(agv_->station_cancellation_bound_); - EXPECT_FALSE(agv_->station_cancellation_requested_during_call_); - - api::AgvFollowPathCommand_Request path_request; - path_request.mutable_header()->set_device_id("test-agv"); - auto* segment = path_request.add_path(); - segment->set_source_station("station-1"); - segment->set_target_station("station-2"); - setMotionOptions(path_request.mutable_options()); - api::AgvFollowPathCommand_Feedback path_response; - grpc::ServerContext path_context; - - const auto path_status = service_->followPath( - &path_context, - &path_request, - &path_response); - - ASSERT_TRUE(path_status.ok()) << path_status.error_message(); - EXPECT_TRUE(path_response.header().success()); - ASSERT_EQ(agv_->path_.size(), 1U); - EXPECT_EQ(agv_->path_[0].source_station, "station-1"); - EXPECT_EQ(agv_->path_[0].target_station, "station-2"); - expectMotionOptions(agv_->path_options_); - EXPECT_TRUE(agv_->path_cancellation_bound_); - EXPECT_FALSE(agv_->path_cancellation_requested_during_call_); -} - -TEST_F(GrpcAgvServiceTest, NativeControllerCodeIsReturnedInGrpcMessage) -{ - api::AgvSetVelocityCommand_Request request; - request.mutable_header()->set_device_id("test-agv"); - request.mutable_velocity()->set_vx(0.1); - api::AgvSetVelocityCommand_Feedback response; - grpc::ServerContext context; - - const auto status = service_->setVelocity( - &context, - &request, - &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_EQ(status.error_message(), kNativeErrorMessage); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.header().error_message(), kNativeErrorMessage); -} - -TEST_F(GrpcAgvServiceTest, NativeNavigationCodeIsReturnedInGrpcMessage) -{ - agv_->pose_result_ = device::AgvResult::failure( - device::AgvErrorCode::CommandFailed, - kNativeNavigationErrorMessage); - api::AgvNavigateToPoseCommand_Request request; - request.mutable_header()->set_device_id("test-agv"); - request.mutable_pose()->set_x(1.0); - request.mutable_pose()->set_y(2.0); - api::AgvNavigateToPoseCommand_Feedback response; - grpc::ServerContext context; - - const auto status = service_->navigateToPose( - &context, - &request, - &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_EQ(status.error_message(), kNativeNavigationErrorMessage); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ( - response.header().error_message(), - kNativeNavigationErrorMessage); - EXPECT_FALSE(agv_->pose_options_.asynchronous); - EXPECT_EQ(agv_->pose_options_.wait_timeout_ms, 0); - EXPECT_EQ(agv_->pose_options_.poll_interval_ms, 0); - EXPECT_TRUE(agv_->pose_cancellation_bound_); - EXPECT_FALSE(agv_->pose_cancellation_requested_during_call_); -} - -TEST_F(GrpcAgvServiceTest, ExplicitAsynchronousNavigationIsForwarded) -{ - api::AgvNavigateToStationCommand_Request request; - request.mutable_header()->set_device_id("test-agv"); - request.set_station_id("station-async"); - request.mutable_options()->set_asynchronous(true); - api::AgvNavigateToStationCommand_Feedback response; - grpc::ServerContext context; - - const auto status = service_->navigateToStation( - &context, - &request, - &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(agv_->station_options_.asynchronous); - EXPECT_TRUE(agv_->station_cancellation_bound_); - EXPECT_FALSE(agv_->station_cancellation_requested_during_call_); -} - -TEST_F(GrpcAgvServiceTest, ActionLeaseBlocksOrdinaryMutatingRpcs) -{ - auto& authority = control::ControlAuthorityManager::instance(); - const auto action_lease = authority.tryAcquire( - "test-agv", - "action-sequence:test-action", - std::chrono::hours(1)); - ASSERT_TRUE(action_lease.acquired) << action_lease.detail; - - api::AgvNavigateToPoseCommand_Request navigation_request; - navigation_request.mutable_header()->set_device_id("test-agv"); - api::AgvNavigateToPoseCommand_Feedback navigation_response; - grpc::ServerContext navigation_context; - const auto navigation_status = service_->navigateToPose( - &navigation_context, - &navigation_request, - &navigation_response); - EXPECT_EQ( - navigation_status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_FALSE(navigation_response.header().success()); - - api::CommandHeader_Request clear_fault_request; - clear_fault_request.set_device_id("test-agv"); - api::CommandHeader_Feedback clear_fault_response; - grpc::ServerContext clear_fault_context; - const auto clear_fault_status = service_->clearFault( - &clear_fault_context, - &clear_fault_request, - &clear_fault_response); - EXPECT_EQ( - clear_fault_status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_FALSE(clear_fault_response.success()); - - api::AgvMapCommand_Request switch_map_request; - switch_map_request.mutable_header()->set_device_id("test-agv"); - switch_map_request.set_map_name("map-1"); - api::AgvMapCommand_Feedback switch_map_response; - grpc::ServerContext switch_map_context; - const auto switch_map_status = service_->switchMap( - &switch_map_context, - &switch_map_request, - &switch_map_response); - EXPECT_EQ( - switch_map_status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_FALSE(switch_map_response.header().success()); - - authority.release(action_lease.token); -} - -TEST_F(GrpcAgvServiceTest, NavigationCancellationIncludesControlLease) -{ - auto& authority = control::ControlAuthorityManager::instance(); - const auto barrier = authority.preemptAcquire( - "test-agv", "stop-all-test", std::chrono::hours(1)); - ASSERT_TRUE(barrier.acquired) << barrier.detail; - - api::AgvNavigateToPoseCommand_Request request; - request.mutable_header()->set_device_id("test-agv"); - api::AgvNavigateToPoseCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->navigateToPose( - &context, &request, &response); - - EXPECT_EQ( - status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ(agv_->navigate_pose_calls_, 0); - authority.release(barrier.token); -} - -TEST_F(GrpcAgvServiceTest, SetVelocityDispatchRunsUnderControlFence) -{ - api::AgvSetVelocityCommand_Request request; - request.mutable_header()->set_device_id("test-agv"); - request.mutable_velocity()->set_vx(0.1); - api::AgvSetVelocityCommand_Feedback response; - grpc::ServerContext context; - - const auto status = service_->setVelocity( - &context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_EQ(agv_->set_velocity_calls_, 1); -} - -TEST_F(GrpcAgvServiceTest, - StopAllGateRejectsMutatingCommandsButAllowsReadsAndStops) -{ - auto& admission = globalStopAllAdmissionGate(); - const auto ticket = admission.beginStopAll(); - ASSERT_TRUE(ticket.valid()); - - api::AgvNavigateToPoseCommand_Request navigation_request; - navigation_request.mutable_header()->set_device_id("test-agv"); - api::AgvNavigateToPoseCommand_Feedback navigation_response; - grpc::ServerContext navigation_context; - const auto navigation_status = service_->navigateToPose( - &navigation_context, &navigation_request, &navigation_response); - - api::AgvSetVelocityCommand_Request velocity_request; - velocity_request.mutable_header()->set_device_id("test-agv"); - velocity_request.mutable_velocity()->set_vx(0.1); - api::AgvSetVelocityCommand_Feedback velocity_response; - grpc::ServerContext velocity_context; - const auto velocity_status = service_->setVelocity( - &velocity_context, &velocity_request, &velocity_response); - - api::AgvRuntimeStateCommand_Request state_request; - state_request.mutable_header()->set_device_id("test-agv"); - api::AgvRuntimeStateCommand_Feedback state_response; - grpc::ServerContext state_context; - const auto state_status = service_->getRuntimeState( - &state_context, &state_request, &state_response); - - api::CommandHeader_Request stop_request; - stop_request.set_device_id("test-agv"); - api::CommandHeader_Feedback stop_response; - grpc::ServerContext stop_context; - const auto stop_status = service_->cancelNavigation( - &stop_context, &stop_request, &stop_response); - - EXPECT_EQ( - navigation_status.error_code(), - grpc::StatusCode::UNAVAILABLE); - EXPECT_FALSE(navigation_response.header().success()); - EXPECT_EQ(agv_->navigate_pose_calls_, 0); - EXPECT_EQ( - velocity_status.error_code(), - grpc::StatusCode::UNAVAILABLE); - EXPECT_FALSE(velocity_response.header().success()); - EXPECT_EQ(agv_->set_velocity_calls_, 0); - - EXPECT_TRUE(state_status.ok()) << state_status.error_message(); - EXPECT_TRUE(state_response.header().success()); - EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_TRUE(stop_response.success()) - << stop_response.error_message(); - EXPECT_EQ(agv_->cancel_navigation_calls_, 2); - - EXPECT_TRUE(admission.finishStopAll(ticket, true)); - const auto resumed_status = service_->navigateToPose( - &navigation_context, &navigation_request, &navigation_response); - EXPECT_TRUE(resumed_status.ok()) << resumed_status.error_message(); - EXPECT_TRUE(navigation_response.header().success()); - EXPECT_EQ(agv_->navigate_pose_calls_, 1); -} - -TEST_F(GrpcAgvServiceTest, QueriesBypassAndSafetyStopsPreemptActionLease) -{ - auto& authority = control::ControlAuthorityManager::instance(); - const auto action_lease = authority.tryAcquire( - "test-agv", - "action-sequence:test-action", - std::chrono::hours(1)); - ASSERT_TRUE(action_lease.acquired) << action_lease.detail; - - api::AgvRuntimeStateCommand_Request state_request; - state_request.mutable_header()->set_device_id("test-agv"); - api::AgvRuntimeStateCommand_Feedback state_response; - grpc::ServerContext state_context; - const auto state_status = service_->getRuntimeState( - &state_context, - &state_request, - &state_response); - EXPECT_TRUE(state_status.ok()) << state_status.error_message(); - EXPECT_TRUE(state_response.header().success()); - EXPECT_TRUE(authority.validate(action_lease.token)); - - api::CommandHeader_Request stop_request; - stop_request.set_device_id("test-agv"); - - api::CommandHeader_Feedback emergency_response; - grpc::ServerContext emergency_context; - auto emergency = std::async( - std::launch::async, - [this, &emergency_context, &stop_request, &emergency_response]() { - return service_->emergencyStop( - &emergency_context, - &stop_request, - &emergency_response); - }); - const auto emergency_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (authority.validate(action_lease.token) && - std::chrono::steady_clock::now() < emergency_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_FALSE(authority.validate(action_lease.token)); - authority.release(action_lease.token); - const auto emergency_status = emergency.get(); - EXPECT_TRUE(emergency_status.ok()) << emergency_status.error_message(); - EXPECT_TRUE(agv_->emergency_stop_barrier_observed_); - - const auto released_lease_after_emergency = authority.tryAcquire( - "test-agv", - "action-sequence:after-emergency-release", - std::chrono::hours(1)); - ASSERT_TRUE(released_lease_after_emergency.acquired) - << released_lease_after_emergency.detail; - - api::CommandHeader_Feedback cancel_response; - grpc::ServerContext cancel_context; - auto cancel = std::async( - std::launch::async, - [this, &cancel_context, &stop_request, &cancel_response]() { - return service_->cancelNavigation( - &cancel_context, - &stop_request, - &cancel_response); - }); - const auto cancel_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (authority.validate(released_lease_after_emergency.token) && - std::chrono::steady_clock::now() < cancel_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_FALSE(authority.validate( - released_lease_after_emergency.token)); - authority.release(released_lease_after_emergency.token); - const auto cancel_status = cancel.get(); - EXPECT_TRUE(cancel_status.ok()) << cancel_status.error_message(); - EXPECT_TRUE(agv_->cancel_navigation_barrier_observed_); - - const auto released_lease_after_cancel = authority.tryAcquire( - "test-agv", - "action-sequence:after-cancel-release", - std::chrono::hours(1)); - ASSERT_TRUE(released_lease_after_cancel.acquired) - << released_lease_after_cancel.detail; - - api::CommandHeader_Feedback velocity_response; - grpc::ServerContext velocity_context; - auto velocity = std::async( - std::launch::async, - [this, &velocity_context, &stop_request, &velocity_response]() { - return service_->stopVelocityControl( - &velocity_context, - &stop_request, - &velocity_response); - }); - const auto velocity_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (authority.validate(released_lease_after_cancel.token) && - std::chrono::steady_clock::now() < velocity_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_FALSE(authority.validate( - released_lease_after_cancel.token)); - authority.release(released_lease_after_cancel.token); - const auto velocity_status = velocity.get(); - EXPECT_TRUE(velocity_status.ok()) << velocity_status.error_message(); - EXPECT_TRUE(agv_->stop_velocity_barrier_observed_); - - const auto lease_after_stops = authority.tryAcquire( - "test-agv", - "action-sequence:after-stops", - std::chrono::hours(1)); - ASSERT_TRUE(lease_after_stops.acquired) - << lease_after_stops.detail; - - EXPECT_EQ(agv_->emergency_stop_calls_, 2); - EXPECT_EQ(agv_->cancel_navigation_calls_, 2); - EXPECT_EQ(agv_->stop_velocity_calls_, 2); - EXPECT_EQ(agv_->confirm_stopped_calls_, 3); - EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_); - - authority.release(lease_after_stops.token); -} - -TEST_F(GrpcAgvServiceTest, - SafetyStopQuarantinesControlWhenStoppedStateIsUnconfirmed) -{ - agv_->confirm_stopped_result_ = device::AgvResult::failure( - device::AgvErrorCode::Timeout, - "two zero-velocity samples were not observed"); - - api::CommandHeader_Request request; - request.set_device_id("test-agv"); - api::CommandHeader_Feedback response; - grpc::ServerContext context; - const auto status = service_->cancelNavigation( - &context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); - EXPECT_FALSE(response.success()); - EXPECT_NE( - response.error_message().find("confirmed stopped state"), - std::string::npos); - EXPECT_TRUE(agv_->cancel_navigation_barrier_observed_); - EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_); - - const auto lease = - control::ControlAuthorityManager::instance().tryAcquire( - "test-agv", - "normal-control-after-unconfirmed-stop", - std::chrono::hours(1)); - EXPECT_FALSE(lease.acquired); - - auto& authority = control::ControlAuthorityManager::instance(); - const auto recovery = authority.preemptAcquire( - "test-agv", "confirmed-stop-recovery", std::chrono::hours(1)); - ASSERT_TRUE(recovery.acquired) << recovery.detail; - ASSERT_TRUE(authority.waitForPreemptedRelease( - recovery.token, std::chrono::milliseconds::zero())); - ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token)); - authority.release(recovery.token); - const auto recovered = authority.tryAcquire( - "test-agv", "normal-control-after-recovery", std::chrono::hours(1)); - EXPECT_TRUE(recovered.acquired) << recovered.detail; - authority.release(recovered.token); -} - -TEST_F(GrpcAgvServiceTest, StopMappingFailureReleasesOrdinaryControlLease) -{ - api::CommandHeader_Request request; - request.set_device_id("test-agv"); - api::CommandHeader_Feedback response; - grpc::ServerContext context; - - const auto status = service_->stopMapping( - &context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_FALSE(response.success()); - auto& authority = control::ControlAuthorityManager::instance(); - const auto lease = authority.tryAcquire( - "test-agv", "normal-control-after-mapping-failure", - std::chrono::hours(1)); - EXPECT_TRUE(lease.acquired) << lease.detail; - authority.release(lease.token); -} - -TEST_F(GrpcAgvServiceTest, NavigateToStationForwardsPgvAdapterParams) -{ - api::AgvNavigateToStationCommand_Request request; - request.mutable_header()->set_device_id("test-agv"); - request.set_station_id("AP1"); - auto* values = request.mutable_adapter_params()->mutable_values(); - (*values)["use_pgv"] = "true"; - (*values)["pgv_adjust_dist"] = "0.3"; - (*values)["pgv_adjust_cx"] = "-0.3"; - (*values)["pgv_adjust_cy"] = "0"; - api::AgvNavigateToStationCommand_Feedback response; - grpc::ServerContext context; - - const auto status = service_->navigateToStation( - &context, - &request, - &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()); - EXPECT_EQ(agv_->station_id_, "AP1"); - EXPECT_EQ( - agv_->station_adapter_params_.getString("use_pgv").value_or(""), - "true"); - EXPECT_EQ( - agv_->station_adapter_params_.getString("pgv_adjust_dist").value_or(""), - "0.3"); - EXPECT_EQ( - agv_->station_adapter_params_.getString("pgv_adjust_cx").value_or(""), - "-0.3"); - EXPECT_EQ( - agv_->station_adapter_params_.getString("pgv_adjust_cy").value_or(""), - "0"); -} - -TEST_F(GrpcAgvServiceTest, NavigationErrorsMapToGrpcCodesAndPreserveDetails) -{ - struct ErrorCase { - device::AgvErrorCode device_code; - grpc::StatusCode grpc_code; - }; - const ErrorCase cases[] = { - {device::AgvErrorCode::InvalidArgument, - grpc::StatusCode::INVALID_ARGUMENT}, - {device::AgvErrorCode::TaskCanceled, - grpc::StatusCode::CANCELLED}, - {device::AgvErrorCode::Timeout, - grpc::StatusCode::DEADLINE_EXCEEDED}, - }; - - for (const auto& test_case : cases) { - const std::string detail = - "SEER Robokit navigation detail for code=" - + std::to_string(static_cast(test_case.device_code)); - agv_->pose_result_ = device::AgvResult::failure( - test_case.device_code, - detail); - api::AgvNavigateToPoseCommand_Request request; - request.mutable_header()->set_device_id("test-agv"); - api::AgvNavigateToPoseCommand_Feedback response; - grpc::ServerContext context; - - const auto status = service_->navigateToPose( - &context, - &request, - &response); - - EXPECT_EQ(status.error_code(), test_case.grpc_code); - EXPECT_EQ(status.error_message(), detail); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.header().error_message(), detail); - } -} - -TEST(AbstractAgvCompatibilityTest, FollowPathOptionsDelegateToLegacyOverride) -{ - LegacyFollowPathAgv legacy; - device::AbstractAGV* abstract = &legacy; - const std::vector path = { - {"station-1", "station-2"}, - }; - device::AgvMotionOptions options; - - const auto result = abstract->followPath(path, options); - - ASSERT_TRUE(result.ok()) << result.message; - ASSERT_EQ(legacy.path_.size(), 1U); - EXPECT_EQ(legacy.path_[0].source_station, "station-1"); - EXPECT_EQ(legacy.path_[0].target_station, "station-2"); -} - -TEST(AbstractAgvCompatibilityTest, SynchronousActionSupportDefaultsToFalse) -{ - LegacyFollowPathAgv legacy; - - EXPECT_FALSE(legacy.supportsSynchronousAction( - device::AgvActionKind::NavigateToPose)); - EXPECT_FALSE(legacy.supportsSynchronousAction( - device::AgvActionKind::NavigateToStation)); - EXPECT_FALSE(legacy.supportsSynchronousAction( - device::AgvActionKind::FollowPath)); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_arm_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_arm_service_test.cpp deleted file mode 100644 index 980d5b7f..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_arm_service_test.cpp +++ /dev/null @@ -1,1236 +0,0 @@ -#include "service/grpc/server/include/grpc_arm_service.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" -#include "manager/device_manager/include/device_manager.h" -#include "service/grpc/server/include/grpc_error_logging_interceptor.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { -namespace { - -class JsonCommandRobotArm final : public device::RobotArm { -public: - explicit JsonCommandRobotArm(std::string id) - { - id_ = std::move(id); - } - - std::string typeName() const override { return "JsonCommandRobotArm"; } - - bool executeJsonCommand(const std::string& request_json, - std::string& response_json) override - { - ++execute_calls; - last_request_json = request_json; - response_json = next_response_json; - return next_success; - } - - device::RobotModel getRobotModel() const override { return {}; } - std::size_t getDof() const override { return 0U; } - device::ArmState getRobotState() const override { return {}; } - device::JointGroupState getJointState() const override { return {}; } - device::CartesianPose getTcpPose( - device::FrameType = device::FrameType::Base) const override - { - return {}; - } - device::RobotMode getRobotMode() const override - { - return device::RobotMode::Unknown; - } - device::SafetyMode getSafetyMode() const override - { - return device::SafetyMode::Unknown; - } - device::ControlMode getControlMode() const override - { - return device::ControlMode::None; - } - - device::Result torqueOn() override { return torqueOn({}); } - device::Result torqueOn( - const std::function& cancellation_requested) override - { - std::unique_lock lock(motion_mutex_); - ++torque_on_calls_; - last_torque_on_had_cancellation_ = - static_cast(cancellation_requested); - if (!block_next_torque_on_) { - return device::Result::success(); - } - - block_next_torque_on_ = false; - blocking_torque_on_started_ = true; - torque_on_started_cv_.notify_all(); - while (!release_blocking_torque_on_) { - lock.unlock(); - const bool cancelled = - cancellation_requested && cancellation_requested(); - lock.lock(); - if (cancelled) { - last_torque_on_cancellation_requested_ = true; - if (fail_next_torque_on_cancellation_) { - fail_next_torque_on_cancellation_ = false; - return device::Result::failure( - device::ArmErrorCode::CommandFailed, - "simulated torqueOn safety termination failure"); - } - return device::Result::failure( - device::ArmErrorCode::CommandRejected, - "simulated torqueOn cancellation"); - } - torque_on_release_cv_.wait_for( - lock, std::chrono::milliseconds(5)); - } - return device::Result::success(); - } - device::Result torqueOff() override - { - std::lock_guard lock(motion_mutex_); - ++torque_off_calls_; - return device::Result::success(); - } - device::Result calibrateZeroQ(const std::string&) override - { - return device::Result::success(); - } - device::Result emergencyStop() override - { - return device::Result::success(); - } - device::Result protectiveStop() override - { - return device::Result::success(); - } - device::Result setSpeedScaling(double) override - { - return device::Result::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; } - - device::Result moveJ(const device::JointPositionCommand&, - const device::MotionOptions& options) override - { - return enterMotion("moveJ", move_j_calls_, options); - } - device::Result speedJ(const device::JointVelocityCommand&, - double, - double) override - { - return device::Result::success(); - } - device::Result stopJ(double) override - { - return device::Result::success(); - } - device::Result moveL( - const device::CartesianPose&, - const device::MotionOptions& options, - device::FrameType = device::FrameType::Base) override - { - return enterMotion("moveL", move_l_calls_, options); - } - device::Result speedL( - const device::CartesianVelocity&, - double, - double, - device::FrameType = device::FrameType::Base) override - { - return device::Result::success(); - } - device::Result stopL(std::optional = std::nullopt) override - { - return device::Result::success(); - } - device::Result stopMotion() override - { - std::unique_lock lock(motion_mutex_); - ++stop_motion_calls_; - if (block_next_stop_) { - block_next_stop_ = false; - blocking_stop_started_ = true; - stop_started_cv_.notify_all(); - stop_release_cv_.wait( - lock, - [this]() { return release_blocking_stop_; }); - } - if (throw_next_stop_) { - throw_next_stop_ = false; - throw std::runtime_error("simulated stopMotion exception"); - } - if (fail_next_stop_) { - fail_next_stop_ = false; - return device::Result::failure( - device::ArmErrorCode::CommandFailed, - "simulated stopMotion failure"); - } - return device::Result::success(); - } - - void blockNextMotion() - { - std::lock_guard lock(motion_mutex_); - block_next_motion_ = true; - blocking_motion_started_ = false; - release_blocking_motion_ = false; - blocking_motion_name_.clear(); - } - - void blockNextTorqueOn() - { - std::lock_guard lock(motion_mutex_); - block_next_torque_on_ = true; - blocking_torque_on_started_ = false; - release_blocking_torque_on_ = false; - last_torque_on_cancellation_requested_ = false; - } - - void failNextTorqueOnCancellation() - { - std::lock_guard lock(motion_mutex_); - fail_next_torque_on_cancellation_ = true; - } - - bool waitForBlockingTorqueOn( - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(motion_mutex_); - return torque_on_started_cv_.wait_for( - lock, - timeout, - [this]() { return blocking_torque_on_started_; }); - } - - void releaseBlockingTorqueOn() - { - { - std::lock_guard lock(motion_mutex_); - release_blocking_torque_on_ = true; - } - torque_on_release_cv_.notify_all(); - } - - bool waitForBlockingMotion( - const std::string& operation, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(motion_mutex_); - return motion_started_cv_.wait_for( - lock, - timeout, - [this, &operation]() { - return blocking_motion_started_ && - blocking_motion_name_ == operation; - }); - } - - void releaseBlockingMotion() - { - { - std::lock_guard lock(motion_mutex_); - release_blocking_motion_ = true; - } - motion_release_cv_.notify_all(); - } - - void blockNextStopMotion() - { - std::lock_guard lock(motion_mutex_); - block_next_stop_ = true; - blocking_stop_started_ = false; - release_blocking_stop_ = false; - } - - void failNextStopMotion() - { - std::lock_guard lock(motion_mutex_); - fail_next_stop_ = true; - } - - void throwNextStopMotion() - { - std::lock_guard lock(motion_mutex_); - throw_next_stop_ = true; - } - - bool waitForBlockingStop(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(motion_mutex_); - return stop_started_cv_.wait_for( - lock, - timeout, - [this]() { return blocking_stop_started_; }); - } - - void releaseBlockingStop() - { - { - std::lock_guard lock(motion_mutex_); - release_blocking_stop_ = true; - } - stop_release_cv_.notify_all(); - } - - int moveJCalls() const - { - std::lock_guard lock(motion_mutex_); - return move_j_calls_; - } - - int moveLCalls() const - { - std::lock_guard lock(motion_mutex_); - return move_l_calls_; - } - - int stopMotionCalls() const - { - std::lock_guard lock(motion_mutex_); - return stop_motion_calls_; - } - - int torqueOffCalls() const - { - std::lock_guard lock(motion_mutex_); - return torque_off_calls_; - } - - int torqueOnCalls() const - { - std::lock_guard lock(motion_mutex_); - return torque_on_calls_; - } - - bool lastTorqueOnHadCancellation() const - { - std::lock_guard lock(motion_mutex_); - return last_torque_on_had_cancellation_; - } - - bool lastTorqueOnCancellationRequested() const - { - std::lock_guard lock(motion_mutex_); - return last_torque_on_cancellation_requested_; - } - - bool lastMotionHadCancellation() const - { - std::lock_guard lock(motion_mutex_); - return last_motion_had_cancellation_; - } - - bool lastMotionCancellationRequested() const - { - std::lock_guard lock(motion_mutex_); - return last_motion_cancellation_requested_; - } - - device::Result startServoMode(const device::ServoOptions&) override - { - return device::Result::success(); - } - device::Result servoJ(const device::JointPositionCommand&) override - { - return device::Result::success(); - } - device::Result servoL( - const device::CartesianPose&, - device::FrameType = device::FrameType::Base) override - { - return device::Result::success(); - } - device::Result servoSpeedJ(const device::JointVelocityCommand&) override - { - return device::Result::success(); - } - device::Result servoSpeedL( - const device::CartesianVelocity&, - device::FrameType = device::FrameType::Base) override - { - return device::Result::success(); - } - device::Result stopServoMode() override - { - return device::Result::success(); - } - - device::Result connect(const std::string&, int) override - { - return device::Result::success(); - } - device::Result disconnect() override - { - return device::Result::success(); - } - bool isConnected() const override { return true; } - device::Result powerOn() override { return device::Result::success(); } - device::Result powerOff() override { return device::Result::success(); } - device::Result brakeRelease() override - { - return device::Result::success(); - } - device::Result shutdown() override - { - return device::Result::success(); - } - device::Result clearFault() override - { - return device::Result::success(); - } - device::Result unlockProtectiveStop() override - { - return device::Result::success(); - } - device::Result loadProgram(const std::string&) override - { - return device::Result::success(); - } - device::Result playProgram() override - { - return device::Result::success(); - } - device::Result pauseProgram() override - { - return device::Result::success(); - } - device::Result stopProgram() override - { - return device::Result::success(); - } - - std::vector ik(const std::string&, - const std::string&, - const device::CartesianPose&) override - { - return {}; - } - std::shared_ptr kinematicsSolver() const override - { - return nullptr; - } - device::CartesianPose fk(const std::string&, - const std::string&) override - { - return {}; - } - device::CartesianPose fk(bool = true) override { return {}; } - device::CartesianVelocity getSpeedLCommandTwistBase() const override - { - return {}; - } - bool busy() const override { return false; } - - int execute_calls{0}; - bool next_success{true}; - std::string next_response_json; - std::string last_request_json; - -private: - device::Result enterMotion( - const char* operation, - int& call_count, - const device::MotionOptions& options) - { - std::unique_lock lock(motion_mutex_); - ++call_count; - last_motion_had_cancellation_ = - static_cast(options.cancellation_requested); - last_motion_cancellation_requested_ = - last_motion_had_cancellation_ && - options.cancellation_requested(); - if (!block_next_motion_) { - return device::Result::success(); - } - - block_next_motion_ = false; - blocking_motion_started_ = true; - blocking_motion_name_ = operation; - motion_started_cv_.notify_all(); - motion_release_cv_.wait( - lock, - [this]() { return release_blocking_motion_; }); - return device::Result::success(); - } - - mutable std::mutex motion_mutex_; - std::condition_variable motion_started_cv_; - std::condition_variable motion_release_cv_; - std::condition_variable stop_started_cv_; - std::condition_variable stop_release_cv_; - std::condition_variable torque_on_started_cv_; - std::condition_variable torque_on_release_cv_; - bool block_next_motion_{false}; - bool blocking_motion_started_{false}; - bool release_blocking_motion_{false}; - bool block_next_stop_{false}; - bool blocking_stop_started_{false}; - bool release_blocking_stop_{false}; - bool fail_next_stop_{false}; - bool throw_next_stop_{false}; - bool block_next_torque_on_{false}; - bool blocking_torque_on_started_{false}; - bool release_blocking_torque_on_{false}; - bool fail_next_torque_on_cancellation_{false}; - std::string blocking_motion_name_; - int move_j_calls_{0}; - int move_l_calls_{0}; - int stop_motion_calls_{0}; - int torque_off_calls_{0}; - int torque_on_calls_{0}; - bool last_torque_on_had_cancellation_{false}; - bool last_torque_on_cancellation_requested_{false}; - bool last_motion_had_cancellation_{false}; - bool last_motion_cancellation_requested_{false}; -}; - -class JsonCommandNonArmDevice final : public device::AbstractDevice { -public: - explicit JsonCommandNonArmDevice(std::string id) - : AbstractDevice(std::move(id)) - { - } - - std::string typeName() const override { return "JsonCommandNonArmDevice"; } - - bool executeJsonCommand(const std::string&, - std::string& response_json) override - { - ++execute_calls; - response_json = R"({"success":true})"; - return true; - } - - int execute_calls{0}; -}; - -class GrpcArmServiceTest : public ::testing::Test { -protected: - void SetUp() override - { - control::ControlAuthorityManager::instance().clear(); - globalStopAllAdmissionGate().clearForTesting(); - device::DeviceManager::destroyInstance(); - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - - left_arm_ = std::make_shared("left_arm"); - aubo_arm_ = std::make_shared("aubo_arm"); - non_arm_ = std::make_shared("camera"); - manager.registerDevice(left_arm_); - manager.registerDevice(aubo_arm_); - manager.registerDevice(non_arm_); - service_ = std::make_unique(); - } - - void TearDown() override - { - service_.reset(); - non_arm_.reset(); - aubo_arm_.reset(); - left_arm_.reset(); - device::DeviceManager::destroyInstance(); - control::ControlAuthorityManager::instance().clear(); - globalStopAllAdmissionGate().clearForTesting(); - } - - grpc::Status execute(const std::string& device_id, - const std::string& request_json, - api::JsonDeviceCommand_Feedback& response) - { - api::JsonDeviceCommand_Request request; - request.mutable_header()->set_device_id(device_id); - request.set_request_json(request_json); - grpc::ServerContext context; - return service_->ExecuteJsonCommand(&context, &request, &response); - } - - struct MoveOutcome { - grpc::Status status; - bool response_success{false}; - std::string response_error; - }; - - MoveOutcome moveJ(const std::string& device_id) - { - api::MoveJ_Request request; - request.mutable_header()->set_device_id(device_id); - request.mutable_target()->add_position(0.1); - api::MoveJ_Response response; - grpc::ServerContext context; - auto status = service_->moveJ(&context, &request, &response); - return { - std::move(status), - response.header().success(), - response.header().error_message()}; - } - - grpc::Status identifiedMoveJ( - const std::string& command_id, - const double position, - api::MoveJ_Response& response) - { - api::MoveJ_Request request; - auto* header = request.mutable_header(); - header->set_device_id("aubo_arm"); - header->set_command_id(command_id); - header->set_expected_service_instance_id( - device::DeviceManager::getInstance() - .safetyManager() - .serviceInstanceId()); - header->set_valid_for_ms(1000); - request.mutable_target()->add_position(position); - grpc::ServerContext context; - return service_->moveJ(&context, &request, &response); - } - - MoveOutcome moveL(const std::string& device_id) - { - api::MoveL_Request request; - request.mutable_header()->set_device_id(device_id); - request.mutable_target()->set_x(0.1); - api::MoveL_Response response; - grpc::ServerContext context; - auto status = service_->moveL(&context, &request, &response); - return { - std::move(status), - response.header().success(), - response.header().error_message()}; - } - - grpc::Status stopMotion( - const std::string& device_id, - api::CommandHeader_Feedback& response) - { - api::CommandHeader_Request request; - request.set_device_id(device_id); - grpc::ServerContext context; - return service_->stopMotion(&context, &request, &response); - } - - grpc::Status torqueOff( - const std::string& device_id, - api::CommandHeader_Feedback& response) - { - api::CommandHeader_Request request; - request.set_device_id(device_id); - grpc::ServerContext context; - return service_->torqueOff(&context, &request, &response); - } - - MoveOutcome torqueOn(const std::string& device_id) - { - api::CommandHeader_Request request; - request.set_device_id(device_id); - api::CommandHeader_Feedback response; - grpc::ServerContext context; - auto status = service_->torqueOn(&context, &request, &response); - return { - std::move(status), - response.success(), - response.error_message()}; - } - - std::shared_ptr left_arm_; - std::shared_ptr aubo_arm_; - std::shared_ptr non_arm_; - std::unique_ptr service_; -}; - -TEST(GrpcArmServiceDescriptorTest, - ExecuteJsonCommandBelongsOnlyToArmService) -{ - const auto* pool = google::protobuf::DescriptorPool::generated_pool(); - const auto* arm_service = - pool->FindServiceByName("cmvr.api.ArmService"); - const auto* system_service = - pool->FindServiceByName("cmvr.api.SystemService"); - - ASSERT_NE(arm_service, nullptr); - ASSERT_NE(system_service, nullptr); - EXPECT_NE(arm_service->FindMethodByName("ExecuteJsonCommand"), nullptr); - EXPECT_EQ(system_service->FindMethodByName("ExecuteJsonCommand"), nullptr); -} - -TEST_F(GrpcArmServiceTest, RoutesByHeaderDeviceIdAndForwardsSuccessfulJson) -{ - const std::string request_json = - R"({"command":"cabinet_io","operation":"get_di","index":0})"; - const std::string response_json = - R"({"success":true,"operation":"get_di","index":0,"value":false})"; - aubo_arm_->next_response_json = response_json; - - api::JsonDeviceCommand_Feedback response; - const auto status = execute("aubo_arm", request_json, response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()); - EXPECT_TRUE(response.header().error_message().empty()); - EXPECT_TRUE(response.header().has_timestamp()); - EXPECT_GT(response.header().timestamp().seconds(), 0); - EXPECT_EQ(response.response_json(), response_json); - EXPECT_EQ(aubo_arm_->execute_calls, 1); - EXPECT_EQ(aubo_arm_->last_request_json, request_json); - EXPECT_EQ(left_arm_->execute_calls, 0); -} - -TEST_F(GrpcArmServiceTest, ForwardsDeviceJsonFailureWithLegacyGrpcOkSemantics) -{ - const std::string response_json = - R"({"success":false,"error_code":"not_connected"})"; - aubo_arm_->next_success = false; - aubo_arm_->next_response_json = response_json; - - api::JsonDeviceCommand_Feedback response; - const auto status = execute( - "aubo_arm", - R"({"command":"cabinet_io","operation":"get_do","index":0})", - response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.header().error_message(), response_json); - EXPECT_TRUE(response.header().has_timestamp()); - EXPECT_GT(response.header().timestamp().seconds(), 0); - EXPECT_EQ(response.response_json(), response_json); - EXPECT_EQ(aubo_arm_->execute_calls, 1); - EXPECT_EQ(left_arm_->execute_calls, 0); -} - -TEST_F(GrpcArmServiceTest, - MissingOrNonArmIdReturnsBusinessFailureWithoutBackendDispatch) -{ - api::JsonDeviceCommand_Feedback non_arm_response; - const auto non_arm_status = execute( - "camera", R"({"command":"cabinet_io"})", non_arm_response); - - ASSERT_TRUE(non_arm_status.ok()) << non_arm_status.error_message(); - EXPECT_FALSE(non_arm_response.header().success()); - EXPECT_EQ(non_arm_response.header().error_message(), - "Device not found: camera"); - EXPECT_TRUE(non_arm_response.header().has_timestamp()); - EXPECT_TRUE(non_arm_response.response_json().empty()); - EXPECT_EQ(non_arm_->execute_calls, 0); - EXPECT_EQ(aubo_arm_->execute_calls, 0); - EXPECT_EQ(left_arm_->execute_calls, 0); - - api::JsonDeviceCommand_Feedback missing_response; - const auto missing_status = execute( - "missing_arm", R"({"command":"cabinet_io"})", missing_response); - - ASSERT_TRUE(missing_status.ok()) << missing_status.error_message(); - EXPECT_FALSE(missing_response.header().success()); - EXPECT_EQ(missing_response.header().error_message(), - "Device not found: missing_arm"); - EXPECT_TRUE(missing_response.header().has_timestamp()); - EXPECT_TRUE(missing_response.response_json().empty()); - EXPECT_EQ(non_arm_->execute_calls, 0); - EXPECT_EQ(aubo_arm_->execute_calls, 0); - EXPECT_EQ(left_arm_->execute_calls, 0); -} - -TEST_F(GrpcArmServiceTest, - StopMotionRevokesBlockedMoveJLeaseBeforeMoveLReturns) -{ - aubo_arm_->blockNextMotion(); - auto blocked_move = std::async( - std::launch::async, - [this]() { return moveJ("aubo_arm"); }); - - const bool move_started = aubo_arm_->waitForBlockingMotion( - "moveJ", std::chrono::seconds(2)); - - MoveOutcome conflict; - MoveOutcome during_stop; - api::CommandHeader_Feedback torque_off_response; - grpc::Status torque_off_status; - api::CommandHeader_Feedback stop_response; - grpc::Status stop_status; - MoveOutcome before_retired_handler_release; - bool stop_started = false; - std::future blocked_stop; - std::future torque_off; - if (move_started) { - conflict = moveL("aubo_arm"); - aubo_arm_->blockNextStopMotion(); - blocked_stop = std::async( - std::launch::async, - [this, &stop_response]() { - return stopMotion("aubo_arm", stop_response); - }); - stop_started = aubo_arm_->waitForBlockingStop( - std::chrono::seconds(2)); - if (stop_started) { - torque_off = std::async( - std::launch::async, - [this, &torque_off_response]() { - return torqueOff("aubo_arm", torque_off_response); - }); - during_stop = moveL("aubo_arm"); - } - aubo_arm_->releaseBlockingStop(); - before_retired_handler_release = moveL("aubo_arm"); - } - - // Keep the original RPC active until after the replacement MoveL has - // attempted to acquire control. This models a driver whose stopped motion - // takes time to unwind and guards the lease hand-off itself. - aubo_arm_->releaseBlockingMotion(); - const auto original_move = blocked_move.get(); - if (blocked_stop.valid()) { - stop_status = blocked_stop.get(); - } - if (torque_off.valid()) { - torque_off_status = torque_off.get(); - } - const auto resumed_move = moveL("aubo_arm"); - - ASSERT_TRUE(move_started); - ASSERT_TRUE(stop_started); - EXPECT_EQ(conflict.status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ(conflict.response_error, conflict.status.error_message()); - EXPECT_EQ(during_stop.status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ(during_stop.response_error, - during_stop.status.error_message()); - EXPECT_TRUE(torque_off_status.ok()) - << torque_off_status.error_message(); - EXPECT_TRUE(torque_off_response.success()) - << torque_off_response.error_message(); - EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_TRUE(stop_response.success()) - << stop_response.error_message(); - EXPECT_EQ( - before_retired_handler_release.status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_TRUE(resumed_move.status.ok()) - << resumed_move.status.error_message(); - EXPECT_TRUE(resumed_move.response_success) - << resumed_move.response_error; - EXPECT_TRUE(original_move.status.ok()) - << original_move.status.error_message(); - EXPECT_TRUE(original_move.response_success) - << original_move.response_error; - EXPECT_EQ(aubo_arm_->moveJCalls(), 1); - EXPECT_EQ(aubo_arm_->moveLCalls(), 1); - EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2); - EXPECT_EQ(aubo_arm_->torqueOffCalls(), 2); -} - -TEST_F(GrpcArmServiceTest, MoveBindsLeaseRevocationCancellation) -{ - const auto outcome = moveJ("aubo_arm"); - - ASSERT_TRUE(outcome.status.ok()) - << outcome.status.error_message(); - EXPECT_TRUE(outcome.response_success) << outcome.response_error; - EXPECT_TRUE(aubo_arm_->lastMotionHadCancellation()); - EXPECT_FALSE(aubo_arm_->lastMotionCancellationRequested()); -} - -TEST_F(GrpcArmServiceTest, - IdenticalCommandIdReplaysCachedResultWithoutRedispatch) -{ - api::MoveJ_Response first; - api::MoveJ_Response retry; - - const auto first_status = identifiedMoveJ( - "arm-movej-idempotency-1", 0.1, first); - const auto retry_status = identifiedMoveJ( - "arm-movej-idempotency-1", 0.1, retry); - - ASSERT_TRUE(first_status.ok()) << first_status.error_message(); - ASSERT_TRUE(retry_status.ok()) << retry_status.error_message(); - EXPECT_TRUE(first.header().success()); - EXPECT_TRUE(retry.header().success()); - EXPECT_EQ( - retry.header().execution_state(), - api::COMMAND_EXECUTION_STATE_COMPLETED); - EXPECT_EQ(retry.header().command_id(), "arm-movej-idempotency-1"); - EXPECT_EQ(aubo_arm_->moveJCalls(), 1); -} - -TEST_F(GrpcArmServiceTest, - ReusedCommandIdWithDifferentPayloadIsRejectedWithoutRedispatch) -{ - api::MoveJ_Response first; - api::MoveJ_Response conflict; - - const auto first_status = identifiedMoveJ( - "arm-movej-conflict-1", 0.1, first); - const auto conflict_status = identifiedMoveJ( - "arm-movej-conflict-1", 0.2, conflict); - - ASSERT_TRUE(first_status.ok()) << first_status.error_message(); - EXPECT_EQ( - conflict_status.error_code(), grpc::StatusCode::ALREADY_EXISTS); - EXPECT_FALSE(conflict.header().success()); - EXPECT_EQ( - conflict.header().reason_code(), - api::COMMAND_REASON_CODE_COMMAND_ID_CONFLICT); - EXPECT_EQ(aubo_arm_->moveJCalls(), 1); -} - -TEST_F(GrpcArmServiceTest, - StopAllCancelsInFlightTorqueOnWithoutReportingSuccess) -{ - aubo_arm_->blockNextTorqueOn(); - auto blocked_torque_on = std::async( - std::launch::async, - [this]() { return torqueOn("aubo_arm"); }); - - const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn( - std::chrono::seconds(2)); - auto& admission = globalStopAllAdmissionGate(); - StopAllAdmissionGate::StopAllTicket stop_all_ticket; - if (torque_on_started) { - stop_all_ticket = admission.beginStopAll(); - } - - const bool cancelled_promptly = - blocked_torque_on.wait_for(std::chrono::seconds(2)) == - std::future_status::ready; - if (!cancelled_promptly) { - aubo_arm_->releaseBlockingTorqueOn(); - } - const auto outcome = blocked_torque_on.get(); - - ASSERT_TRUE(torque_on_started); - ASSERT_TRUE(stop_all_ticket.valid()); - EXPECT_TRUE(cancelled_promptly); - EXPECT_EQ(outcome.status.error_code(), grpc::StatusCode::UNAVAILABLE); - EXPECT_FALSE(outcome.response_success); - EXPECT_EQ(outcome.response_error, outcome.status.error_message()); - EXPECT_NE(outcome.response_error.find("StopAll"), std::string::npos); - EXPECT_EQ(aubo_arm_->torqueOnCalls(), 1); - EXPECT_TRUE(aubo_arm_->lastTorqueOnHadCancellation()); - EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested()); - - EXPECT_TRUE(admission.finishStopAll(stop_all_ticket, true)); -} - -TEST_F(GrpcArmServiceTest, - StopAllPreservesTorqueOnSafetyTerminationFailure) -{ - aubo_arm_->blockNextTorqueOn(); - aubo_arm_->failNextTorqueOnCancellation(); - auto blocked_torque_on = std::async( - std::launch::async, - [this]() { return torqueOn("aubo_arm"); }); - - const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn( - std::chrono::seconds(2)); - auto& admission = globalStopAllAdmissionGate(); - StopAllAdmissionGate::StopAllTicket stop_all_ticket; - if (torque_on_started) { - stop_all_ticket = admission.beginStopAll(); - } - - const bool completed_promptly = - blocked_torque_on.wait_for(std::chrono::seconds(2)) == - std::future_status::ready; - if (!completed_promptly) { - aubo_arm_->releaseBlockingTorqueOn(); - } - const auto outcome = blocked_torque_on.get(); - - ASSERT_TRUE(torque_on_started); - ASSERT_TRUE(stop_all_ticket.valid()); - EXPECT_TRUE(completed_promptly); - EXPECT_EQ(outcome.status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_FALSE(outcome.response_success); - EXPECT_EQ( - outcome.response_error, - "simulated torqueOn safety termination failure"); - EXPECT_EQ(outcome.status.error_message(), outcome.response_error); - EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested()); - - EXPECT_FALSE(admission.finishStopAll(stop_all_ticket, false)); -} - -TEST_F(GrpcArmServiceTest, - ClientCancellationPreservesTorqueOnSafetyTerminationFailure) -{ - std::mutex records_mutex; - std::condition_variable records_changed; - std::vector records; - - grpc::ServerBuilder builder; - const std::string socket_path = - "/tmp/cmvr_grpc_arm_service_test_" + - std::to_string(static_cast(::getpid())) + ".sock"; - std::remove(socket_path.c_str()); - const std::string address = "unix:" + socket_path; - builder.AddListeningPort( - address, - grpc::InsecureServerCredentials()); - builder.RegisterService(service_.get()); - std::vector> factories; - factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory( - [&records, &records_mutex, &records_changed]( - const GrpcFailureRecord& record) { - { - std::lock_guard lock(records_mutex); - records.push_back(record); - } - records_changed.notify_all(); - })); - builder.experimental().SetInterceptorCreators(std::move(factories)); - auto server = builder.BuildAndStart(); - ASSERT_NE(server, nullptr); - - const auto channel = grpc::CreateChannel( - address, - grpc::InsecureChannelCredentials()); - auto stub = api::ArmService::NewStub(channel); - grpc::ClientContext client_context; - client_context.set_deadline( - std::chrono::system_clock::now() + std::chrono::seconds(3)); - api::CommandHeader_Request request; - request.set_device_id("aubo_arm"); - api::CommandHeader_Feedback response; - grpc::Status client_status; - - aubo_arm_->blockNextTorqueOn(); - aubo_arm_->failNextTorqueOnCancellation(); - std::thread client_call([&]() { - client_status = stub->torqueOn( - &client_context, request, &response); - }); - const bool torque_on_started = aubo_arm_->waitForBlockingTorqueOn( - std::chrono::seconds(2)); - client_context.TryCancel(); - client_call.join(); - - bool failure_recorded = false; - { - std::unique_lock lock(records_mutex); - failure_recorded = records_changed.wait_for( - lock, - std::chrono::seconds(2), - [&records]() { return !records.empty(); }); - } - if (!failure_recorded) { - aubo_arm_->releaseBlockingTorqueOn(); - } - server->Shutdown(); - server->Wait(); - std::remove(socket_path.c_str()); - - ASSERT_TRUE(torque_on_started); - EXPECT_EQ(client_status.error_code(), grpc::StatusCode::CANCELLED); - ASSERT_TRUE(failure_recorded); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records.front().kind, GrpcFailureKind::GRPC_STATUS); - EXPECT_EQ(records.front().status_code, grpc::StatusCode::INTERNAL); - EXPECT_EQ(records.front().severity, GrpcFailureSeverity::ERROR); - EXPECT_EQ( - records.front().detail, - "simulated torqueOn safety termination failure"); - EXPECT_TRUE(aubo_arm_->lastTorqueOnCancellationRequested()); -} - -TEST_F(GrpcArmServiceTest, - StopAllGateRejectsMutatingCommandsButAllowsReadsAndStops) -{ - auto& admission = globalStopAllAdmissionGate(); - const auto ticket = admission.beginStopAll(); - ASSERT_TRUE(ticket.valid()); - - const auto rejected_move = moveJ("aubo_arm"); - - api::JsonDeviceCommand_Feedback json_response; - const auto json_status = execute( - "aubo_arm", R"({"command":"cabinet_io"})", json_response); - - api::JointRequest state_request; - state_request.mutable_header()->set_device_id("aubo_arm"); - api::JointResponse state_response; - grpc::ServerContext state_context; - const auto state_status = service_->getJointState( - &state_context, &state_request, &state_response); - - api::CommandHeader_Feedback stop_response; - const auto stop_status = stopMotion("aubo_arm", stop_response); - - EXPECT_EQ( - rejected_move.status.error_code(), - grpc::StatusCode::UNAVAILABLE); - EXPECT_FALSE(rejected_move.response_success); - EXPECT_EQ(aubo_arm_->moveJCalls(), 0); - EXPECT_EQ(json_status.error_code(), grpc::StatusCode::UNAVAILABLE); - EXPECT_FALSE(json_response.header().success()); - EXPECT_EQ(aubo_arm_->execute_calls, 0); - - EXPECT_TRUE(state_status.ok()) << state_status.error_message(); - EXPECT_TRUE(state_response.header().success()); - EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_TRUE(stop_response.success()) - << stop_response.error_message(); - EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2); - - EXPECT_TRUE(admission.finishStopAll(ticket, true)); - const auto resumed_move = moveJ("aubo_arm"); - EXPECT_TRUE(resumed_move.status.ok()) - << resumed_move.status.error_message(); - EXPECT_TRUE(resumed_move.response_success) - << resumed_move.response_error; - EXPECT_EQ(aubo_arm_->moveJCalls(), 1); -} - -TEST_F(GrpcArmServiceTest, - StopMotionRevokesBlockedMoveLLeaseBeforeMoveJReturns) -{ - aubo_arm_->blockNextMotion(); - auto blocked_move = std::async( - std::launch::async, - [this]() { return moveL("aubo_arm"); }); - - const bool move_started = aubo_arm_->waitForBlockingMotion( - "moveL", std::chrono::seconds(2)); - - MoveOutcome conflict; - api::CommandHeader_Feedback stop_response; - grpc::Status stop_status; - MoveOutcome before_retired_handler_release; - if (move_started) { - conflict = moveJ("aubo_arm"); - auto stop = std::async( - std::launch::async, - [this, &stop_response]() { - return stopMotion("aubo_arm", stop_response); - }); - before_retired_handler_release = moveJ("aubo_arm"); - aubo_arm_->releaseBlockingMotion(); - stop_status = stop.get(); - } - - aubo_arm_->releaseBlockingMotion(); - const auto original_move = blocked_move.get(); - const auto resumed_move = moveJ("aubo_arm"); - - ASSERT_TRUE(move_started); - EXPECT_EQ(conflict.status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ(conflict.response_error, conflict.status.error_message()); - EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_TRUE(stop_response.success()) - << stop_response.error_message(); - EXPECT_EQ( - before_retired_handler_release.status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_TRUE(resumed_move.status.ok()) - << resumed_move.status.error_message(); - EXPECT_TRUE(resumed_move.response_success) - << resumed_move.response_error; - EXPECT_TRUE(original_move.status.ok()) - << original_move.status.error_message(); - EXPECT_TRUE(original_move.response_success) - << original_move.response_error; - EXPECT_EQ(aubo_arm_->moveJCalls(), 1); - EXPECT_EQ(aubo_arm_->moveLCalls(), 1); - EXPECT_EQ(aubo_arm_->stopMotionCalls(), 2); -} - -TEST_F(GrpcArmServiceTest, StopMotionFailureRetainsSafetyBarrier) -{ - auto& authority = control::ControlAuthorityManager::instance(); - const auto action_lease = authority.tryAcquire( - "aubo_arm", "action-queue:test", std::chrono::hours(1)); - ASSERT_TRUE(action_lease.acquired) << action_lease.detail; - aubo_arm_->failNextStopMotion(); - - api::CommandHeader_Feedback stop_response; - const auto stop_status = stopMotion("aubo_arm", stop_response); - const auto rejected_move = moveJ("aubo_arm"); - - EXPECT_EQ(stop_status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_FALSE(stop_response.success()); - EXPECT_EQ( - stop_response.reason_code(), - api::COMMAND_REASON_CODE_STOP_UNCONFIRMED); - EXPECT_FALSE(authority.validate(action_lease.token)); - EXPECT_TRUE(authority.isLeased("aubo_arm")); - EXPECT_EQ( - rejected_move.status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ(aubo_arm_->moveJCalls(), 0); - EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); - - authority.release(action_lease.token); - const auto recovery = authority.preemptAcquire( - "aubo_arm", "confirmed-stop-recovery", std::chrono::hours(1)); - ASSERT_TRUE(recovery.acquired) << recovery.detail; - ASSERT_TRUE(authority.waitForPreemptedRelease( - recovery.token, std::chrono::milliseconds::zero())); - ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token)); - authority.release(recovery.token); - const auto recovered = authority.tryAcquire( - "aubo_arm", "move-after-recovery", std::chrono::hours(1)); - EXPECT_TRUE(recovered.acquired) << recovered.detail; - authority.release(recovered.token); -} - -TEST_F(GrpcArmServiceTest, StopMotionExceptionRetainsSafetyBarrier) -{ - auto& authority = control::ControlAuthorityManager::instance(); - const auto action_lease = authority.tryAcquire( - "aubo_arm", "action-queue:test", std::chrono::hours(1)); - ASSERT_TRUE(action_lease.acquired) << action_lease.detail; - aubo_arm_->throwNextStopMotion(); - - api::CommandHeader_Feedback stop_response; - const auto stop_status = stopMotion("aubo_arm", stop_response); - const auto rejected_move = moveL("aubo_arm"); - - EXPECT_EQ( - stop_status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_FALSE(stop_response.success()); - EXPECT_EQ( - stop_response.reason_code(), - api::COMMAND_REASON_CODE_STOP_UNCONFIRMED); - EXPECT_FALSE(authority.validate(action_lease.token)); - EXPECT_TRUE(authority.isLeased("aubo_arm")); - EXPECT_EQ( - rejected_move.status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ(aubo_arm_->moveLCalls(), 0); - EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); - - authority.release(action_lease.token); - const auto recovery = authority.preemptAcquire( - "aubo_arm", "confirmed-exception-recovery", std::chrono::hours(1)); - ASSERT_TRUE(recovery.acquired) << recovery.detail; - ASSERT_TRUE(authority.waitForPreemptedRelease( - recovery.token, std::chrono::milliseconds::zero())); - ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token)); - authority.release(recovery.token); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_arm_teleop_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_arm_teleop_service_test.cpp deleted file mode 100644 index 0a6be187..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_arm_teleop_service_test.cpp +++ /dev/null @@ -1,920 +0,0 @@ -#include "service/grpc/server/include/grpc_arm_teleop_service.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "manager/safety_manager/include/device_safety_endpoint.h" -#include "manager/safety_manager/include/safety_manager.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { -namespace { - -using namespace std::chrono_literals; - -std::atomic g_socket_sequence{0}; - -arm_teleop::RobotManifest makeManifest() -{ - arm_teleop::RobotManifest manifest; - manifest.set_robot_id("fake_humanoid_arm"); - manifest.set_model_sha256(std::string(64, 'a')); - manifest.set_calibration_sha256(std::string(64, 'b')); - manifest.add_joint_names("shoulder_joint"); - manifest.add_joint_names("elbow_joint"); - manifest.set_position_unit("rad"); - manifest.set_velocity_unit("rad/s"); - manifest.set_effort_unit("N*m"); - manifest.set_base_frame("base_link"); - manifest.set_tool_frame("tool_link"); - return manifest; -} - -arm_teleop::ClientFrame makeOpenFrame( - const arm_teleop::RobotManifest& manifest, - const std::uint32_t watchdog_ms = 100, - const std::uint32_t lease_ms = 2000, - const bool request_force_feedback = false) -{ - arm_teleop::ClientFrame frame; - auto* open = frame.mutable_open(); - open->set_protocol_major(1); - open->set_protocol_minor(0); - open->set_client_instance_id("test-client"); - *open->mutable_expected_robot() = manifest; - open->set_requested_command_rate_hz(200); - open->set_requested_state_rate_hz(100); - open->set_watchdog_timeout_ms(watchdog_ms); - open->set_requested_lease_ms(lease_ms); - open->set_request_force_feedback(request_force_feedback); - return frame; -} - -arm_teleop::ClientFrame makeSetpoint( - const std::uint64_t sequence, - const std::uint32_t valid_for_us = 50000) -{ - arm_teleop::ClientFrame frame; - auto* setpoint = frame.mutable_setpoint(); - setpoint->set_sequence(sequence); - setpoint->add_position_rad(0.1 * static_cast(sequence)); - setpoint->add_position_rad(0.2 * static_cast(sequence)); - setpoint->add_velocity_rad_s(0.01); - setpoint->add_velocity_rad_s(0.02); - setpoint->set_valid_for_us(valid_for_us); - return frame; -} - -arm_teleop::ClientFrame makeHeartbeat(const std::uint64_t sequence) -{ - arm_teleop::ClientFrame frame; - frame.mutable_heartbeat()->set_sequence(sequence); - return frame; -} - -arm_teleop::ClientFrame makeStop( - const arm_teleop::StopReason reason = - arm_teleop::STOP_REASON_OPERATOR_REQUEST) -{ - arm_teleop::ClientFrame frame; - frame.mutable_stop()->set_reason(reason); - frame.mutable_stop()->set_detail("test stop"); - return frame; -} - -class FakeArmTeleopBackend final : public ArmTeleopBackend { -public: - explicit FakeArmTeleopBackend( - arm_teleop::RobotManifest manifest = makeManifest()) - : manifest_(std::move(manifest)) - { - } - - bool available() const noexcept override { return true; } - - arm_teleop::RobotManifest manifest() const override - { - recordThread(); - return manifest_; - } - - bool supportsForceFeedback() const noexcept override { return true; } - - ArmTeleopBackendResult open( - const arm_teleop::OpenSession&) override - { - recordThread(); - std::lock_guard lock(mutex_); - ++open_calls_; - return open_result_; - } - - ArmTeleopBackendResult applySetpoint( - const arm_teleop::JointSetpoint& setpoint, - const std::chrono::steady_clock::time_point deadline) override - { - if (std::chrono::steady_clock::now() >= deadline) { - return ArmTeleopBackendResult::failure( - grpc::StatusCode::DEADLINE_EXCEEDED, - "fake backend received an expired setpoint"); - } - recordThread(); - std::unique_lock lock(mutex_); - apply_entered_ = true; - apply_entered_sequence_ = setpoint.sequence(); - cv_.notify_all(); - cv_.wait(lock, [&]() { return !block_apply_; }); - applied_sequences_.push_back(setpoint.sequence()); - return apply_result_; - } - - ArmTeleopBackendResult stop( - const arm_teleop::StopReason reason, - const std::string&) override - { - recordThread(); - std::lock_guard lock(mutex_); - stop_reasons_.push_back(reason); - return stop_result_; - } - - ArmTeleopBackendSnapshot snapshot() const override - { - recordThread(); - std::lock_guard lock(mutex_); - ArmTeleopBackendSnapshot snapshot; - auto* state = &snapshot.joint_state; - state->set_sample_sequence(++sample_sequence_); - state->add_position_rad(0.1); - state->add_position_rad(0.2); - state->add_velocity_rad_s(0.01); - state->add_velocity_rad_s(0.02); - state->add_effort_nm(1.0); - state->add_effort_nm(2.0); - state->set_position_valid(true); - state->set_velocity_valid(true); - state->set_effort_valid(true); - state->set_effort_source( - arm_teleop::EFFORT_SOURCE_JOINT_SENSOR); - snapshot.safety.set_connected(true); - snapshot.safety.set_powered_on(true); - return snapshot; - } - - void blockApply() - { - std::lock_guard lock(mutex_); - block_apply_ = true; - apply_entered_ = false; - apply_entered_sequence_ = 0; - } - - void releaseApply() - { - { - std::lock_guard lock(mutex_); - block_apply_ = false; - } - cv_.notify_all(); - } - - bool waitForApply( - const std::uint64_t sequence, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return cv_.wait_for(lock, timeout, [&]() { - return apply_entered_ && - apply_entered_sequence_ == sequence; - }); - } - - int openCalls() const - { - std::lock_guard lock(mutex_); - return open_calls_; - } - - std::vector appliedSequences() const - { - std::lock_guard lock(mutex_); - return applied_sequences_; - } - - std::vector stopReasons() const - { - std::lock_guard lock(mutex_); - return stop_reasons_; - } - - std::size_t backendThreadCount() const - { - std::lock_guard lock(thread_mutex_); - return backend_threads_.size(); - } - -private: - void recordThread() const - { - std::lock_guard lock(thread_mutex_); - backend_threads_.insert(std::this_thread::get_id()); - } - - arm_teleop::RobotManifest manifest_; - mutable std::mutex mutex_; - mutable std::condition_variable cv_; - bool block_apply_{false}; - bool apply_entered_{false}; - std::uint64_t apply_entered_sequence_{0}; - int open_calls_{0}; - std::vector applied_sequences_; - std::vector stop_reasons_; - mutable std::uint64_t sample_sequence_{0}; - ArmTeleopBackendResult open_result_{ - ArmTeleopBackendResult::ok()}; - ArmTeleopBackendResult apply_result_{ - ArmTeleopBackendResult::ok()}; - ArmTeleopBackendResult stop_result_{ - ArmTeleopBackendResult::ok()}; - - mutable std::mutex thread_mutex_; - mutable std::set backend_threads_; -}; - -class FakeTeleopSafetyEndpoint final - : public safety::DeviceSafetyEndpoint { -public: - FakeTeleopSafetyEndpoint() - { - descriptor_.device_id = makeManifest().robot_id(); - descriptor_.kind = device::DeviceKind::Arm; - descriptor_.default_policy = - safety::SafetyPolicyFamily::Control; - descriptor_.maximum_snapshot_age = 1s; - descriptor_.supports_active_refresh = true; - } - - safety::DeviceSafetyDescriptor descriptor() const override - { - return descriptor_; - } - - void bindPublisher( - safety::SafetySnapshotPublisher publisher) override - { - publisher_ = std::move(publisher); - } - - void requestSafetyRefresh() noexcept override - { - if (!publisher_) { - return; - } - safety::DeviceSafetySnapshot snapshot; - snapshot.device_id = descriptor_.device_id; - snapshot.condition = safety::SafetyCondition::Nominal; - snapshot.device_generation = generation_; - snapshot.sample_sequence = ++sequence_; - snapshot.observed_at = safety::SafetyClock::now(); - snapshot.connected = safety::TriState::True; - snapshot.operational_ready = safety::TriState::True; - snapshot.quiescent = safety::TriState::True; - snapshot.motion_active = safety::TriState::False; - snapshot.actuator_enabled = safety::TriState::True; - snapshot.emergency_stop_active = safety::TriState::False; - snapshot.protective_stop_active = safety::TriState::False; - snapshot.fault_active = safety::TriState::False; - (void)publisher_(std::move(snapshot)); - } - - void onDeviceGenerationChanged( - const std::uint64_t generation) noexcept override - { - generation_ = generation; - requestSafetyRefresh(); - } - - safety::HardwareCheckResult validateBeforeDispatch( - const safety::AdmissionPermit&) override - { - ++hardware_checks_; - return {true, safety::SafetyReason::None, {}}; - } - - safety::RecoveryCheckResult reconcileAdmissionState( - const safety::RecoveryContext&) override - { - return {true, safety::SafetyReason::None, {}}; - } - - int hardwareChecks() const noexcept - { - return hardware_checks_.load(); - } - -private: - safety::DeviceSafetyDescriptor descriptor_; - safety::SafetySnapshotPublisher publisher_; - std::uint64_t generation_{1}; - std::uint64_t sequence_{0}; - std::atomic hardware_checks_{0}; -}; - -class TeleopServerHarness final { -public: - explicit TeleopServerHarness( - std::shared_ptr backend, - safety::SafetyManager* safety_manager = nullptr) - : service_( - std::move(backend), nullptr, nullptr, - safety_manager) - { - socket_path_ = - "/tmp/cmvr_arm_teleop_service_test_" + - std::to_string(static_cast(::getpid())) + "_" + - std::to_string( - g_socket_sequence.fetch_add( - 1, std::memory_order_relaxed)) + - ".sock"; - std::remove(socket_path_.c_str()); - const std::string address = "unix:" + socket_path_; - - grpc::ServerBuilder builder; - builder.AddListeningPort( - address, grpc::InsecureServerCredentials()); - builder.RegisterService(&service_); - server_ = builder.BuildAndStart(); - if (!server_) { - throw std::runtime_error( - "failed to start in-process arm teleoperation server"); - } - channel_ = grpc::CreateChannel( - address, grpc::InsecureChannelCredentials()); - if (!channel_->WaitForConnected( - std::chrono::system_clock::now() + 2s)) { - throw std::runtime_error( - "failed to connect arm teleoperation test channel"); - } - stub_ = arm_teleop::ArmTeleopService::NewStub(channel_); - } - - ~TeleopServerHarness() - { - if (server_) { - server_->Shutdown(); - server_->Wait(); - } - if (!socket_path_.empty()) { - std::remove(socket_path_.c_str()); - } - } - - arm_teleop::ArmTeleopService::Stub& stub() { return *stub_; } - -private: - ArmTeleopServiceImpl service_; - std::unique_ptr server_; - std::shared_ptr channel_; - std::unique_ptr stub_; - std::string socket_path_; -}; - -template -void expectOpeningFrames(Stream& stream) -{ - arm_teleop::ServerFrame response; - ASSERT_TRUE(stream.Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_OPENED); - EXPECT_FALSE(response.status().session_id().empty()); - ASSERT_TRUE(stream.Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_READY); - EXPECT_TRUE(response.safety().connected()); -} - -TEST(ArmTeleopServiceTest, ProductionDisabledBackendRejectsOpen) -{ - TeleopServerHarness harness(makeDisabledArmTeleopBackend()); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 2s); - auto stream = harness.stub().Teleoperate(&context); - - ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest()))); - ASSERT_TRUE(stream->WritesDone()); - - arm_teleop::ServerFrame response; - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_REJECTED); - EXPECT_TRUE(response.safety().fault()); - const grpc::Status status = stream->Finish(); - EXPECT_EQ( - status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); -} - -TEST(ArmTeleopServiceTest, RequiresOpenAsFirstFrame) -{ - auto backend = std::make_shared(); - TeleopServerHarness harness(backend); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 2s); - auto stream = harness.stub().Teleoperate(&context); - - ASSERT_TRUE(stream->Write(makeSetpoint(1))); - ASSERT_TRUE(stream->WritesDone()); - - arm_teleop::ServerFrame response; - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_REJECTED); - const grpc::Status status = stream->Finish(); - EXPECT_EQ( - status.error_code(), - grpc::StatusCode::INVALID_ARGUMENT); - EXPECT_EQ(backend->openCalls(), 0); -} - -TEST(ArmTeleopServiceTest, RejectsManifestMismatchBeforeBackendOpen) -{ - auto backend = std::make_shared(); - TeleopServerHarness harness(backend); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 2s); - auto stream = harness.stub().Teleoperate(&context); - - auto mismatched = makeManifest(); - mismatched.set_calibration_sha256(std::string(64, 'c')); - ASSERT_TRUE(stream->Write(makeOpenFrame(mismatched))); - ASSERT_TRUE(stream->WritesDone()); - - arm_teleop::ServerFrame response; - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_REJECTED); - const grpc::Status status = stream->Finish(); - EXPECT_EQ( - status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ(backend->openCalls(), 0); -} - -TEST(ArmTeleopServiceTest, RejectsNonIncreasingSequenceAndStops) -{ - auto backend = std::make_shared(); - TeleopServerHarness harness(backend); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 2s); - auto stream = harness.stub().Teleoperate(&context); - - ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest()))); - expectOpeningFrames(*stream); - ASSERT_TRUE(stream->Write(makeSetpoint(1))); - - arm_teleop::ServerFrame response; - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_ACTIVE); - EXPECT_EQ(response.status().applied_sequence(), 1U); - - ASSERT_TRUE(stream->Write(makeHeartbeat(1))); - ASSERT_TRUE(stream->WritesDone()); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_REJECTED); - EXPECT_EQ( - response.status().stop_reason(), - arm_teleop::STOP_REASON_PROTOCOL_ERROR); - - const grpc::Status status = stream->Finish(); - EXPECT_EQ( - status.error_code(), - grpc::StatusCode::INVALID_ARGUMENT); - ASSERT_FALSE(backend->stopReasons().empty()); - EXPECT_EQ( - backend->stopReasons().back(), - arm_teleop::STOP_REASON_PROTOCOL_ERROR); - EXPECT_EQ(backend->backendThreadCount(), 1U); -} - -TEST(ArmTeleopServiceTest, ProtocolErrorCancelsReaderWithoutClientHalfClose) -{ - auto backend = std::make_shared(); - TeleopServerHarness harness(backend); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 2s); - auto stream = harness.stub().Teleoperate(&context); - - ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest()))); - expectOpeningFrames(*stream); - ASSERT_TRUE(stream->Write(makeHeartbeat(1))); - - arm_teleop::ServerFrame response; - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_READY); - - // Deliberately keep the client write half open after the duplicate - // sequence. The server must cancel its reader and return promptly. - ASSERT_TRUE(stream->Write(makeHeartbeat(1))); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_REJECTED); - EXPECT_EQ( - response.status().stop_reason(), - arm_teleop::STOP_REASON_PROTOCOL_ERROR); - const auto status = stream->Finish(); - // TryCancel is required to interrupt the server reader's blocking Read - // when the peer deliberately keeps its write half open. Depending on - // gRPC completion ordering, the client may therefore observe CANCELLED - // after it has already received the explicit protocol-error frame. - EXPECT_TRUE( - status.error_code() == grpc::StatusCode::INVALID_ARGUMENT || - status.error_code() == grpc::StatusCode::CANCELLED) - << status.error_message(); -} - -TEST(ArmTeleopServiceTest, WatchdogExpiresWithoutValidClientActivity) -{ - auto backend = std::make_shared(); - TeleopServerHarness harness(backend); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 2s); - auto stream = harness.stub().Teleoperate(&context); - - ASSERT_TRUE(stream->Write( - makeOpenFrame(makeManifest(), 20, 2000))); - expectOpeningFrames(*stream); - - arm_teleop::ServerFrame response; - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_WATCHDOG_EXPIRED); - EXPECT_EQ( - response.status().stop_reason(), - arm_teleop::STOP_REASON_WATCHDOG); - - const grpc::Status status = stream->Finish(); - EXPECT_TRUE( - status.error_code() == grpc::StatusCode::DEADLINE_EXCEEDED || - status.error_code() == grpc::StatusCode::CANCELLED) - << status.error_message(); - ASSERT_FALSE(backend->stopReasons().empty()); - EXPECT_EQ( - backend->stopReasons().back(), - arm_teleop::STOP_REASON_WATCHDOG); -} - -TEST(ArmTeleopServiceTest, ValidHeartbeatsRenewControlLease) -{ - auto backend = std::make_shared(); - TeleopServerHarness harness(backend); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 3s); - auto stream = harness.stub().Teleoperate(&context); - - ASSERT_TRUE(stream->Write( - makeOpenFrame(makeManifest(), 100, 200))); - expectOpeningFrames(*stream); - - arm_teleop::ServerFrame response; - for (std::uint64_t sequence = 1; sequence <= 7; ++sequence) { - std::this_thread::sleep_for(40ms); - ASSERT_TRUE(stream->Write(makeHeartbeat(sequence))); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_READY); - EXPECT_EQ( - response.status().received_sequence(), - sequence); - EXPECT_GT(response.status().lease_remaining_ms(), 0U); - } - - ASSERT_TRUE(stream->Write(makeStop())); - ASSERT_TRUE(stream->WritesDone()); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_STOPPED); - EXPECT_TRUE(stream->Finish().ok()); -} - -TEST(ArmTeleopServiceTest, RejectsSecondControllerWhileLeaseIsActive) -{ - auto backend = std::make_shared(); - TeleopServerHarness harness(backend); - - grpc::ClientContext first_context; - first_context.set_deadline( - std::chrono::system_clock::now() + 3s); - auto first_stream = - harness.stub().Teleoperate(&first_context); - ASSERT_TRUE(first_stream->Write( - makeOpenFrame(makeManifest(), 500, 2000))); - expectOpeningFrames(*first_stream); - - grpc::ClientContext second_context; - second_context.set_deadline( - std::chrono::system_clock::now() + 2s); - auto second_stream = - harness.stub().Teleoperate(&second_context); - ASSERT_TRUE(second_stream->Write( - makeOpenFrame(makeManifest(), 500, 2000))); - ASSERT_TRUE(second_stream->WritesDone()); - - arm_teleop::ServerFrame response; - ASSERT_TRUE(second_stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_REJECTED); - const grpc::Status second_status = - second_stream->Finish(); - EXPECT_EQ( - second_status.error_code(), - grpc::StatusCode::RESOURCE_EXHAUSTED); - - ASSERT_TRUE(first_stream->Write(makeStop())); - ASSERT_TRUE(first_stream->WritesDone()); - ASSERT_TRUE(first_stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_STOPPED); - EXPECT_TRUE(first_stream->Finish().ok()); -} - -TEST(ArmTeleopServiceTest, LatestOnlySlotDropsIntermediateSetpoints) -{ - auto backend = std::make_shared(); - TeleopServerHarness harness(backend); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 3s); - auto stream = harness.stub().Teleoperate(&context); - - ASSERT_TRUE(stream->Write( - makeOpenFrame(makeManifest(), 500, 2000))); - expectOpeningFrames(*stream); - - backend->blockApply(); - ASSERT_TRUE(stream->Write(makeSetpoint(1, 400000))); - ASSERT_TRUE(backend->waitForApply(1, 1s)); - ASSERT_TRUE(stream->Write(makeSetpoint(2, 400000))); - ASSERT_TRUE(stream->Write(makeSetpoint(3, 400000))); - ASSERT_TRUE(stream->Write(makeSetpoint(4, 400000))); - std::this_thread::sleep_for(30ms); - backend->releaseApply(); - - arm_teleop::ServerFrame response; - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ(response.status().applied_sequence(), 1U); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ(response.status().applied_sequence(), 4U); - EXPECT_GE(response.status().dropped_setpoints(), 2U); - - ASSERT_TRUE(stream->Write(makeStop())); - ASSERT_TRUE(stream->WritesDone()); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_STOPPED); - EXPECT_TRUE(stream->Finish().ok()); - - const auto applied = backend->appliedSequences(); - ASSERT_EQ(applied.size(), 2U); - EXPECT_EQ(applied[0], 1U); - EXPECT_EQ(applied[1], 4U); - EXPECT_EQ(backend->backendThreadCount(), 1U); -} - -TEST(ArmTeleopServiceTest, ExpiredSetpointIsNeverDispatched) -{ - auto backend = std::make_shared(); - TeleopServerHarness harness(backend); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 3s); - auto stream = harness.stub().Teleoperate(&context); - - ASSERT_TRUE(stream->Write( - makeOpenFrame(makeManifest(), 500, 2000))); - expectOpeningFrames(*stream); - - backend->blockApply(); - ASSERT_TRUE(stream->Write(makeSetpoint(1, 400000))); - ASSERT_TRUE(backend->waitForApply(1, 1s)); - ASSERT_TRUE(stream->Write(makeSetpoint(2, 1000))); - std::this_thread::sleep_for(30ms); - backend->releaseApply(); - - arm_teleop::ServerFrame response; - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ(response.status().applied_sequence(), 1U); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_HOLDING); - EXPECT_EQ(response.status().received_sequence(), 2U); - EXPECT_EQ(response.status().applied_sequence(), 1U); - EXPECT_EQ(response.status().rejected_setpoints(), 1U); - - ASSERT_TRUE(stream->Write(makeStop())); - ASSERT_TRUE(stream->WritesDone()); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_STOPPED); - EXPECT_TRUE(stream->Finish().ok()); - - const auto applied = backend->appliedSequences(); - ASSERT_EQ(applied.size(), 1U); - EXPECT_EQ(applied.front(), 1U); -} - -TEST(ArmTeleopServiceTest, - StopAllFencesDispatchRejectsAdmissionAndAllowsReuse) -{ - auto& authority = control::ControlAuthorityManager::instance(); - auto& admission = globalStopAllAdmissionGate(); - authority.clear(); - admission.clearForTesting(); - - auto backend = std::make_shared(); - TeleopServerHarness harness(backend); - grpc::ClientContext active_context; - active_context.set_deadline( - std::chrono::system_clock::now() + 3s); - auto active_stream = harness.stub().Teleoperate(&active_context); - - ASSERT_TRUE(active_stream->Write( - makeOpenFrame(makeManifest(), 500, 2000))); - expectOpeningFrames(*active_stream); - - backend->blockApply(); - ASSERT_TRUE(active_stream->Write(makeSetpoint(1, 400000))); - ASSERT_TRUE(backend->waitForApply(1, 1s)); - - const auto stop_all_ticket = admission.beginStopAll(); - ASSERT_TRUE(stop_all_ticket.valid()); - - grpc::ClientContext blocked_context; - blocked_context.set_deadline( - std::chrono::system_clock::now() + 2s); - auto blocked_stream = harness.stub().Teleoperate(&blocked_context); - ASSERT_TRUE(blocked_stream->Write(makeOpenFrame(makeManifest()))); - ASSERT_TRUE(blocked_stream->WritesDone()); - arm_teleop::ServerFrame response; - ASSERT_TRUE(blocked_stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_REJECTED); - const auto blocked_status = blocked_stream->Finish(); - EXPECT_EQ( - blocked_status.error_code(), grpc::StatusCode::UNAVAILABLE); - EXPECT_EQ(backend->openCalls(), 1); - - const auto safety_lease = authority.preemptAcquire( - makeManifest().robot_id(), "stop-all-test", - std::chrono::hours(1)); - ASSERT_TRUE(safety_lease.acquired) << safety_lease.detail; - const auto safety_token = safety_lease.token; - auto dispatch_fence = std::async( - std::launch::async, - [&authority, safety_token] { - return authority.waitForPreemptedRelease(safety_token, 1s); - }); - - EXPECT_EQ( - dispatch_fence.wait_for(50ms), std::future_status::timeout); - backend->releaseApply(); - ASSERT_EQ( - dispatch_fence.wait_for(1s), std::future_status::ready); - EXPECT_TRUE(dispatch_fence.get()); - - ASSERT_TRUE(active_stream->Read(&response)); - EXPECT_EQ(response.status().applied_sequence(), 1U); - ASSERT_TRUE(active_stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_LEASE_LOST); - EXPECT_EQ( - response.status().stop_reason(), - arm_teleop::STOP_REASON_LEASE_REVOKED); - const auto active_status = active_stream->Finish(); - EXPECT_TRUE( - active_status.error_code() == grpc::StatusCode::ABORTED || - active_status.error_code() == grpc::StatusCode::CANCELLED) - << active_status.error_message(); - - const auto applied = backend->appliedSequences(); - ASSERT_EQ(applied.size(), 1U); - EXPECT_EQ(applied.front(), 1U); - const auto stopped = backend->stopReasons(); - ASSERT_FALSE(stopped.empty()); - EXPECT_EQ( - stopped.back(), arm_teleop::STOP_REASON_LEASE_REVOKED); - - authority.release(safety_token); - ASSERT_TRUE(admission.finishStopAll(stop_all_ticket, true)); - - grpc::ClientContext resumed_context; - resumed_context.set_deadline( - std::chrono::system_clock::now() + 2s); - auto resumed_stream = harness.stub().Teleoperate(&resumed_context); - ASSERT_TRUE(resumed_stream->Write(makeOpenFrame(makeManifest()))); - expectOpeningFrames(*resumed_stream); - ASSERT_TRUE(resumed_stream->Write(makeStop())); - ASSERT_TRUE(resumed_stream->WritesDone()); - ASSERT_TRUE(resumed_stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_STOPPED); - EXPECT_TRUE(resumed_stream->Finish().ok()); - EXPECT_EQ(backend->openCalls(), 2); - - authority.clear(); - admission.clearForTesting(); -} - -TEST(ArmTeleopServiceTest, - CoordinatorInvalidationStopsExistingSessionBeforeAnotherSetpoint) -{ - safety::SafetyManagerConfig config; - config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyManager coordinator(config); - auto endpoint = std::make_shared(); - ASSERT_TRUE(coordinator.registerDevice( - {endpoint->descriptor(), endpoint, {}})); - coordinator.updateDeviceRuntimeState( - makeManifest().robot_id(), - device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - auto backend = std::make_shared(); - TeleopServerHarness harness(backend, &coordinator); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 2s); - auto stream = harness.stub().Teleoperate(&context); - - ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest(), 500, 1500))); - expectOpeningFrames(*stream); - EXPECT_EQ(endpoint->hardwareChecks(), 1); - - ASSERT_TRUE(stream->Write(makeSetpoint(1))); - arm_teleop::ServerFrame response; - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ(response.status().phase(), arm_teleop::SESSION_PHASE_ACTIVE); - EXPECT_EQ(response.status().applied_sequence(), 1U); - EXPECT_EQ(endpoint->hardwareChecks(), 2); - - coordinator.quarantineDevice( - makeManifest().robot_id(), - safety::SafetyReason::OutcomeUnknown, - "test-quarantine"); - - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ( - response.status().phase(), - arm_teleop::SESSION_PHASE_LEASE_LOST); - EXPECT_EQ( - response.status().stop_reason(), - arm_teleop::STOP_REASON_EMERGENCY_STOP); - const auto status = stream->Finish(); - EXPECT_TRUE( - status.error_code() == grpc::StatusCode::FAILED_PRECONDITION || - status.error_code() == grpc::StatusCode::CANCELLED) - << status.error_message(); - EXPECT_EQ(backend->appliedSequences(), - (std::vector{1U})); - ASSERT_FALSE(backend->stopReasons().empty()); - EXPECT_EQ( - backend->stopReasons().back(), - arm_teleop::STOP_REASON_EMERGENCY_STOP); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_command_transaction_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_command_transaction_test.cpp deleted file mode 100644 index 3ded4775..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_command_transaction_test.cpp +++ /dev/null @@ -1,454 +0,0 @@ -#include "service/grpc/server/include/grpc_command_transaction.h" - -#include -#include -#include -#include -#include - -#include - -#include "cmvr/api/arm_command.pb.h" -#include "manager/safety_manager/include/device_safety_endpoint.h" - -namespace cmvr::service { -namespace { - -class FakeEndpoint final : public safety::DeviceSafetyEndpoint { -public: - explicit FakeEndpoint(std::string device_id) - { - descriptor_.device_id = std::move(device_id); - descriptor_.kind = device::DeviceKind::Arm; - descriptor_.default_policy = safety::SafetyPolicyFamily::Control; - descriptor_.maximum_snapshot_age = std::chrono::seconds(1); - descriptor_.supports_active_refresh = true; - } - - safety::DeviceSafetyDescriptor descriptor() const override - { - return descriptor_; - } - - void bindPublisher(safety::SafetySnapshotPublisher publisher) override - { - publisher_ = std::move(publisher); - } - - void requestSafetyRefresh() noexcept override - { - if (!publisher_) { - return; - } - safety::DeviceSafetySnapshot snapshot; - snapshot.device_id = descriptor_.device_id; - snapshot.condition = safety::SafetyCondition::Nominal; - snapshot.device_generation = 1; - snapshot.sample_sequence = ++sequence_; - snapshot.observed_at = safety::SafetyClock::now(); - snapshot.connected = safety::TriState::True; - snapshot.operational_ready = safety::TriState::True; - snapshot.quiescent = safety::TriState::True; - snapshot.motion_active = safety::TriState::False; - snapshot.actuator_enabled = safety::TriState::True; - snapshot.emergency_stop_active = safety::TriState::False; - snapshot.protective_stop_active = safety::TriState::False; - snapshot.fault_active = safety::TriState::False; - (void)publisher_(std::move(snapshot)); - } - - safety::HardwareCheckResult validateBeforeDispatch( - const safety::AdmissionPermit&) override - { - ++hardware_checks; - return final_check; - } - - safety::RecoveryCheckResult reconcileAdmissionState( - const safety::RecoveryContext&) override - { - return {true, safety::SafetyReason::None, {}}; - } - - safety::DeviceSafetyDescriptor descriptor_; - safety::SafetySnapshotPublisher publisher_; - safety::HardwareCheckResult final_check{ - true, safety::SafetyReason::None, {}}; - std::atomic sequence_{0}; - std::atomic hardware_checks{0}; -}; - -api::MoveJ_Request moveRequest( - const safety::SafetyManager& coordinator, - const std::string& command_id, - const double target = 0.25) -{ - api::MoveJ_Request request; - request.mutable_header()->set_device_id("arm"); - request.mutable_header()->set_command_id(command_id); - request.mutable_header()->set_expected_service_instance_id( - coordinator.serviceInstanceId()); - request.mutable_header()->set_valid_for_ms(1000); - request.mutable_target()->add_position(target); - return request; -} - -grpc::Status executeMove( - safety::SafetyManager& coordinator, - const api::MoveJ_Request& request, - api::MoveJ_Response& response, - int& dispatches, - const bool throw_after_dispatch = false) -{ - grpc::ServerContext context; - return executeRegisteredGrpcCommand( - makeDefaultGrpcSecurityGateway(), - &context, - coordinator, - "/cmvr.api.ArmService/moveJ", - &request, - &response, - [&](GrpcCommandTransaction& transaction) { - if (!transaction.beginDispatch()) { - return transaction.dispatchStatus(); - } - ++dispatches; - if (throw_after_dispatch) { - throw std::runtime_error("simulated lost driver acknowledgement"); - } - response.mutable_header()->set_success(true); - return grpc::Status::OK; - }); -} - -TEST(GrpcCommandTransactionTest, - DeterministicHashIgnoresRetryIdentityButIncludesPayload) -{ - safety::SafetyManager coordinator; - auto first = moveRequest(coordinator, "command-1", 0.25); - auto retry = first; - retry.mutable_header()->set_command_id("command-2"); - retry.mutable_header()->set_valid_for_ms(2000); - retry.mutable_header()->mutable_timestamp()->set_seconds(1234); - auto changed = retry; - changed.mutable_target()->set_position(0, 0.5); - - const auto first_hash = deterministicGrpcPayloadHash( - "/cmvr.api.ArmService/moveJ", first); - EXPECT_FALSE(first_hash.empty()); - EXPECT_EQ( - first_hash, - deterministicGrpcPayloadHash( - "/cmvr.api.ArmService/moveJ", retry)); - EXPECT_NE( - first_hash, - deterministicGrpcPayloadHash( - "/cmvr.api.ArmService/moveJ", changed)); - EXPECT_NE( - first_hash, - deterministicGrpcPayloadHash( - "/cmvr.api.ArmService/moveL", first)); -} - -TEST(GrpcCommandTransactionTest, SameIdReturnsCachedResponseWithoutRedispatch) -{ - safety::SafetyManager coordinator; - auto endpoint = std::make_shared("arm"); - ASSERT_TRUE(coordinator.registerDevice( - {endpoint->descriptor(), endpoint, {}})); - coordinator.markStartupComplete(); - - const auto request = moveRequest(coordinator, "command-cache"); - api::MoveJ_Response first_response; - int dispatches = 0; - ASSERT_TRUE(executeMove( - coordinator, request, first_response, dispatches).ok()); - ASSERT_TRUE(first_response.header().success()); - EXPECT_EQ(dispatches, 1); - - auto retry = request; - retry.mutable_header()->set_valid_for_ms(2500); - retry.mutable_header()->mutable_timestamp()->set_seconds(42); - api::MoveJ_Response cached_response; - ASSERT_TRUE(executeMove( - coordinator, retry, cached_response, dispatches).ok()); - EXPECT_EQ(dispatches, 1); - EXPECT_TRUE(cached_response.header().success()); - EXPECT_EQ( - cached_response.header().execution_state(), - api::COMMAND_EXECUTION_STATE_COMPLETED); - EXPECT_EQ(cached_response.header().command_id(), "command-cache"); - - auto conflict = request; - conflict.mutable_target()->set_position(0, 0.75); - api::MoveJ_Response conflict_response; - const auto conflict_status = executeMove( - coordinator, conflict, conflict_response, dispatches); - EXPECT_EQ(conflict_status.error_code(), grpc::StatusCode::ALREADY_EXISTS); - EXPECT_EQ(dispatches, 1); - EXPECT_EQ( - conflict_response.header().reason_code(), - api::COMMAND_REASON_CODE_COMMAND_ID_CONFLICT); -} - -TEST(GrpcCommandTransactionTest, - EnforcedCommandRequiresIdentityAndRunsFinalHardwareCheck) -{ - safety::SafetyManagerConfig config; - config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyManager coordinator(config); - auto endpoint = std::make_shared("arm"); - ASSERT_TRUE(coordinator.registerDevice( - {endpoint->descriptor(), endpoint, {}})); - coordinator.updateDeviceRuntimeState( - "arm", - device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - auto missing_identity = moveRequest(coordinator, ""); - api::MoveJ_Response rejected; - int dispatches = 0; - const auto rejected_status = executeMove( - coordinator, missing_identity, rejected, dispatches); - EXPECT_EQ( - rejected_status.error_code(), grpc::StatusCode::INVALID_ARGUMENT); - EXPECT_EQ(dispatches, 0); - EXPECT_EQ( - rejected.header().reason_code(), - api::COMMAND_REASON_CODE_COMMAND_ID_REQUIRED); - - auto accepted = moveRequest(coordinator, "command-enforced"); - api::MoveJ_Response accepted_response; - ASSERT_TRUE(executeMove( - coordinator, accepted, accepted_response, dispatches).ok()); - EXPECT_EQ(dispatches, 1); - EXPECT_EQ(endpoint->hardware_checks.load(), 1); - EXPECT_EQ( - accepted_response.header().device_generation(), 1U); -} - -TEST(GrpcCommandTransactionTest, - ExceptionAfterDispatchIsQuarantinedAndNeverRedispatched) -{ - safety::SafetyManagerConfig config; - config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyManager coordinator(config); - auto endpoint = std::make_shared("arm"); - ASSERT_TRUE(coordinator.registerDevice( - {endpoint->descriptor(), endpoint, {}})); - coordinator.updateDeviceRuntimeState( - "arm", - device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - const auto request = moveRequest(coordinator, "command-unknown"); - api::MoveJ_Response response; - int dispatches = 0; - ASSERT_TRUE(executeMove( - coordinator, request, response, dispatches, true).ok()); - EXPECT_EQ(dispatches, 1); - EXPECT_EQ( - response.header().reason_code(), - api::COMMAND_REASON_CODE_OUTCOME_UNKNOWN); - ASSERT_EQ(coordinator.snapshot().devices.size(), 1U); - EXPECT_EQ( - coordinator.snapshot().devices.front().admission_state, - safety::DeviceAdmissionState::Quarantined); - - api::MoveJ_Response retry_response; - ASSERT_TRUE(executeMove( - coordinator, request, retry_response, dispatches).ok()); - EXPECT_EQ(dispatches, 1); - EXPECT_EQ( - retry_response.header().execution_state(), - api::COMMAND_EXECUTION_STATE_OUTCOME_UNKNOWN); -} - -TEST(GrpcCommandTransactionTest, - ScopedDispatchChecksHardwareForEverySubmission) -{ - safety::SafetyManagerConfig config; - config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyManager coordinator(config); - auto endpoint = std::make_shared("arm"); - ASSERT_TRUE(coordinator.registerDevice( - {endpoint->descriptor(), endpoint, {}})); - coordinator.updateDeviceRuntimeState( - "arm", - device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - const auto request = moveRequest(coordinator, "command-scoped"); - api::MoveJ_Response response; - grpc::ServerContext context; - int dispatches = 0; - const auto status = executeRegisteredGrpcCommand( - makeDefaultGrpcSecurityGateway(), - &context, - coordinator, - "/cmvr.api.ArmService/moveJ", - &request, - &response, - [&](GrpcCommandTransaction& transaction) { - for (int index = 0; index < 2; ++index) { - auto dispatch = transaction.beginScopedDispatch(); - if (!dispatch.acquired()) { - return transaction.dispatchStatus(); - } - ++dispatches; - } - response.mutable_header()->set_success(true); - return grpc::Status::OK; - }); - - EXPECT_TRUE(status.ok()); - EXPECT_TRUE(response.header().success()); - EXPECT_EQ(dispatches, 2); - EXPECT_EQ(endpoint->hardware_checks.load(), 2); -} - -TEST(GrpcCommandTransactionTest, - RevokedActuationPermitCannotSuppressInternalSafetyStop) -{ - safety::SafetyManagerConfig config; - config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyManager coordinator(config); - auto endpoint = std::make_shared("arm"); - ASSERT_TRUE(coordinator.registerDevice( - {endpoint->descriptor(), endpoint, {}})); - coordinator.updateDeviceRuntimeState( - "arm", - device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - - const auto request = moveRequest(coordinator, "command-revoked"); - api::MoveJ_Response response; - grpc::ServerContext context; - int actuations = 0; - int safety_stops = 0; - const auto status = executeRegisteredGrpcCommand( - makeDefaultGrpcSecurityGateway(), - &context, - coordinator, - "/cmvr.api.ArmService/moveJ", - &request, - &response, - [&](GrpcCommandTransaction& transaction) { - { - auto dispatch = transaction.beginScopedDispatch(); - if (!dispatch.acquired()) { - return transaction.dispatchStatus(); - } - ++actuations; - } - - const auto stop_all = coordinator.stopAll("revoke-scoped-permit"); - EXPECT_TRUE(stop_all.success); - EXPECT_FALSE(transaction.revalidate()); - const auto revoked_status = transaction.dispatchStatus(); - EXPECT_FALSE(transaction.beginScopedDispatch().acquired()); - - auto stop_dispatch = transaction.beginSafetyStopDispatch(); - if (stop_dispatch.acquired()) { - ++safety_stops; - } - response.mutable_header()->set_success(false); - return revoked_status; - }); - - // Ledger-backed commands report their terminal outcome in Feedback. - EXPECT_TRUE(status.ok()); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ( - response.header().reason_code(), - api::COMMAND_REASON_CODE_SAFETY_LATCHED); - EXPECT_EQ(actuations, 1); - EXPECT_EQ(safety_stops, 1); - EXPECT_EQ(endpoint->hardware_checks.load(), 2); -} - -TEST(GrpcCommandTransactionTest, - ServerDerivedStopRemainsDispatchableWhenActuationIsQuarantined) -{ - safety::SafetyManagerConfig config; - config.enforcement_mode = safety::EnforcementMode::EnforceAll; - safety::SafetyManager coordinator(config); - auto endpoint = std::make_shared("arm"); - ASSERT_TRUE(coordinator.registerDevice( - {endpoint->descriptor(), endpoint, {}})); - coordinator.updateDeviceRuntimeState( - "arm", - device::ManagedDeviceState::Running, - {device::DeviceHealthState::Healthy, {}}); - coordinator.markStartupComplete(); - coordinator.quarantineDevice( - "arm", safety::SafetyReason::OutcomeUnknown, - "uncertain-ptz-start"); - - const auto registered = defaultGrpcMethodPolicyRegistry().find( - "/cmvr.api.CameraService/ControlPtz"); - ASSERT_TRUE(registered.has_value()); - - auto start_request = moveRequest(coordinator, "ptz-start"); - api::MoveJ_Response start_response; - grpc::ServerContext start_context; - int start_dispatches = 0; - const auto start_status = executeServerDerivedGrpcCommand( - makeDefaultGrpcSecurityGateway(), - &start_context, - coordinator, - "/cmvr.api.CameraService/ControlPtz", - *registered, - &start_request, - &start_response, - [&](GrpcCommandTransaction& transaction) { - if (!transaction.beginDispatch()) { - return transaction.dispatchStatus(); - } - ++start_dispatches; - start_response.mutable_header()->set_success(true); - return grpc::Status::OK; - }); - EXPECT_TRUE(start_status.ok()); - EXPECT_EQ(start_dispatches, 0); - EXPECT_FALSE(start_response.header().success()); - EXPECT_EQ( - start_response.header().reason_code(), - api::COMMAND_REASON_CODE_SAFETY_LATCHED); - - auto stop_request = moveRequest(coordinator, "ptz-stop"); - api::MoveJ_Response stop_response; - grpc::ServerContext stop_context; - auto stop_policy = *registered; - stop_policy.access = GrpcAccessClass::Stop; - stop_policy.command_intent = safety::CommandIntent::Stop; - stop_policy.safety_lane = true; - int stop_dispatches = 0; - const auto stop_status = executeServerDerivedGrpcCommand( - makeDefaultGrpcSecurityGateway(), - &stop_context, - coordinator, - "/cmvr.api.CameraService/ControlPtz", - std::move(stop_policy), - &stop_request, - &stop_response, - [&](GrpcCommandTransaction& transaction) { - if (!transaction.beginDispatch()) { - return transaction.dispatchStatus(); - } - ++stop_dispatches; - stop_response.mutable_header()->set_success(true); - return grpc::Status::OK; - }); - EXPECT_TRUE(stop_status.ok()); - EXPECT_TRUE(stop_response.header().success()); - EXPECT_EQ(stop_dispatches, 1); - EXPECT_EQ(endpoint->hardware_checks.load(), 1); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_dexhand_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_dexhand_service_test.cpp deleted file mode 100644 index 6638b27e..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_dexhand_service_test.cpp +++ /dev/null @@ -1,351 +0,0 @@ -#include "service/grpc/server/include/grpc_dexhand_service.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" -#include "manager/device_manager/include/device_manager.h" -#include "service/grpc/server/include/media_activity_coordinator.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { -namespace { - -using namespace std::chrono_literals; - -class FakeDexHand final : public device::AbstractDexHand { -public: - FakeDexHand() { - id_ = "test-dexhand"; - sensor_points_[0] = TactilePoint::fromFz(42); - } - - std::string typeName() const override { return "FakeDexHand"; } - Status state() const override { return Status::STREAMING; } - std::string lastError() const override { return {}; } - - bool stopOperationalActivity() override { - ++stop_operational_calls; - return true; - } - - bool resumeOperationalActivity() override { - ++resume_operational_calls; - return true; - } - - void setAngles(const std::vector&) override { - ++set_angle_calls; - std::unique_lock lock(command_mutex_); - command_entered_ = true; - command_cv_.notify_all(); - command_cv_.wait(lock, [this] { return !block_angle_command_; }); - } - - void setPositions(const std::vector&) override { - ++set_position_calls; - } - - void setVelocities(const std::vector&) override { - ++set_speed_calls; - } - - void setForce(const std::vector&) override { - ++set_force_calls; - } - - void setPresetAct(int) override { - ++set_preset_calls; - } - - void setTactilePollingRegions( - const std::vector&) override { - ++configure_sensor_calls; - } - - std::vector getSensorData() override { - ++sensor_read_calls; - return {sensorRegion()}; - } - - TactileRegionData getSensorData( - FingerType finger, TactileRegion region) override { - return finger == FingerType::INDEX && region == TactileRegion::TIP - ? sensorRegion() - : TactileRegionData{}; - } - - ResultantForce getResultantForce( - FingerType finger, TactileRegion region) override { - return finger == FingerType::INDEX && region == TactileRegion::TIP - ? sensor_points_[0] - : ResultantForce{}; - } - - void blockAngleCommand() { - std::lock_guard lock(command_mutex_); - block_angle_command_ = true; - command_entered_ = false; - } - - bool waitForAngleCommand(const std::chrono::milliseconds timeout) { - std::unique_lock lock(command_mutex_); - return command_cv_.wait_for( - lock, timeout, [this] { return command_entered_; }); - } - - void releaseAngleCommand() { - { - std::lock_guard lock(command_mutex_); - block_angle_command_ = false; - } - command_cv_.notify_all(); - } - - std::atomic set_position_calls{0}; - std::atomic set_angle_calls{0}; - std::atomic set_force_calls{0}; - std::atomic set_speed_calls{0}; - std::atomic set_preset_calls{0}; - std::atomic configure_sensor_calls{0}; - std::atomic sensor_read_calls{0}; - std::atomic stop_operational_calls{0}; - std::atomic resume_operational_calls{0}; - -private: - TactileRegionData sensorRegion() { - return TactileRegionData( - FingerType::INDEX, - TactileRegion::TIP, - TactileMatrixView{sensor_points_.data(), 1, 1}, - "fake-tactile"); - } - - std::array sensor_points_{}; - std::mutex command_mutex_; - std::condition_variable command_cv_; - bool block_angle_command_{false}; - bool command_entered_{false}; -}; - -class GrpcDexHandServiceTest : public ::testing::Test { -protected: - void SetUp() override { - device::DeviceManager::destroyInstance(); - control::ControlAuthorityManager::instance().clear(); - globalStopAllAdmissionGate().clearForTesting(); - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - hand_ = std::make_shared(); - manager.registerDevice(hand_); - service_ = std::make_unique(); - } - - void TearDown() override { - hand_->releaseAngleCommand(); - if (server_) { - server_->Shutdown(); - server_->Wait(); - } - stub_.reset(); - service_.reset(); - hand_.reset(); - if (!socket_path_.empty()) { - std::remove(socket_path_.c_str()); - } - device::DeviceManager::destroyInstance(); - control::ControlAuthorityManager::instance().clear(); - globalStopAllAdmissionGate().clearForTesting(); - } - - bool startGrpcServer() { - socket_path_ = - "/tmp/cmvr_dexhand_service_test_" + - std::to_string(static_cast(::getpid())) + ".sock"; - std::remove(socket_path_.c_str()); - const std::string address = "unix:" + socket_path_; - grpc::ServerBuilder builder; - builder.AddListeningPort( - address, - grpc::InsecureServerCredentials()); - builder.RegisterService(service_.get()); - server_ = builder.BuildAndStart(); - if (!server_) { - return false; - } - stub_ = api::DexHandService::NewStub(grpc::CreateChannel( - address, - grpc::InsecureChannelCredentials())); - return stub_ != nullptr; - } - - static api::GetSensorDataStreamCommand_Request sensorStreamRequest() { - api::GetSensorDataStreamCommand_Request request; - request.mutable_header()->set_device_id("test-dexhand"); - return request; - } - - std::shared_ptr hand_; - std::unique_ptr service_; - std::unique_ptr server_; - std::unique_ptr stub_; - std::string socket_path_; -}; - -TEST_F(GrpcDexHandServiceTest, StopAllGateRejectsEveryControlCommand) { - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - ASSERT_TRUE(ticket.valid()); - - grpc::ServerContext position_context; - api::SetDexHandPositionsCommand_Request position_request; - api::SetDexHandPositionsCommand_Feedback position_response; - position_request.mutable_header()->set_device_id("test-dexhand"); - EXPECT_TRUE(service_->SetDexHandPos( - &position_context, &position_request, &position_response).ok()); - EXPECT_FALSE(position_response.header().success()); - - grpc::ServerContext angle_context; - api::SetDexHandAnglesCommand_Request angle_request; - api::SetDexHandAnglesCommand_Feedback angle_response; - angle_request.mutable_header()->set_device_id("test-dexhand"); - EXPECT_TRUE(service_->SetDexHandAngle( - &angle_context, &angle_request, &angle_response).ok()); - EXPECT_FALSE(angle_response.header().success()); - - grpc::ServerContext force_context; - api::SetDexHandForceCommand_Request force_request; - api::SetDexHandForceCommand_Feedback force_response; - force_request.mutable_header()->set_device_id("test-dexhand"); - EXPECT_TRUE(service_->SetDexHandForce( - &force_context, &force_request, &force_response).ok()); - EXPECT_FALSE(force_response.header().success()); - - grpc::ServerContext speed_context; - api::SetDexHandSpeedCommand_Request speed_request; - api::SetDexHandSpeedCommand_Feedback speed_response; - speed_request.mutable_header()->set_device_id("test-dexhand"); - EXPECT_TRUE(service_->SetDexHandSpeed( - &speed_context, &speed_request, &speed_response).ok()); - EXPECT_FALSE(speed_response.header().success()); - - grpc::ServerContext preset_context; - api::SetDexHandPresetActCommand_Request preset_request; - api::SetDexHandPresetActCommand_Feedback preset_response; - preset_request.mutable_header()->set_device_id("test-dexhand"); - EXPECT_TRUE(service_->SetDexHandPresetAct( - &preset_context, &preset_request, &preset_response).ok()); - EXPECT_FALSE(preset_response.header().success()); - - EXPECT_EQ(hand_->set_position_calls.load(), 0); - EXPECT_EQ(hand_->set_angle_calls.load(), 0); - EXPECT_EQ(hand_->set_force_calls.load(), 0); - EXPECT_EQ(hand_->set_speed_calls.load(), 0); - EXPECT_EQ(hand_->set_preset_calls.load(), 0); - EXPECT_EQ(hand_->resume_operational_calls.load(), 0); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); -} - -TEST_F(GrpcDexHandServiceTest, - SafetyPreemptionCannotPassAnExecutingDeviceDispatch) { - hand_->blockAngleCommand(); - grpc::ServerContext context; - api::SetDexHandAnglesCommand_Request request; - api::SetDexHandAnglesCommand_Feedback response; - request.mutable_header()->set_device_id("test-dexhand"); - request.add_values()->set_value(0.5F); - - auto rpc = std::async(std::launch::async, [&] { - return service_->SetDexHandAngle(&context, &request, &response); - }); - ASSERT_TRUE(hand_->waitForAngleCommand(1s)); - - auto& authority = control::ControlAuthorityManager::instance(); - auto safety_result = authority.preemptAcquire( - "test-dexhand", - "test-stop-all", - std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>(1h)); - ASSERT_TRUE(safety_result.acquired); - const auto safety_token = safety_result.token; - auto dispatch_fence = std::async(std::launch::async, - [&authority, safety_token] { - return authority.waitForPreemptedRelease(safety_token, 1s); - }); - EXPECT_EQ( - dispatch_fence.wait_for(50ms), std::future_status::timeout); - - hand_->releaseAngleCommand(); - ASSERT_EQ(rpc.wait_for(1s), std::future_status::ready); - EXPECT_TRUE(rpc.get().ok()); - EXPECT_TRUE(response.header().success()); - ASSERT_EQ(dispatch_fence.wait_for(1s), std::future_status::ready); - EXPECT_TRUE(dispatch_fence.get()); - authority.release(safety_token); -} - -TEST_F(GrpcDexHandServiceTest, - StopAllCancelsOldSensorStreamAndARecoveredStreamResumesActivity) { - ASSERT_TRUE(startGrpcServer()); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 3s); - auto stream = stub_->GetSensorDataStream(&context); - ASSERT_TRUE(stream->Write(sensorStreamRequest())); - - api::GetSensorDataStreamCommand_Feedback feedback; - ASSERT_TRUE(stream->Read(&feedback)); - ASSERT_TRUE(feedback.header().success()); - ASSERT_GE(hand_->sensor_read_calls.load(), 1); - - const auto admission_ticket = globalStopAllAdmissionGate().beginStopAll(); - const auto media_ticket = globalMediaActivityCoordinator().beginStopAll(); - ASSERT_TRUE(admission_ticket.valid()); - ASSERT_TRUE(media_ticket.valid()); - - stream->WritesDone(); - while (stream->Read(&feedback)) { - } - const auto status = stream->Finish(); - EXPECT_TRUE( - status.ok() || status.error_code() == grpc::StatusCode::CANCELLED); - EXPECT_TRUE(globalMediaActivityCoordinator().waitForStopped( - media_ticket, 1s)); - const int stopped_stream_reads = hand_->sensor_read_calls.load(); - std::this_thread::sleep_for(50ms); - EXPECT_EQ(hand_->sensor_read_calls.load(), stopped_stream_reads); - EXPECT_TRUE(globalMediaActivityCoordinator().finishStopAll( - media_ticket, true)); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll( - admission_ticket, true)); - - grpc::ClientContext resumed_context; - resumed_context.set_deadline(std::chrono::system_clock::now() + 3s); - auto resumed = stub_->GetSensorDataStream(&resumed_context); - ASSERT_TRUE(resumed->Write(sensorStreamRequest())); - ASSERT_TRUE(resumed->Read(&feedback)); - EXPECT_TRUE(feedback.header().success()); - EXPECT_GT(hand_->sensor_read_calls.load(), stopped_stream_reads); - EXPECT_GE(hand_->resume_operational_calls.load(), 2); - resumed_context.TryCancel(); - resumed->WritesDone(); - while (resumed->Read(&feedback)) { - } - (void)resumed->Finish(); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_error_logging_interceptor_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_error_logging_interceptor_test.cpp deleted file mode 100644 index 2287e2d2..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_error_logging_interceptor_test.cpp +++ /dev/null @@ -1,864 +0,0 @@ -#include "service/grpc/server/include/grpc_error_logging_interceptor.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "cmvr/api/agv_service.grpc.pb.h" -#include "cmvr/api/camera_service.grpc.pb.h" -#include "cmvr/api/test_service.grpc.pb.h" -#include "common/base/logging/logger.h" - -namespace cmvr::service { -namespace { - -std::atomic g_socket_sequence{0}; - -void setRpcDeadline(grpc::ClientContext& context) -{ - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::seconds(3)); -} - -grpc::ByteBuffer byteBuffer(const std::string& payload) -{ - const grpc::Slice slice(payload); - return grpc::ByteBuffer(&slice, 1); -} - -std::string byteBufferString(const grpc::ByteBuffer& buffer) -{ - std::vector slices; - EXPECT_TRUE(buffer.Dump(&slices).ok()); - std::string payload; - for (const auto& slice : slices) { - payload.append( - reinterpret_cast(slice.begin()), slice.size()); - } - return payload; -} - -grpc::Status rawUnaryCall(const std::shared_ptr& channel, - const std::string& method, - const std::string& request_payload, - grpc::ByteBuffer& response) -{ - grpc::ClientContext context; - setRpcDeadline(context); - const auto request = byteBuffer(request_payload); - const grpc::internal::RpcMethod rpc_method( - method.c_str(), grpc::internal::RpcMethod::NORMAL_RPC); - return grpc::internal::BlockingUnaryCall( - channel.get(), rpc_method, &context, request, &response); -} - -class ErrorLoggingTestService final : public api::TestService::Service { -public: - grpc::Status Call(grpc::ServerContext* context, - const api::TestReqeust* request, - api::TestResponse* response) override - { - if (request->data() == "grpc-error") { - return grpc::Status( - grpc::StatusCode::INTERNAL, "transport sentinel"); - } - if (request->data() == "cancelled") { - return grpc::Status( - grpc::StatusCode::CANCELLED, "client closed stream"); - } - if (request->data() == "server-cancel") { - context->TryCancel(); - return grpc::Status::OK; - } - if (request->data() == "client-cancel") { - { - std::lock_guard lock(cancel_mutex_); - cancel_started_ = true; - } - cancel_started_condition_.notify_all(); - const auto timeout = - std::chrono::steady_clock::now() + std::chrono::seconds(3); - while (!context->IsCancelled() && - std::chrono::steady_clock::now() < timeout) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - return grpc::Status::OK; - } - response->set_data("ok"); - return grpc::Status::OK; - } - - bool waitForCancelableCall() - { - std::unique_lock lock(cancel_mutex_); - return cancel_started_condition_.wait_for( - lock, std::chrono::seconds(3), - [this] { return cancel_started_; }); - } - -private: - std::mutex cancel_mutex_; - std::condition_variable cancel_started_condition_; - bool cancel_started_{false}; -}; - -class ErrorLoggingAgvService final : public api::AgvService::Service { -public: - grpc::Status emergencyStop( - grpc::ServerContext*, - const api::CommandHeader_Request* request, - api::CommandHeader_Feedback* response) override - { - if (request->device_id() == "missing") { - response->set_success(false); - response->set_error_message("missing sentinel"); - return grpc::Status( - grpc::StatusCode::NOT_FOUND, "missing sentinel"); - } - response->set_success(false); - response->set_error_message("direct sentinel"); - return grpc::Status::OK; - } - - grpc::Status streamMap( - grpc::ServerContext*, - const api::AgvMapStreamCommand_Request* request, - grpc::ServerWriter* writer) override - { - const auto write_failure = [writer](const std::string& detail) { - api::AgvMapStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message(detail); - return writer->Write(response); - }; - - if (request->resume_token() == "multiple") { - write_failure("first stream sentinel"); - write_failure("second stream sentinel"); - return grpc::Status::OK; - } - if (request->resume_token() == "hold") { - write_failure("immediate stream sentinel"); - std::unique_lock lock(stream_mutex_); - stream_released_.wait_for( - lock, std::chrono::seconds(3), - [this] { return release_stream_; }); - return grpc::Status::OK; - } - if (request->resume_token() == "different") { - write_failure("application stream sentinel"); - return grpc::Status( - grpc::StatusCode::INTERNAL, "status stream sentinel"); - } - if (request->resume_token() == "long-different") { - const std::string common_prefix(1100, 'x'); - write_failure(common_prefix + " application"); - return grpc::Status( - grpc::StatusCode::INTERNAL, - common_prefix + " status"); - } - - write_failure("stream sentinel"); - return grpc::Status( - grpc::StatusCode::INTERNAL, "stream sentinel"); - } - - void releaseHeldStream() - { - { - std::lock_guard lock(stream_mutex_); - release_stream_ = true; - } - stream_released_.notify_all(); - } - -private: - std::mutex stream_mutex_; - std::condition_variable stream_released_; - bool release_stream_{false}; -}; - -class ErrorLoggingCameraService final : public api::CameraService::Service { -public: - grpc::Status StartCamera( - grpc::ServerContext*, - const api::StartCameraCommand_Request*, - api::StartCameraCommand_Feedback* response) override - { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message("nested sentinel"); - return grpc::Status::OK; - } - - grpc::Status GetRGBImage( - grpc::ServerContext*, - const api::GetRGBImageCommand_Request*, - api::GetRGBImageCommand_Feedback* response) override - { - response->mutable_header()->set_success(true); - response->mutable_color_frame()->set_data( - std::string(2 * 1024 * 1024, 'x')); - return grpc::Status::OK; - } -}; - -class RawCameraService final - : public api::CameraService::WithRawCallbackMethod_StartCamera< - api::CameraService::Service> { -public: - explicit RawCameraService(std::string response) - : response_(std::move(response)) - { - } - - grpc::ServerUnaryReactor* StartCamera( - grpc::CallbackServerContext* context, - const grpc::ByteBuffer*, - grpc::ByteBuffer* response) override - { - *response = byteBuffer(response_); - auto* reactor = context->DefaultReactor(); - reactor->Finish(grpc::Status::OK); - return reactor; - } - -private: - std::string response_; -}; - -struct RawCallResult { - grpc::Status status; - std::vector records; - std::string response; -}; - -class ScopedServer final { -public: - ScopedServer(std::unique_ptr server, - std::string socket_path = {}) - : server_(std::move(server)), socket_path_(std::move(socket_path)) - { - } - - ~ScopedServer() - { - if (server_) { - server_->Shutdown(); - server_->Wait(); - } - if (!socket_path_.empty()) { - std::remove(socket_path_.c_str()); - } - } - - grpc::Server* get() const { return server_.get(); } - -private: - std::unique_ptr server_; - std::string socket_path_; -}; - -class ScopedSocket final { -public: - explicit ScopedSocket(std::string path) : path_(std::move(path)) - { - std::remove(path_.c_str()); - } - - ~ScopedSocket() { std::remove(path_.c_str()); } - -private: - std::string path_; -}; - -RawCallResult callRawStartCamera(const std::string& response_payload) -{ - std::mutex records_mutex; - std::vector records; - RawCameraService service(response_payload); - grpc::ServerBuilder builder; - const std::string address = - "unix:/tmp/cmvr_grpc_error_logging_raw_" + - std::to_string(static_cast(::getpid())) + "_" + - std::to_string(g_socket_sequence.fetch_add(1U)) + ".sock"; - const std::string socket_path = address.substr(5); - ScopedSocket socket(socket_path); - builder.AddListeningPort(address, grpc::InsecureServerCredentials()); - builder.RegisterService(&service); - std::vector> factories; - factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory( - [&records, &records_mutex](const GrpcFailureRecord& record) { - std::lock_guard lock(records_mutex); - records.push_back(record); - })); - builder.experimental().SetInterceptorCreators(std::move(factories)); - ScopedServer server(builder.BuildAndStart()); - EXPECT_NE(server.get(), nullptr); - if (server.get() == nullptr) { - return {}; - } - - const auto channel = grpc::CreateChannel( - address, grpc::InsecureChannelCredentials()); - api::StartCameraCommand_Request request; - grpc::ByteBuffer response; - const auto status = rawUnaryCall( - channel, "/cmvr.api.CameraService/StartCamera", - request.SerializeAsString(), response); - std::lock_guard lock(records_mutex); - return {status, records, byteBufferString(response)}; -} - -class GrpcErrorLoggingInterceptorTest : public testing::Test { -protected: - void SetUp() override - { - grpc::ServerBuilder builder; - socket_path_ = - "/tmp/cmvr_grpc_error_logging_interceptor_test_" + - std::to_string(static_cast(::getpid())) + "_" + - std::to_string(g_socket_sequence.fetch_add(1U)) + ".sock"; - std::remove(socket_path_.c_str()); - const std::string address = "unix:" + socket_path_; - builder.AddListeningPort( - address, grpc::InsecureServerCredentials()); - builder.RegisterService(&test_service_); - builder.RegisterService(&agv_service_); - builder.RegisterService(&camera_service_); - - std::vector> factories; - factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory( - [this](const GrpcFailureRecord& record) { - { - std::lock_guard lock(records_mutex_); - records_.push_back(record); - } - records_changed_.notify_all(); - })); - builder.experimental().SetInterceptorCreators(std::move(factories)); - server_ = builder.BuildAndStart(); - ASSERT_NE(server_, nullptr); - - const auto channel = grpc::CreateChannel( - address, - grpc::InsecureChannelCredentials()); - test_stub_ = api::TestService::NewStub(channel); - agv_stub_ = api::AgvService::NewStub(channel); - camera_stub_ = api::CameraService::NewStub(channel); - } - - void TearDown() override - { - if (server_) { - server_->Shutdown(); - server_->Wait(); - } - test_stub_.reset(); - agv_stub_.reset(); - camera_stub_.reset(); - if (!socket_path_.empty()) { - std::remove(socket_path_.c_str()); - } - } - - std::vector records() - { - std::lock_guard lock(records_mutex_); - return records_; - } - - std::vector waitForRecords( - const std::size_t count, - const std::chrono::milliseconds timeout = std::chrono::seconds(3)) - { - std::unique_lock lock(records_mutex_); - records_changed_.wait_for( - lock, timeout, [this, count] { return records_.size() >= count; }); - return records_; - } - - ErrorLoggingTestService test_service_; - ErrorLoggingAgvService agv_service_; - ErrorLoggingCameraService camera_service_; - std::unique_ptr server_; - std::unique_ptr test_stub_; - std::unique_ptr agv_stub_; - std::unique_ptr camera_stub_; - std::string socket_path_; - std::mutex records_mutex_; - std::condition_variable records_changed_; - std::vector records_; -}; - -TEST_F(GrpcErrorLoggingInterceptorTest, RecordsNonOkGrpcStatus) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::TestReqeust request; - api::TestResponse response; - request.set_data("grpc-error"); - - const grpc::Status status = test_stub_->Call(&context, request, &response); - ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - - const auto captured = waitForRecords(1U); - ASSERT_EQ(captured.size(), 1U); - EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS); - EXPECT_EQ(captured.front().method, "/cmvr.api.TestService/Call"); - EXPECT_EQ(captured.front().status_code, grpc::StatusCode::INTERNAL); - EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::ERROR); - EXPECT_EQ(captured.front().detail, "transport sentinel"); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, RecordsDirectFeedbackFailure) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::CommandHeader_Request request; - api::CommandHeader_Feedback response; - - ASSERT_TRUE(agv_stub_->emergencyStop(&context, request, &response).ok()); - ASSERT_FALSE(response.success()); - - const auto captured = waitForRecords(1U); - ASSERT_EQ(captured.size(), 1U); - EXPECT_EQ(captured.front().kind, GrpcFailureKind::APPLICATION); - EXPECT_EQ(captured.front().method, "/cmvr.api.AgvService/emergencyStop"); - EXPECT_EQ(captured.front().status_code, grpc::StatusCode::OK); - EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::ERROR); - EXPECT_EQ(captured.front().detail, "direct sentinel"); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, RecordsCancellationAsWarning) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::TestReqeust request; - api::TestResponse response; - request.set_data("cancelled"); - - const grpc::Status status = test_stub_->Call(&context, request, &response); - ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED); - - const auto captured = waitForRecords(1U); - ASSERT_EQ(captured.size(), 1U); - EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS); - EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING); - EXPECT_EQ(captured.front().detail, "client closed stream"); -} - -TEST(GrpcErrorLoggingInterceptorFileTest, WarningStatusIsFlushedToFile) -{ - std::array directory_pattern{}; - const std::string pattern = "/tmp/cmvr-grpc-log-test-XXXXXX"; - std::copy(pattern.begin(), pattern.end(), directory_pattern.begin()); - const char* created = ::mkdtemp(directory_pattern.data()); - ASSERT_NE(created, nullptr); - const std::filesystem::path directory(created); - - struct Cleanup { - ~Cleanup() - { - logging::shutdownLogging(); - if (!socket_path.empty()) { - std::remove(socket_path.c_str()); - } - std::error_code error; - std::filesystem::remove_all(directory, error); - } - - std::filesystem::path directory; - std::string socket_path; - } cleanup{directory, {}}; - - config::LoggerConfig config; - config.set_minimum_level(config::LOG_LEVEL_WARNING); - config.set_directory(directory.string()); - config.set_flush_interval_seconds(3600); - auto* warning_route = config.add_routes(); - warning_route->set_level(config::LOG_LEVEL_WARNING); - warning_route->set_file(true); - ASSERT_TRUE(logging::initLogging(config, "grpc_log_test", directory)); - - ErrorLoggingTestService service; - grpc::ServerBuilder builder; - const std::string socket_path = - "/tmp/cmvr_grpc_error_logging_file_test_" + - std::to_string(static_cast(::getpid())) + "_" + - std::to_string(g_socket_sequence.fetch_add(1U)) + ".sock"; - cleanup.socket_path = socket_path; - ScopedSocket socket(socket_path); - const std::string address = "unix:" + socket_path; - builder.AddListeningPort(address, grpc::InsecureServerCredentials()); - builder.RegisterService(&service); - std::vector> factories; - factories.emplace_back(makeGrpcErrorLoggingInterceptorFactory()); - builder.experimental().SetInterceptorCreators(std::move(factories)); - ScopedServer server(builder.BuildAndStart()); - ASSERT_NE(server.get(), nullptr); - - const auto channel = grpc::CreateChannel( - address, grpc::InsecureChannelCredentials()); - auto stub = api::TestService::NewStub(channel); - grpc::ClientContext context; - setRpcDeadline(context); - api::TestReqeust request; - api::TestResponse response; - request.set_data("cancelled"); - const grpc::Status status = stub->Call(&context, request, &response); - ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED); - - std::ifstream input(directory / "grpc_log_test.log"); - const std::string contents{ - std::istreambuf_iterator(input), - std::istreambuf_iterator()}; - EXPECT_NE(contents.find("/cmvr.api.TestService/Call"), std::string::npos); - EXPECT_NE(contents.find("code=1(CANCELLED)"), std::string::npos); - EXPECT_NE(contents.find("client closed stream"), std::string::npos); - -} - -TEST_F(GrpcErrorLoggingInterceptorTest, RecordsServerCancellationReturnedAsOk) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::TestReqeust request; - api::TestResponse response; - request.set_data("server-cancel"); - - const grpc::Status status = test_stub_->Call(&context, request, &response); - ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED); - - // TryCancel is best effort: the final status may be intercepted before - // the cancellation hook. Either way, the interceptor must not recurse or - // emit duplicate records. - const auto captured = waitForRecords(1U, std::chrono::milliseconds(50)); - ASSERT_LE(captured.size(), 1U); - if (!captured.empty()) { - EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS); - EXPECT_EQ(captured.front().status_code, grpc::StatusCode::CANCELLED); - EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING); - EXPECT_EQ( - captured.front().detail, - "RPC cancellation was requested by the edge server before completion"); - } -} - -TEST_F(GrpcErrorLoggingInterceptorTest, RecordsClientCancellation) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::TestReqeust request; - api::TestResponse response; - request.set_data("client-cancel"); - - grpc::Status status; - std::thread call([&] { - status = test_stub_->Call(&context, request, &response); - }); - const bool call_started = test_service_.waitForCancelableCall(); - context.TryCancel(); - call.join(); - - ASSERT_TRUE(call_started); - ASSERT_EQ(status.error_code(), grpc::StatusCode::CANCELLED); - const auto captured = waitForRecords(1U); - ASSERT_EQ(captured.size(), 1U); - EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS); - EXPECT_EQ(captured.front().status_code, grpc::StatusCode::CANCELLED); - EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING); - EXPECT_EQ( - captured.front().detail, - "RPC was cancelled by the client or transport before completion"); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, - RecordsStatusWhenNonOkSuppressesUnaryResponse) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::CommandHeader_Request request; - api::CommandHeader_Feedback response; - request.set_device_id("missing"); - - const grpc::Status status = - agv_stub_->emergencyStop(&context, request, &response); - ASSERT_EQ(status.error_code(), grpc::StatusCode::NOT_FOUND); - - const auto captured = waitForRecords(1U); - ASSERT_EQ(captured.size(), 1U); - EXPECT_EQ(captured.front().kind, GrpcFailureKind::GRPC_STATUS); - EXPECT_EQ(captured.front().status_code, grpc::StatusCode::NOT_FOUND); - EXPECT_EQ(captured.front().severity, GrpcFailureSeverity::WARNING); - EXPECT_EQ(captured.front().detail, "missing sentinel"); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, RecordsNestedFeedbackFailure) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::StartCameraCommand_Request request; - api::StartCameraCommand_Feedback response; - - ASSERT_TRUE(camera_stub_->StartCamera(&context, request, &response).ok()); - ASSERT_FALSE(response.header().success()); - - const auto captured = waitForRecords(1U); - ASSERT_EQ(captured.size(), 1U); - EXPECT_EQ(captured.front().kind, GrpcFailureKind::APPLICATION); - EXPECT_EQ(captured.front().method, "/cmvr.api.CameraService/StartCamera"); - EXPECT_EQ(captured.front().detail, "nested sentinel"); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, - RecordsStreamingResponseAndFinalNonOkStatus) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::AgvMapStreamCommand_Request request; - auto reader = agv_stub_->streamMap(&context, request); - api::AgvMapStreamCommand_Feedback response; - - ASSERT_TRUE(reader->Read(&response)); - ASSERT_FALSE(response.header().success()); - const grpc::Status status = reader->Finish(); - ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - - const auto captured = waitForRecords(2U); - ASSERT_EQ(captured.size(), 2U); - EXPECT_EQ(captured[0].kind, GrpcFailureKind::APPLICATION); - EXPECT_EQ(captured[0].method, "/cmvr.api.AgvService/streamMap"); - EXPECT_EQ(captured[0].status_code, grpc::StatusCode::OK); - EXPECT_EQ(captured[0].severity, GrpcFailureSeverity::ERROR); - EXPECT_EQ(captured[0].detail, "stream sentinel"); - EXPECT_EQ(captured[1].kind, GrpcFailureKind::GRPC_STATUS); - EXPECT_EQ(captured[1].status_code, grpc::StatusCode::INTERNAL); - EXPECT_EQ(captured[1].severity, GrpcFailureSeverity::ERROR); - EXPECT_EQ(captured[1].detail, "stream sentinel"); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, - RecordsStreamingFailureBeforeTheStreamFinishes) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::AgvMapStreamCommand_Request request; - request.set_resume_token("hold"); - auto reader = agv_stub_->streamMap(&context, request); - api::AgvMapStreamCommand_Feedback response; - - const bool read = reader->Read(&response); - const auto captured_before_finish = waitForRecords(1U); - agv_service_.releaseHeldStream(); - const grpc::Status status = reader->Finish(); - - ASSERT_TRUE(read); - ASSERT_TRUE(status.ok()); - ASSERT_EQ(captured_before_finish.size(), 1U); - EXPECT_EQ( - captured_before_finish.front().detail, - "immediate stream sentinel"); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, - RecordsEveryStreamingFailureResponse) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::AgvMapStreamCommand_Request request; - request.set_resume_token("multiple"); - auto reader = agv_stub_->streamMap(&context, request); - api::AgvMapStreamCommand_Feedback response; - - ASSERT_TRUE(reader->Read(&response)); - ASSERT_TRUE(reader->Read(&response)); - ASSERT_TRUE(reader->Finish().ok()); - - const auto captured = waitForRecords(2U); - ASSERT_EQ(captured.size(), 2U); - EXPECT_EQ(captured[0].detail, "first stream sentinel"); - EXPECT_EQ(captured[1].detail, "second stream sentinel"); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, - PreservesDifferentResponseAndStatusErrors) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::AgvMapStreamCommand_Request request; - request.set_resume_token("different"); - auto reader = agv_stub_->streamMap(&context, request); - api::AgvMapStreamCommand_Feedback response; - - ASSERT_TRUE(reader->Read(&response)); - const grpc::Status status = reader->Finish(); - ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - - const auto captured = waitForRecords(2U); - ASSERT_EQ(captured.size(), 2U); - EXPECT_EQ(captured[0].kind, GrpcFailureKind::APPLICATION); - EXPECT_EQ(captured[0].detail, "application stream sentinel"); - EXPECT_EQ(captured[1].kind, GrpcFailureKind::GRPC_STATUS); - EXPECT_EQ(captured[1].status_code, grpc::StatusCode::INTERNAL); - EXPECT_EQ(captured[1].detail, "status stream sentinel"); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, - DoesNotDeduplicateLongErrorsByTruncatedPrefix) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::AgvMapStreamCommand_Request request; - request.set_resume_token("long-different"); - auto reader = agv_stub_->streamMap(&context, request); - api::AgvMapStreamCommand_Feedback response; - - ASSERT_TRUE(reader->Read(&response)); - const grpc::Status status = reader->Finish(); - ASSERT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - - const auto captured = waitForRecords(2U); - ASSERT_EQ(captured.size(), 2U); - EXPECT_EQ(captured[0].kind, GrpcFailureKind::APPLICATION); - EXPECT_EQ(captured[1].kind, GrpcFailureKind::GRPC_STATUS); -} - -TEST(GrpcErrorLoggingInterceptorWireTest, RequiresHeaderAsFirstField) -{ - api::CommandHeader_Feedback header; - header.set_success(false); - header.set_error_message("out-of-order sentinel"); - std::string payload; - payload.push_back(static_cast(0x12)); - payload.push_back(static_cast(0x01)); - payload.push_back('x'); - payload.push_back(static_cast(0x0a)); - payload.push_back(static_cast(header.ByteSizeLong())); - payload.append(header.SerializeAsString()); - - const auto result = callRawStartCamera(payload); - - ASSERT_TRUE(result.status.ok()); - ASSERT_EQ(result.response, payload); - ASSERT_EQ(result.records.size(), 1U); - EXPECT_EQ( - result.records.front().kind, - GrpcFailureKind::MALFORMED_RESPONSE); -} - -TEST(GrpcErrorLoggingInterceptorWireTest, DoesNotScanPayloadAfterHeader) -{ - api::CommandHeader_Feedback header; - header.set_success(false); - header.set_error_message("leading header sentinel"); - std::string payload; - payload.push_back(static_cast(0x0a)); - payload.push_back(static_cast(header.ByteSizeLong())); - payload.append(header.SerializeAsString()); - payload.push_back(static_cast(0x12)); - payload.push_back(static_cast(0xff)); - payload.append(2U * 1024U * 1024U, 'x'); - - const auto result = callRawStartCamera(payload); - - ASSERT_TRUE(result.status.ok()); - ASSERT_EQ(result.records.size(), 1U); - EXPECT_EQ(result.records.front().kind, GrpcFailureKind::APPLICATION); - EXPECT_EQ(result.records.front().detail, "leading header sentinel"); -} - -TEST(GrpcErrorLoggingInterceptorWireTest, TreatsExplicitEmptyHeaderAsFailure) -{ - const std::string payload{ - static_cast(0x0a), static_cast(0x00)}; - - const auto result = callRawStartCamera(payload); - - ASSERT_TRUE(result.status.ok()); - ASSERT_EQ(result.records.size(), 1U); - EXPECT_EQ(result.records.front().kind, GrpcFailureKind::APPLICATION); - EXPECT_EQ( - result.records.front().detail, - "operation failed without an error message"); -} - -TEST(GrpcErrorLoggingInterceptorWireTest, RejectsInvalidHeaderWireType) -{ - const std::string payload{ - static_cast(0x08), static_cast(0x01)}; - - const auto result = callRawStartCamera(payload); - - ASSERT_TRUE(result.status.ok()); - ASSERT_EQ(result.records.size(), 1U); - EXPECT_EQ( - result.records.front().kind, - GrpcFailureKind::MALFORMED_RESPONSE); -} - -TEST(GrpcErrorLoggingInterceptorWireTest, RejectsInvalidUtf8ErrorMessage) -{ - const std::string payload{ - static_cast(0x0a), static_cast(0x04), - static_cast(0x12), static_cast(0x02), - static_cast(0xc3), static_cast(0x28)}; - - const auto result = callRawStartCamera(payload); - - ASSERT_TRUE(result.status.ok()); - ASSERT_EQ(result.records.size(), 1U); - EXPECT_EQ( - result.records.front().kind, - GrpcFailureKind::MALFORMED_RESPONSE); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, DoesNotRecordSuccessfulLargeResponse) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::GetRGBImageCommand_Request request; - api::GetRGBImageCommand_Feedback response; - - ASSERT_TRUE(camera_stub_->GetRGBImage(&context, request, &response).ok()); - ASSERT_TRUE(response.header().success()); - ASSERT_EQ(response.color_frame().data().size(), 2U * 1024U * 1024U); - EXPECT_TRUE(records().empty()); -} - -TEST_F(GrpcErrorLoggingInterceptorTest, IgnoresSuccessfulResponseWithoutHeader) -{ - grpc::ClientContext context; - setRpcDeadline(context); - api::TestReqeust request; - api::TestResponse response; - request.set_data("ok"); - - ASSERT_TRUE(test_stub_->Call(&context, request, &response).ok()); - EXPECT_EQ(response.data(), "ok"); - EXPECT_TRUE(records().empty()); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_head_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_head_service_test.cpp deleted file mode 100644 index 0bef7581..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_head_service_test.cpp +++ /dev/null @@ -1,391 +0,0 @@ -#include "service/grpc/server/include/grpc_head_service.h" - -#include -#include -#include -#include -#include - -#include -#include - -#include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "manager/device_manager/include/device_manager.h" -#include "service/grpc/server/include/media_activity_coordinator.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { -namespace { - -using namespace std::chrono_literals; - -class FakeBiohead final : public device::AbstractBiohead { -public: - FakeBiohead() { id_ = "test-head"; } - - std::string typeName() const override { return "FakeBiohead"; } - bool stop() override - { - ++lifecycle_stop_calls; - return true; - } - - bool setExpressionPoseIfCurrent( - const OperationalToken token, - device::FacialExpressionState&, - double, - double) override - { - return recordIfCurrent(token, set_expression_calls); - } - - bool streamFacialPoseIfCurrent( - const OperationalToken token, - device::FacialExpressionState&, - double, - double) override - { - return recordIfCurrent(token, stream_expression_calls); - } - - bool speakStartIfCurrent(const OperationalToken token) override - { - return recordIfCurrent(token, speak_start_calls); - } - - bool expressionHappyIfCurrent(const OperationalToken token) override - { - return recordIfCurrent(token, happy_calls); - } - - bool expressionSurprisedIfCurrent(const OperationalToken token) override - { - return recordIfCurrent(token, surprise_calls); - } - - bool expressionTiredIfCurrent(const OperationalToken token) override - { - return recordIfCurrent(token, tired_calls); - } - - bool expressionAngryIfCurrent(const OperationalToken token) override - { - return recordIfCurrent(token, angry_calls); - } - - bool expressionSadnessIfCurrent(const OperationalToken token) override - { - return recordIfCurrent(token, sadness_calls); - } - - bool expressionYawnIfCurrent(const OperationalToken token) override - { - return recordIfCurrent(token, yawn_calls); - } - - void speakstop() override { ++speak_stop_calls; } - - bool stopOperationalActivity() override - { - invalidateOperationalActivities_(); - ++operational_stop_calls; - ++speak_stop_calls; - return stop_confirmed.load(std::memory_order_acquire); - } - - bool recordIfCurrent( - const OperationalToken token, - std::atomic& calls) - { - return runIfOperationalActivityCurrent_(token, [&] { ++calls; }); - } - - std::atomic set_expression_calls{0}; - std::atomic stream_expression_calls{0}; - std::atomic speak_start_calls{0}; - std::atomic speak_stop_calls{0}; - std::atomic happy_calls{0}; - std::atomic surprise_calls{0}; - std::atomic tired_calls{0}; - std::atomic angry_calls{0}; - std::atomic sadness_calls{0}; - std::atomic yawn_calls{0}; - std::atomic operational_stop_calls{0}; - std::atomic lifecycle_stop_calls{0}; - std::atomic stop_confirmed{true}; -}; - -class GrpcHeadServiceTest : public ::testing::Test { -protected: - void SetUp() override - { - device::DeviceManager::destroyInstance(); - globalStopAllAdmissionGate().clearForTesting(); - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - head_ = std::make_shared(); - manager.registerDevice(head_); - service_ = std::make_unique(); - } - - void TearDown() override - { - if (server_) { - server_->Shutdown(); - server_->Wait(); - } - stub_.reset(); - service_.reset(); - head_.reset(); - device::DeviceManager::destroyInstance(); - globalStopAllAdmissionGate().clearForTesting(); - } - - bool startGrpcServer() - { - int selected_port = 0; - grpc::ServerBuilder builder; - builder.AddListeningPort( - "127.0.0.1:0", - grpc::InsecureServerCredentials(), - &selected_port); - builder.RegisterService(service_.get()); - server_ = builder.BuildAndStart(); - if (!server_ || selected_port <= 0) { - return false; - } - const std::string address = - "127.0.0.1:" + std::to_string(selected_port); - stub_ = api::BioHeadService::NewStub(grpc::CreateChannel( - address, grpc::InsecureChannelCredentials())); - return stub_ != nullptr; - } - - static api::StreamFacialExpression_Request streamRequest() - { - api::StreamFacialExpression_Request request; - request.mutable_header()->set_device_id("test-head"); - request.mutable_expr()->mutable_jaw()->set_x(0.5F); - return request; - } - - std::shared_ptr head_; - std::unique_ptr service_; - std::unique_ptr server_; - std::unique_ptr stub_; -}; - -TEST_F(GrpcHeadServiceTest, - OperationalStopInvalidatesOldTokenWithoutStoppingLifecycle) -{ - const auto old_token = head_->beginOperationalActivity(); - ASSERT_TRUE(head_->recordIfCurrent(old_token, head_->happy_calls)); - - EXPECT_TRUE(head_->stopOperationalActivity()); - EXPECT_FALSE(head_->recordIfCurrent(old_token, head_->happy_calls)); - EXPECT_EQ(head_->happy_calls.load(), 1); - EXPECT_EQ(head_->operational_stop_calls.load(), 1); - EXPECT_EQ(head_->lifecycle_stop_calls.load(), 0); - - const auto current_token = head_->beginOperationalActivity(); - EXPECT_NE(current_token, old_token); - EXPECT_TRUE(head_->recordIfCurrent(current_token, head_->happy_calls)); - EXPECT_EQ(head_->happy_calls.load(), 2); -} - -TEST_F(GrpcHeadServiceTest, - StopAllGateRejectsMotionButAllowsOperationalStopRpcs) -{ - const auto ticket = globalStopAllAdmissionGate().beginStopAll(); - ASSERT_TRUE(ticket.valid()); - - api::Happy_Request happy_request; - happy_request.mutable_header()->set_device_id("test-head"); - api::Happy_Feedback happy_response; - grpc::ServerContext happy_context; - EXPECT_TRUE(service_->Happy( - &happy_context, &happy_request, &happy_response).ok()); - EXPECT_FALSE(happy_response.header().success()); - EXPECT_EQ(head_->happy_calls.load(), 0); - - api::Surprise_Request surprise_request; - surprise_request.mutable_header()->set_device_id("test-head"); - api::Surprise_Feedback surprise_response; - grpc::ServerContext surprise_context; - EXPECT_TRUE(service_->Surprise( - &surprise_context, &surprise_request, &surprise_response).ok()); - EXPECT_FALSE(surprise_response.header().success()); - EXPECT_EQ(head_->surprise_calls.load(), 0); - - api::ExpressionTired_Request tired_request; - tired_request.mutable_header()->set_device_id("test-head"); - api::ExpressionTired_Feedback tired_response; - grpc::ServerContext tired_context; - EXPECT_TRUE(service_->ExpressionTired( - &tired_context, &tired_request, &tired_response).ok()); - EXPECT_FALSE(tired_response.header().success()); - EXPECT_EQ(head_->tired_calls.load(), 0); - - api::ExpressionAngry_Request angry_request; - angry_request.mutable_header()->set_device_id("test-head"); - api::ExpressionAngry_Feedback angry_response; - grpc::ServerContext angry_context; - EXPECT_TRUE(service_->ExpressionAngry( - &angry_context, &angry_request, &angry_response).ok()); - EXPECT_FALSE(angry_response.header().success()); - EXPECT_EQ(head_->angry_calls.load(), 0); - - api::ExpressionSadness_Request sadness_request; - sadness_request.mutable_header()->set_device_id("test-head"); - api::ExpressionSadness_Feedback sadness_response; - grpc::ServerContext sadness_context; - EXPECT_TRUE(service_->ExpressionSadness( - &sadness_context, &sadness_request, &sadness_response).ok()); - EXPECT_FALSE(sadness_response.header().success()); - EXPECT_EQ(head_->sadness_calls.load(), 0); - - api::ExpressionYawn_Request yawn_request; - yawn_request.mutable_header()->set_device_id("test-head"); - api::ExpressionYawn_Feedback yawn_response; - grpc::ServerContext yawn_context; - EXPECT_TRUE(service_->ExpressionYawn( - &yawn_context, &yawn_request, &yawn_response).ok()); - EXPECT_FALSE(yawn_response.header().success()); - EXPECT_EQ(head_->yawn_calls.load(), 0); - - api::SetFacialExpression_Request expression_request; - expression_request.mutable_header()->set_device_id("test-head"); - api::SetFacialExpression_Feedback expression_response; - grpc::ServerContext expression_context; - EXPECT_TRUE(service_->SetExpression( - &expression_context, - &expression_request, - &expression_response).ok()); - EXPECT_FALSE(expression_response.header().success()); - EXPECT_EQ(head_->set_expression_calls.load(), 0); - - api::SpeakStart_Request speak_request; - speak_request.mutable_header()->set_device_id("test-head"); - api::SpeakStart_Feedback speak_response; - grpc::ServerContext speak_context; - EXPECT_TRUE(service_->SpeakStart( - &speak_context, &speak_request, &speak_response).ok()); - EXPECT_FALSE(speak_response.header().success()); - EXPECT_EQ(head_->speak_start_calls.load(), 0); - - api::SpeakStop_Request speak_stop_request; - speak_stop_request.mutable_header()->set_device_id("test-head"); - api::SpeakStop_Feedback speak_stop_response; - grpc::ServerContext speak_stop_context; - EXPECT_TRUE(service_->SpeakStop( - &speak_stop_context, - &speak_stop_request, - &speak_stop_response).ok()); - EXPECT_TRUE(speak_stop_response.header().success()); - - api::EmergencyStop_Request stop_request; - stop_request.mutable_header()->set_device_id("test-head"); - api::EmergencyStop_Feedback stop_response; - grpc::ServerContext stop_context; - EXPECT_TRUE(service_->EmergencyStop( - &stop_context, &stop_request, &stop_response).ok()); - EXPECT_TRUE(stop_response.header().success()); - EXPECT_EQ(head_->operational_stop_calls.load(), 1); - EXPECT_EQ(head_->lifecycle_stop_calls.load(), 0); - - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll(ticket, true)); -} - -TEST_F(GrpcHeadServiceTest, - StreamIsCancelledByStopAllAndNewStreamWorksAfterRecovery) -{ - ASSERT_TRUE(startGrpcServer()); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 2s); - auto stream = stub_->StreamExpression(&context); - - ASSERT_TRUE(stream->Write(streamRequest())); - api::StreamFacialExpression_Feedback feedback; - ASSERT_TRUE(stream->Read(&feedback)); - ASSERT_TRUE(feedback.header().success()); - ASSERT_EQ(head_->stream_expression_calls.load(), 1); - - const auto admission_ticket = globalStopAllAdmissionGate().beginStopAll(); - const auto media_ticket = globalMediaActivityCoordinator().beginStopAll(); - ASSERT_TRUE(admission_ticket.valid()); - ASSERT_TRUE(media_ticket.valid()); - - (void)stream->Write(streamRequest()); - stream->WritesDone(); - while (stream->Read(&feedback)) { - } - const auto status = stream->Finish(); - EXPECT_TRUE( - status.ok() || status.error_code() == grpc::StatusCode::CANCELLED); - EXPECT_TRUE(globalMediaActivityCoordinator().waitForStopped( - media_ticket, 1s)); - EXPECT_EQ(head_->stream_expression_calls.load(), 1); - EXPECT_TRUE(globalMediaActivityCoordinator().finishStopAll( - media_ticket, true)); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll( - admission_ticket, true)); - - grpc::ClientContext resumed_context; - resumed_context.set_deadline(std::chrono::system_clock::now() + 2s); - auto resumed = stub_->StreamExpression(&resumed_context); - ASSERT_TRUE(resumed->Write(streamRequest())); - ASSERT_TRUE(resumed->Read(&feedback)); - EXPECT_TRUE(feedback.header().success()); - EXPECT_EQ(head_->stream_expression_calls.load(), 2); - resumed_context.TryCancel(); - resumed->WritesDone(); - while (resumed->Read(&feedback)) { - } - (void)resumed->Finish(); -} - -TEST_F(GrpcHeadServiceTest, - StopAllCancelsStreamBeforeItsFirstFrameCanClearStopState) -{ - ASSERT_TRUE(startGrpcServer()); - grpc::ClientContext context; - context.set_deadline(std::chrono::system_clock::now() + 2s); - auto stream = stub_->StreamExpression(&context); - - std::this_thread::sleep_for(20ms); - const auto admission_ticket = globalStopAllAdmissionGate().beginStopAll(); - const auto media_ticket = globalMediaActivityCoordinator().beginStopAll(); - ASSERT_TRUE(admission_ticket.valid()); - ASSERT_TRUE(media_ticket.valid()); - - (void)stream->Write(streamRequest()); - stream->WritesDone(); - api::StreamFacialExpression_Feedback feedback; - while (stream->Read(&feedback)) { - } - (void)stream->Finish(); - EXPECT_TRUE(globalMediaActivityCoordinator().waitForStopped( - media_ticket, 1s)); - EXPECT_EQ(head_->stream_expression_calls.load(), 0); - EXPECT_TRUE(globalMediaActivityCoordinator().finishStopAll( - media_ticket, true)); - EXPECT_TRUE(globalStopAllAdmissionGate().finishStopAll( - admission_ticket, true)); -} - -TEST_F(GrpcHeadServiceTest, UnconfirmedOperationalStopIsReported) -{ - head_->stop_confirmed = false; - api::EmergencyStop_Request request; - request.mutable_header()->set_device_id("test-head"); - api::EmergencyStop_Feedback response; - grpc::ServerContext context; - - EXPECT_TRUE(service_->EmergencyStop(&context, &request, &response).ok()); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(head_->operational_stop_calls.load(), 1); - EXPECT_EQ(head_->lifecycle_stop_calls.load(), 0); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_motor_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_motor_service_test.cpp deleted file mode 100644 index ea369cb8..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_motor_service_test.cpp +++ /dev/null @@ -1,1856 +0,0 @@ -#include "service/grpc/server/include/grpc_motor_service.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "cmvr/config/motor_config/motor_config.pb.h" -#include "devices/motor/manager/include/motor_manager.h" -#include "devices/motor/motor_protocol_interface.h" -#include "manager/device_manager/include/device_manager.h" -#include "service/grpc/server/include/motor_activity_coordinator.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -namespace cmvr::service { - -class gRPCMotorServiceImplTestAccess { -public: - static void bestEffortQuickStop(gRPCMotorServiceImpl& service, - const api::MotorTarget& target, - const std::string& error) - { - service.bestEffortQuickStop(target, error); - } - - static std::unique_lock holdExceptionCleanupMutex( - gRPCMotorServiceImpl& service, - const api::MotorTarget& target) - { - gRPCMotorServiceImpl::ResolvedMotor resolved; - const auto status = service.resolveMotor(target, resolved); - if (!status.ok()) { - throw std::runtime_error(status.error_message()); - } - return std::unique_lock( - resolved.control->exception_cleanup_mutex); - } -}; - -namespace { - -class FakeMotorProtocol final : public device::MotorProtocolInterface { -public: - bool initNode(std::uint8_t) override { return true; } - void setMode(std::uint8_t, msgs::RunMode mode) override - { - if (throw_set_mode_.load()) { - throw std::runtime_error("injected setMode exception"); - } - mode_ = mode; - } - msgs::RunMode getMode(std::uint8_t) override { return mode_.load(); } - void setLimitQdd(std::uint8_t, double, double) override {} - void setLimitQd(std::uint8_t, double) override {} - void setLimitQ(std::uint8_t, double, double) override {} - bool calibrateZeroQ(std::uint8_t) override - { - calibrate_started_ = true; - if (calibrate_delay_ms_.load() > 0) { - std::this_thread::sleep_for( - std::chrono::milliseconds(calibrate_delay_ms_.load())); - } - position_ = 0.0; - calibrate_finished_ = true; - return calibrate_success_.load(); - } - bool reachedTargetQ(std::uint8_t) override { return reached_.load(); } - bool commandProfilePosition(std::uint8_t, double target_q, - double, double) override - { - command_started_ = true; - if (throw_profile_position_.load()) { - throw std::runtime_error("injected profile position exception"); - } - profile_command_order_ = ++operation_counter_; - if (profile_delay_ms_.load() > 0) { - std::this_thread::sleep_for( - std::chrono::milliseconds(profile_delay_ms_.load())); - } - if (!hold_position_.load()) { - position_ = target_q; - velocity_ = 0.0; - reached_ = true; - } - return profile_position_success_.load(); - } - bool commandProfileVelocity(std::uint8_t, double target_qd, - double) override - { - profile_velocity_started_ = true; - if (profile_velocity_delay_ms_.load() > 0) { - std::this_thread::sleep_for( - std::chrono::milliseconds(profile_velocity_delay_ms_.load())); - } - velocity_ = target_qd; - return profile_velocity_success_.load(); - } - bool commandCyclicPosition(std::uint8_t, double target_q, - double target_qd) override - { - cyclic_position_started_ = true; - if (cyclic_position_delay_ms_.load() > 0) { - std::this_thread::sleep_for( - std::chrono::milliseconds(cyclic_position_delay_ms_.load())); - } - if (throw_cyclic_position_.load()) { - throw std::runtime_error("injected cyclic position exception"); - } - position_ = target_q; - velocity_ = target_qd; - return true; - } - bool commandCyclicVelocity(std::uint8_t, double target_qd) override - { - velocity_ = target_qd; - return true; - } - bool commandCyclicTorque(std::uint8_t, double) override { return false; } - void setMotorConversion(std::uint8_t, double, double) override {} - bool torqueOn(std::uint8_t) override - { - torque_on_started_ = true; - torque_on_order_ = ++operation_counter_; - if (torque_on_delay_ms_.load() > 0) { - std::this_thread::sleep_for( - std::chrono::milliseconds(torque_on_delay_ms_.load())); - } - return torque_on_success_.load(); - } - bool torqueOff(std::uint8_t) override - { - ++torque_off_count_; - return torque_off_success_.load(); - } - bool brakeRelease(std::uint8_t) override { return true; } - bool quickStop(std::uint8_t) override - { - last_quick_stop_order_ = ++operation_counter_; - ++quick_stop_count_; - if (quick_stop_delay_ms_.load() > 0) { - std::this_thread::sleep_for( - std::chrono::milliseconds(quick_stop_delay_ms_.load())); - } - if (quick_stop_success_.load()) { - velocity_ = 0.0; - return true; - } - return false; - } - double getQ(std::uint8_t) override - { - get_q_started_ = true; - if (get_q_delay_ms_.load() > 0) { - std::this_thread::sleep_for( - std::chrono::milliseconds(get_q_delay_ms_.load())); - } - if (throw_get_q_.load()) { - throw std::runtime_error("injected getQ exception"); - } - if (nonfinite_get_q_.load()) { - return std::numeric_limits::quiet_NaN(); - } - return position_.load(); - } - double getQd(std::uint8_t) override - { - get_qd_started_ = true; - if (nonfinite_get_qd_.load()) { - return std::numeric_limits::quiet_NaN(); - } - return velocity_.load(); - } - - std::atomic hold_position_{false}; - std::atomic calibrate_started_{false}; - std::atomic calibrate_finished_{false}; - std::atomic calibrate_delay_ms_{0}; - std::atomic calibrate_success_{true}; - std::atomic command_started_{false}; - std::atomic quick_stop_count_{0}; - std::atomic quick_stop_success_{true}; - std::atomic quick_stop_delay_ms_{0}; - std::atomic throw_set_mode_{false}; - std::atomic throw_profile_position_{false}; - std::atomic throw_cyclic_position_{false}; - std::atomic throw_get_q_{false}; - std::atomic nonfinite_get_q_{false}; - std::atomic nonfinite_get_qd_{false}; - std::atomic get_qd_started_{false}; - std::atomic profile_position_success_{true}; - std::atomic profile_velocity_success_{true}; - std::atomic torque_on_success_{true}; - std::atomic torque_off_success_{true}; - std::atomic torque_off_count_{0}; - std::atomic profile_delay_ms_{0}; - std::atomic profile_velocity_delay_ms_{0}; - std::atomic profile_velocity_started_{false}; - std::atomic cyclic_position_delay_ms_{0}; - std::atomic cyclic_position_started_{false}; - std::atomic get_q_delay_ms_{0}; - std::atomic get_q_started_{false}; - std::atomic torque_on_delay_ms_{0}; - std::atomic torque_on_started_{false}; - std::atomic operation_counter_{0}; - std::atomic profile_command_order_{0}; - std::atomic torque_on_order_{0}; - std::atomic last_quick_stop_order_{0}; - -private: - std::atomic mode_{msgs::RUN_MODE_UNSPECIFIED}; - std::atomic position_{0.0}; - std::atomic velocity_{0.0}; - std::atomic reached_{false}; -}; - -class FakeMotor final : public device::AbstractMotor { -public: - explicit FakeMotor( - const std::uint8_t node_id, - std::string joint_name = "test_joint") - : AbstractMotor(node_id) - { - info_.id = node_id; - info_.joint_name = std::move(joint_name); - } - - std::string typeName() const override { return "FakeMotor"; } -}; - -class MotorServiceTest : public ::testing::Test { -protected: - void SetUp() override - { - globalStopAllAdmissionGate().clearForTesting(); - globalMotorActivityCoordinator().clearForTesting(); - config::DeviceManagerConfig device_config; - auto& device_manager = device::DeviceManager::getInstance(device_config); - - config::MotorConfig motor_config; - manager_ = std::make_shared( - "test-motor-manager", motor_config); - protocol_ = std::make_shared(); - motor_ = std::make_shared(1); - motor_->setProtocol(protocol_); - ASSERT_TRUE(manager_->addMotor(motor_)); - device_manager.registerDevice("test-motor-manager", manager_); - service_ = std::make_unique(); - } - - void TearDown() override - { - if (grpc_server_) { - grpc_server_->Shutdown(); - grpc_server_->Wait(); - grpc_server_.reset(); - } - stub_.reset(); - if (!grpc_socket_path_.empty()) { - std::remove(grpc_socket_path_.c_str()); - grpc_socket_path_.clear(); - } - service_.reset(); - manager_.reset(); - motor_.reset(); - protocol_.reset(); - device::DeviceManager::destroyInstance(); - globalMotorActivityCoordinator().clearForTesting(); - globalStopAllAdmissionGate().clearForTesting(); - } - - static api::MotorTarget makeTarget() - { - api::MotorTarget target; - target.mutable_header()->set_device_id("test-motor-manager"); - target.set_motor_id(1); - return target; - } - - bool startGrpcServer() - { - grpc_socket_path_ = - "/tmp/cmvr_motor_service_test_" + - std::to_string(static_cast(::getpid())) + ".sock"; - std::remove(grpc_socket_path_.c_str()); - const std::string server_address = "unix:" + grpc_socket_path_; - - grpc::ServerBuilder builder; - builder.AddListeningPort( - server_address, grpc::InsecureServerCredentials()); - builder.RegisterService(service_.get()); - grpc_server_ = builder.BuildAndStart(); - if (!grpc_server_) { - return false; - } - stub_ = api::MotorService::NewStub( - grpc::CreateChannel( - server_address, - grpc::InsecureChannelCredentials())); - return stub_ != nullptr; - } - - std::shared_ptr protocol_; - std::shared_ptr motor_; - std::shared_ptr manager_; - std::unique_ptr service_; - std::unique_ptr grpc_server_; - std::unique_ptr stub_; - std::string grpc_socket_path_; -}; - -TEST_F(MotorServiceTest, SetZeroBackendFailureReportsUnknownOutcome) -{ - protocol_->calibrate_success_ = false; - - api::SetMotorZeroRequest request; - *request.mutable_target() = makeTarget(); - grpc::ServerContext context; - api::MotorCommandResponse response; - const auto status = service_->setZero(&context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_FALSE(response.header().success()); - EXPECT_NE(response.header().error_message().find("outcome unknown"), - std::string::npos); - EXPECT_NE(response.header().error_message().find("device state"), - std::string::npos); - EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); -} - -TEST_F(MotorServiceTest, RejectedSetZeroPreemptedInFlightReturnsAborted) -{ - protocol_->calibrate_success_ = false; - protocol_->calibrate_delay_ms_ = 100; - - api::SetMotorZeroRequest request; - *request.mutable_target() = makeTarget(); - grpc::ServerContext zero_context; - api::MotorCommandResponse zero_response; - grpc::Status zero_status; - std::thread zeroing([&]() { - zero_status = service_->setZero( - &zero_context, &request, &zero_response); - }); - const auto dispatch_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->calibrate_started_.load() && - std::chrono::steady_clock::now() < dispatch_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(protocol_->calibrate_started_.load()); - - api::EmergencyStopRequest stop_request; - *stop_request.mutable_target() = makeTarget(); - grpc::ServerContext stop_context; - api::MotorCommandResponse stop_response; - const auto stop_status = service_->emergencyStop( - &stop_context, &stop_request, &stop_response); - - zeroing.join(); - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_EQ(zero_status.error_code(), grpc::StatusCode::ABORTED); -} - -TEST_F(MotorServiceTest, SetZeroDeadlineDuringDispatchReportsDeadline) -{ - ASSERT_TRUE(startGrpcServer()); - protocol_->calibrate_success_ = false; - protocol_->calibrate_delay_ms_ = 100; - - api::SetMotorZeroRequest request; - *request.mutable_target() = makeTarget(); - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::milliseconds(50)); - api::MotorCommandResponse response; - const auto status = stub_->setZero(&context, request, &response); - EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); - - const auto completion_deadline = - std::chrono::steady_clock::now() + std::chrono::milliseconds(300); - while (!protocol_->calibrate_finished_.load() && - std::chrono::steady_clock::now() < completion_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } - EXPECT_TRUE(protocol_->calibrate_finished_.load()); - EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); -} - -TEST_F(MotorServiceTest, ProfilePositionReturnsOnlyAfterTargetIsReached) -{ - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(1.25); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(2.0); - request.mutable_wait()->set_settle_sample_count(1); - request.mutable_wait()->set_poll_period_ms(1); - - grpc::ServerContext context; - api::MotorCommandResponse response; - const auto status = service_->profilePosition(&context, &request, &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()); - EXPECT_DOUBLE_EQ(response.status().position_rad(), 1.25); - EXPECT_FALSE(response.status().service_busy()); - EXPECT_EQ(response.status().active_control(), api::MOTOR_CONTROL_NONE); -} - -TEST_F(MotorServiceTest, DirectControlRejectsMotorClaimedByRobotArm) -{ - std::uint64_t claim_id = 0; - std::string claim_error; - ASSERT_TRUE(manager_->claimArmJoints( - "test_arm", {"test_joint"}, claim_id, &claim_error)) - << claim_error; - - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(1.25); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(2.0); - grpc::ServerContext context; - api::MotorCommandResponse response; - - const auto status = service_->profilePosition( - &context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_FALSE(response.header().success()); - EXPECT_NE(status.error_message().find("test_arm"), std::string::npos); - EXPECT_NE(status.error_message().find("ArmService"), std::string::npos); - EXPECT_FALSE(protocol_->command_started_.load()); - EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); -} - -TEST_F(MotorServiceTest, ClaimedArmMotorRemainsObservableWithoutStopRegistration) -{ - std::uint64_t claim_id = 0; - ASSERT_TRUE(manager_->claimArmJoints( - "test_arm", {"test_joint"}, claim_id)); - - api::GetMotorStatusRequest request; - *request.mutable_target() = makeTarget(); - grpc::ServerContext context; - api::GetMotorStatusResponse response; - const auto status = service_->getStatus(&context, &request, &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()); - EXPECT_EQ(response.status().joint_name(), "test_joint"); - - auto& coordinator = globalMotorActivityCoordinator(); - const auto ticket = coordinator.beginStopAll(); - ASSERT_TRUE(ticket.valid()); - std::string stop_error; - EXPECT_TRUE(coordinator.requestStop(ticket, &stop_error)) << stop_error; - EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); - EXPECT_TRUE(coordinator.waitForStopped( - ticket, std::chrono::milliseconds(50), &stop_error)) << stop_error; - EXPECT_TRUE(coordinator.finishStopAll(ticket, true)); -} - -TEST_F(MotorServiceTest, ReleasingArmClaimRestoresDirectMotorControl) -{ - std::uint64_t claim_id = 0; - ASSERT_TRUE(manager_->claimArmJoints( - "test_arm", {"test_joint"}, claim_id)); - manager_->releaseArmJoints(claim_id); - - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(0.75); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(2.0); - request.mutable_wait()->set_settle_sample_count(1); - request.mutable_wait()->set_poll_period_ms(1); - grpc::ServerContext context; - api::MotorCommandResponse response; - - const auto status = service_->profilePosition( - &context, &request, &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()); - EXPECT_DOUBLE_EQ(response.status().position_rad(), 0.75); -} - -TEST_F(MotorServiceTest, ArmClaimCannotBeDisplacedOrReleasedByAnotherToken) -{ - std::uint64_t first_claim = 0; - ASSERT_TRUE(manager_->claimArmJoints( - "first_arm", {"test_joint"}, first_claim)); - - std::uint64_t conflicting_claim = 0; - std::string claim_error; - EXPECT_FALSE(manager_->claimArmJoints( - "second_arm", {"test_joint"}, conflicting_claim, &claim_error)); - EXPECT_EQ(conflicting_claim, 0U); - EXPECT_NE(claim_error.find("first_arm"), std::string::npos); - - manager_->releaseArmJoints(first_claim + 1U); - EXPECT_EQ(manager_->armOwnerForJoint("test_joint"), "first_arm"); - - manager_->releaseArmJoints(first_claim); - EXPECT_TRUE(manager_->armOwnerForJoint("test_joint").empty()); -} - -TEST_F(MotorServiceTest, ArmClaimDoesNotRejectUnclaimedMotor) -{ - auto other_protocol = std::make_shared(); - auto other_motor = std::make_shared(2, "other_joint"); - other_motor->setProtocol(other_protocol); - ASSERT_TRUE(manager_->addMotor(other_motor)); - - std::uint64_t claim_id = 0; - ASSERT_TRUE(manager_->claimArmJoints( - "test_arm", {"test_joint"}, claim_id)); - - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.mutable_target()->set_motor_id(2); - request.set_target_position_rad(0.5); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(2.0); - request.mutable_wait()->set_settle_sample_count(1); - request.mutable_wait()->set_poll_period_ms(1); - grpc::ServerContext context; - api::MotorCommandResponse response; - - const auto status = service_->profilePosition( - &context, &request, &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()); - EXPECT_DOUBLE_EQ(response.status().position_rad(), 0.5); -} - -TEST_F(MotorServiceTest, ProfileVelocityReturnsAfterTargetSettles) -{ - api::ProfileVelocityRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_velocity_rad_s(1.5); - request.set_acceleration_rad_s2(2.0); - request.mutable_wait()->set_settle_sample_count(2); - request.mutable_wait()->set_poll_period_ms(1); - request.mutable_wait()->set_velocity_tolerance_rad_s(1e-4); - - grpc::ServerContext context; - api::MotorCommandResponse response; - const auto status = service_->profileVelocity(&context, &request, &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()); - EXPECT_DOUBLE_EQ(response.status().velocity_rad_s(), 1.5); - EXPECT_FALSE(response.status().service_busy()); - EXPECT_EQ(response.status().active_control(), api::MOTOR_CONTROL_NONE); -} - -TEST_F(MotorServiceTest, ProfilePositionStopsImmediatelyOnNonFiniteFeedback) -{ - protocol_->hold_position_ = true; - - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(1.0); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(1.0); - request.mutable_wait()->set_timeout_ms(5000); - request.mutable_wait()->set_poll_period_ms(2); - - grpc::ServerContext context; - api::MotorCommandResponse response; - grpc::Status status; - const auto started = std::chrono::steady_clock::now(); - std::thread motion([&]() { - status = service_->profilePosition(&context, &request, &response); - }); - - const auto sample_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->get_q_started_.load() && - std::chrono::steady_clock::now() < sample_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - if (!protocol_->get_q_started_.load()) { - context.TryCancel(); - motion.join(); - FAIL() << "profile position feedback sampling did not start"; - return; - } - - protocol_->nonfinite_get_q_ = true; - motion.join(); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::UNAVAILABLE); - EXPECT_FALSE(response.header().success()); - EXPECT_FALSE(response.status().emergency_stopped()); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); - EXPECT_LT(std::chrono::steady_clock::now() - started, - std::chrono::seconds(1)); -} - -TEST_F(MotorServiceTest, - ProfileVelocityFailedStopOnNonFiniteFeedbackLatchesEmergency) -{ - protocol_->quick_stop_success_ = false; - - api::ProfileVelocityRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_velocity_rad_s(1.5); - request.set_acceleration_rad_s2(2.0); - request.mutable_wait()->set_timeout_ms(5000); - request.mutable_wait()->set_poll_period_ms(2); - request.mutable_wait()->set_settle_sample_count(1000); - - grpc::ServerContext context; - api::MotorCommandResponse response; - grpc::Status status; - const auto started = std::chrono::steady_clock::now(); - std::thread motion([&]() { - status = service_->profileVelocity(&context, &request, &response); - }); - - const auto sample_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->get_qd_started_.load() && - std::chrono::steady_clock::now() < sample_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - if (!protocol_->get_qd_started_.load()) { - context.TryCancel(); - motion.join(); - FAIL() << "profile velocity feedback sampling did not start"; - return; - } - - protocol_->nonfinite_get_qd_ = true; - motion.join(); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_FALSE(response.header().success()); - EXPECT_TRUE(response.status().emergency_stopped()); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); - EXPECT_LT(std::chrono::steady_clock::now() - started, - std::chrono::seconds(1)); - - api::ProfilePositionRequest blocked_request; - *blocked_request.mutable_target() = makeTarget(); - blocked_request.set_target_position_rad(1.0); - blocked_request.set_max_velocity_rad_s(1.0); - blocked_request.set_acceleration_rad_s2(1.0); - grpc::ServerContext blocked_context; - api::MotorCommandResponse blocked_response; - const auto blocked_status = service_->profilePosition( - &blocked_context, &blocked_request, &blocked_response); - EXPECT_EQ(blocked_status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_TRUE(blocked_response.status().emergency_stopped()); -} - -TEST_F(MotorServiceTest, - ProfileBackendExceptionReportsUnknownOutcomeAndQuickStops) -{ - protocol_->throw_profile_position_ = true; - - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(1.0); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(1.0); - - grpc::ServerContext context; - api::MotorCommandResponse response; - const auto status = service_->profilePosition(&context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::ABORTED); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ( - response.header().reason_code(), - api::COMMAND_REASON_CODE_OUTCOME_UNKNOWN); - EXPECT_NE(response.header().error_message().find( - "injected profile position exception"), - std::string::npos); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); - const auto safety = device::DeviceManager::getInstance() - .safetyManager() - .snapshot(); - ASSERT_EQ(safety.devices.size(), 1U); - EXPECT_EQ( - safety.devices.front().admission_state, - safety::DeviceAdmissionState::Quarantined); -} - -TEST_F(MotorServiceTest, ExceptionCleanupKeepsMotorReservedUntilQuickStopFinishes) -{ - protocol_->throw_profile_position_ = true; - protocol_->quick_stop_delay_ms_ = 100; - - api::ProfilePositionRequest failing_request; - *failing_request.mutable_target() = makeTarget(); - failing_request.set_target_position_rad(1.0); - failing_request.set_max_velocity_rad_s(1.0); - failing_request.set_acceleration_rad_s2(1.0); - grpc::ServerContext failing_context; - api::MotorCommandResponse failing_response; - grpc::Status failing_status; - std::thread failing([&]() { - failing_status = service_->profilePosition( - &failing_context, &failing_request, &failing_response); - }); - - const auto cleanup_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (protocol_->quick_stop_count_.load() == 0 && - std::chrono::steady_clock::now() < cleanup_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - if (protocol_->quick_stop_count_.load() == 0) { - failing.join(); - FAIL() << "exception cleanup did not start"; - return; - } - protocol_->throw_profile_position_ = false; - - api::ProfilePositionRequest competing_request; - *competing_request.mutable_target() = makeTarget(); - competing_request.set_target_position_rad(2.0); - competing_request.set_max_velocity_rad_s(1.0); - competing_request.set_acceleration_rad_s2(1.0); - grpc::ServerContext competing_context; - api::MotorCommandResponse competing_response; - const auto competing_status = service_->profilePosition( - &competing_context, &competing_request, &competing_response); - - failing.join(); - EXPECT_EQ(failing_status.error_code(), grpc::StatusCode::ABORTED); - EXPECT_EQ( - failing_response.header().reason_code(), - api::COMMAND_REASON_CODE_OUTCOME_UNKNOWN); - EXPECT_EQ(competing_status.error_code(), - grpc::StatusCode::RESOURCE_EXHAUSTED); -} - -TEST_F(MotorServiceTest, WaitingStaleCleanupCannotStopOrReleaseNewOwner) -{ - const auto target = makeTarget(); - gRPCMotorServiceImplTestAccess::bestEffortQuickStop( - *service_, target, "first completed exception cleanup"); - ASSERT_EQ(protocol_->quick_stop_count_.load(), 1); - - auto cleanup_gate = - gRPCMotorServiceImplTestAccess::holdExceptionCleanupMutex( - *service_, target); - std::atomic stale_cleanup_entered{false}; - std::thread stale_cleanup([&]() { - stale_cleanup_entered = true; - gRPCMotorServiceImplTestAccess::bestEffortQuickStop( - *service_, target, "second stale exception cleanup"); - }); - const auto stale_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!stale_cleanup_entered.load() && - std::chrono::steady_clock::now() < stale_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - if (!stale_cleanup_entered.load()) { - cleanup_gate.unlock(); - stale_cleanup.join(); - FAIL() << "second cleanup did not start waiting"; - return; - } - - protocol_->profile_delay_ms_ = 100; - protocol_->command_started_ = false; - api::ProfilePositionRequest request; - *request.mutable_target() = target; - request.set_target_position_rad(1.0); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(1.0); - request.mutable_wait()->set_settle_sample_count(1); - grpc::ServerContext motion_context; - api::MotorCommandResponse motion_response; - grpc::Status motion_status; - std::thread motion([&]() { - motion_status = service_->profilePosition( - &motion_context, &request, &motion_response); - }); - const auto motion_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->command_started_.load() && - std::chrono::steady_clock::now() < motion_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - if (!protocol_->command_started_.load()) { - cleanup_gate.unlock(); - stale_cleanup.join(); - motion_context.TryCancel(); - motion.join(); - FAIL() << "new owner did not dispatch while stale cleanup waited"; - return; - } - - cleanup_gate.unlock(); - stale_cleanup.join(); - EXPECT_EQ(protocol_->quick_stop_count_.load(), 1); - - motion.join(); - ASSERT_TRUE(motion_status.ok()) << motion_status.error_message(); - EXPECT_TRUE(motion_response.header().success()); - EXPECT_EQ(protocol_->quick_stop_count_.load(), 1); -} - -TEST_F(MotorServiceTest, GetStatusBackendExceptionReturnsInternal) -{ - protocol_->throw_get_q_ = true; - - api::GetMotorStatusRequest request; - *request.mutable_target() = makeTarget(); - - grpc::ServerContext context; - api::GetMotorStatusResponse response; - const auto status = service_->getStatus(&context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_FALSE(response.header().success()); - EXPECT_NE(response.header().error_message().find( - "injected getQ exception"), - std::string::npos); - EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); -} - -TEST_F(MotorServiceTest, GetStatusRejectsNonFiniteFeedback) -{ - protocol_->nonfinite_get_q_ = true; - - api::GetMotorStatusRequest request; - *request.mutable_target() = makeTarget(); - - grpc::ServerContext context; - api::GetMotorStatusResponse response; - const auto status = service_->getStatus(&context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::UNAVAILABLE); - EXPECT_FALSE(response.header().success()); -} - -TEST_F(MotorServiceTest, ProfileRejectsInfiniteWaitTolerance) -{ - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(1.0); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(1.0); - request.mutable_wait()->set_position_tolerance_rad( - std::numeric_limits::infinity()); - - grpc::ServerContext context; - api::MotorCommandResponse response; - const auto status = service_->profilePosition(&context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::INVALID_ARGUMENT); - EXPECT_FALSE(response.header().success()); - EXPECT_FALSE(protocol_->command_started_.load()); -} - -TEST_F(MotorServiceTest, ProfileRejectAckLossQuickStopsBeforeReturning) -{ - protocol_->profile_position_success_ = false; - - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(1.0); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(1.0); - - grpc::ServerContext context; - api::MotorCommandResponse response; - const auto status = service_->profilePosition(&context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_FALSE(response.header().success()); - EXPECT_FALSE(response.status().emergency_stopped()); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); -} - -TEST_F(MotorServiceTest, ProfileRejectAckLossWithFailedStopReturnsInternal) -{ - protocol_->profile_position_success_ = false; - protocol_->quick_stop_success_ = false; - - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(1.0); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(1.0); - - grpc::ServerContext context; - api::MotorCommandResponse response; - const auto status = service_->profilePosition(&context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_FALSE(response.header().success()); - EXPECT_TRUE(response.status().emergency_stopped()); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); - - grpc::ServerContext blocked_context; - api::MotorCommandResponse blocked_response; - const auto blocked_status = service_->profilePosition( - &blocked_context, &request, &blocked_response); - EXPECT_EQ(blocked_status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_TRUE(blocked_response.status().emergency_stopped()); - - protocol_->quick_stop_success_ = true; - protocol_->profile_position_success_ = true; - api::SetMotorEnabledRequest enable_request; - *enable_request.mutable_target() = makeTarget(); - enable_request.set_enabled(true); - grpc::ServerContext enable_context; - api::MotorCommandResponse enable_response; - const auto enable_status = service_->setEnabled( - &enable_context, &enable_request, &enable_response); - ASSERT_TRUE(enable_status.ok()) << enable_status.error_message(); - EXPECT_FALSE(enable_response.status().emergency_stopped()); - - grpc::ServerContext recovered_context; - api::MotorCommandResponse recovered_response; - const auto recovered_status = service_->profilePosition( - &recovered_context, &request, &recovered_response); - EXPECT_TRUE(recovered_status.ok()) << recovered_status.error_message(); -} - -TEST_F(MotorServiceTest, ProfileAckLossAfterDeadlineStillQuickStops) -{ - ASSERT_TRUE(startGrpcServer()); - protocol_->profile_position_success_ = false; - protocol_->profile_delay_ms_ = 100; - - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(1.0); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(1.0); - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::milliseconds(50)); - api::MotorCommandResponse response; - const auto status = stub_->profilePosition(&context, request, &response); - EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); - - const auto cleanup_deadline = - std::chrono::steady_clock::now() + std::chrono::milliseconds(300); - while (protocol_->quick_stop_count_.load() == 0 && - std::chrono::steady_clock::now() < cleanup_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); -} - -TEST_F(MotorServiceTest, RejectedProfilePositionPreemptedInFlightReturnsAborted) -{ - protocol_->profile_position_success_ = false; - protocol_->profile_delay_ms_ = 100; - - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(1.0); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(1.0); - - grpc::ServerContext motion_context; - api::MotorCommandResponse motion_response; - grpc::Status motion_status; - std::thread motion([&]() { - motion_status = service_->profilePosition( - &motion_context, &request, &motion_response); - }); - const auto dispatch_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->command_started_.load() && - std::chrono::steady_clock::now() < dispatch_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(protocol_->command_started_.load()); - - api::EmergencyStopRequest stop_request; - *stop_request.mutable_target() = makeTarget(); - grpc::ServerContext stop_context; - api::MotorCommandResponse stop_response; - const auto stop_status = service_->emergencyStop( - &stop_context, &stop_request, &stop_response); - - motion.join(); - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED); -} - -TEST_F(MotorServiceTest, RejectedProfileVelocityPreemptedInFlightReturnsAborted) -{ - protocol_->profile_velocity_success_ = false; - protocol_->profile_velocity_delay_ms_ = 100; - - api::ProfileVelocityRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(1.0); - - grpc::ServerContext motion_context; - api::MotorCommandResponse motion_response; - grpc::Status motion_status; - std::thread motion([&]() { - motion_status = service_->profileVelocity( - &motion_context, &request, &motion_response); - }); - const auto dispatch_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->profile_velocity_started_.load() && - std::chrono::steady_clock::now() < dispatch_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(protocol_->profile_velocity_started_.load()); - - api::EmergencyStopRequest stop_request; - *stop_request.mutable_target() = makeTarget(); - grpc::ServerContext stop_context; - api::MotorCommandResponse stop_response; - const auto stop_status = service_->emergencyStop( - &stop_context, &stop_request, &stop_response); - - motion.join(); - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED); -} - -TEST_F(MotorServiceTest, EmergencyStopDuringProfileStatusSampleCannotReturnOk) -{ - protocol_->get_q_delay_ms_ = 100; - - api::ProfilePositionRequest motion_request; - *motion_request.mutable_target() = makeTarget(); - motion_request.set_target_position_rad(1.0); - motion_request.set_max_velocity_rad_s(1.0); - motion_request.set_acceleration_rad_s2(1.0); - motion_request.mutable_wait()->set_settle_sample_count(1); - - grpc::ServerContext motion_context; - api::MotorCommandResponse motion_response; - grpc::Status motion_status; - std::thread motion([&]() { - motion_status = service_->profilePosition( - &motion_context, &motion_request, &motion_response); - }); - - const auto sample_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->get_q_started_.load() && - std::chrono::steady_clock::now() < sample_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - if (!protocol_->get_q_started_.load()) { - motion_context.TryCancel(); - motion.join(); - FAIL() << "profile status sampling did not start"; - return; - } - - api::EmergencyStopRequest stop_request; - *stop_request.mutable_target() = makeTarget(); - grpc::ServerContext stop_context; - api::MotorCommandResponse stop_response; - const auto stop_status = service_->emergencyStop( - &stop_context, &stop_request, &stop_response); - - motion.join(); - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED); - EXPECT_FALSE(motion_response.header().success()); -} - -TEST_F(MotorServiceTest, EmergencyStopPreemptsBlockingProfilePosition) -{ - protocol_->hold_position_ = true; - protocol_->profile_delay_ms_ = 50; - - api::ProfilePositionRequest motion_request; - *motion_request.mutable_target() = makeTarget(); - motion_request.set_target_position_rad(2.0); - motion_request.set_max_velocity_rad_s(1.0); - motion_request.set_acceleration_rad_s2(1.0); - motion_request.mutable_wait()->set_timeout_ms(5000); - motion_request.mutable_wait()->set_poll_period_ms(1); - - grpc::ServerContext motion_context; - api::MotorCommandResponse motion_response; - grpc::Status motion_status; - std::thread motion([&]() { - motion_status = service_->profilePosition( - &motion_context, &motion_request, &motion_response); - }); - - const auto wait_until = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->command_started_.load() && - std::chrono::steady_clock::now() < wait_until) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(protocol_->command_started_.load()); - - api::EmergencyStopRequest stop_request; - *stop_request.mutable_target() = makeTarget(); - grpc::ServerContext stop_context; - api::MotorCommandResponse stop_response; - const auto stop_status = service_->emergencyStop( - &stop_context, &stop_request, &stop_response); - - motion.join(); - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); - EXPECT_GT(protocol_->last_quick_stop_order_.load(), - protocol_->profile_command_order_.load()); - EXPECT_TRUE(stop_response.status().emergency_stopped()); -} - -TEST_F(MotorServiceTest, EnableClearsEmergencyStopLatch) -{ - api::EmergencyStopRequest stop_request; - *stop_request.mutable_target() = makeTarget(); - grpc::ServerContext stop_context; - api::MotorCommandResponse stop_response; - ASSERT_TRUE(service_->emergencyStop( - &stop_context, &stop_request, &stop_response).ok()); - - api::SetMotorEnabledRequest enable_request; - *enable_request.mutable_target() = makeTarget(); - enable_request.set_enabled(true); - grpc::ServerContext enable_context; - api::MotorCommandResponse enable_response; - const auto enable_status = service_->setEnabled( - &enable_context, &enable_request, &enable_response); - - ASSERT_TRUE(enable_status.ok()) << enable_status.error_message(); - EXPECT_FALSE(enable_response.status().emergency_stopped()); -} - -TEST_F(MotorServiceTest, RejectedEnableQuickStopsAndDisables) -{ - protocol_->torque_on_success_ = false; - - api::SetMotorEnabledRequest request; - *request.mutable_target() = makeTarget(); - request.set_enabled(true); - grpc::ServerContext context; - api::MotorCommandResponse response; - const auto status = service_->setEnabled(&context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_FALSE(response.status().emergency_stopped()); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); - EXPECT_GE(protocol_->torque_off_count_.load(), 1); -} - -TEST_F(MotorServiceTest, RejectedEnableCleanupFailureReturnsInternal) -{ - protocol_->torque_on_success_ = false; - protocol_->torque_off_success_ = false; - - api::SetMotorEnabledRequest request; - *request.mutable_target() = makeTarget(); - request.set_enabled(true); - grpc::ServerContext context; - api::MotorCommandResponse response; - const auto status = service_->setEnabled(&context, &request, &response); - - EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_TRUE(response.status().emergency_stopped()); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); - EXPECT_GE(protocol_->torque_off_count_.load(), 1); -} - -TEST_F(MotorServiceTest, RejectedEnablePreemptedInFlightReturnsAborted) -{ - protocol_->torque_on_success_ = false; - protocol_->torque_on_delay_ms_ = 100; - - api::SetMotorEnabledRequest enable_request; - *enable_request.mutable_target() = makeTarget(); - enable_request.set_enabled(true); - grpc::ServerContext enable_context; - api::MotorCommandResponse enable_response; - grpc::Status enable_status; - std::thread enabling([&]() { - enable_status = service_->setEnabled( - &enable_context, &enable_request, &enable_response); - }); - const auto dispatch_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->torque_on_started_.load() && - std::chrono::steady_clock::now() < dispatch_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(protocol_->torque_on_started_.load()); - - api::EmergencyStopRequest stop_request; - *stop_request.mutable_target() = makeTarget(); - grpc::ServerContext stop_context; - api::MotorCommandResponse stop_response; - const auto stop_status = service_->emergencyStop( - &stop_context, &stop_request, &stop_response); - - enabling.join(); - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_EQ(enable_status.error_code(), grpc::StatusCode::ABORTED); -} - -TEST_F(MotorServiceTest, SuccessfulEnableCancelledInFlightRollsBack) -{ - ASSERT_TRUE(startGrpcServer()); - protocol_->torque_on_delay_ms_ = 100; - - api::SetMotorEnabledRequest request; - *request.mutable_target() = makeTarget(); - request.set_enabled(true); - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::milliseconds(50)); - api::MotorCommandResponse response; - const auto status = stub_->setEnabled(&context, request, &response); - EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); - - const auto cleanup_deadline = - std::chrono::steady_clock::now() + std::chrono::milliseconds(400); - while ((protocol_->quick_stop_count_.load() == 0 || - protocol_->torque_off_count_.load() == 0) && - std::chrono::steady_clock::now() < cleanup_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); - EXPECT_GE(protocol_->torque_off_count_.load(), 1); -} - -TEST_F(MotorServiceTest, EmergencyStopPreemptsEnableDuringDispatch) -{ - api::EmergencyStopRequest initial_stop; - *initial_stop.mutable_target() = makeTarget(); - grpc::ServerContext initial_stop_context; - api::MotorCommandResponse initial_stop_response; - ASSERT_TRUE(service_->emergencyStop( - &initial_stop_context, &initial_stop, &initial_stop_response).ok()); - - protocol_->operation_counter_ = 0; - protocol_->last_quick_stop_order_ = 0; - protocol_->torque_on_started_ = false; - protocol_->torque_on_delay_ms_ = 50; - - api::SetMotorEnabledRequest enable_request; - *enable_request.mutable_target() = makeTarget(); - enable_request.set_enabled(true); - grpc::ServerContext enable_context; - api::MotorCommandResponse enable_response; - grpc::Status enable_status; - std::thread enabling([&]() { - enable_status = service_->setEnabled( - &enable_context, &enable_request, &enable_response); - }); - - const auto wait_until = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->torque_on_started_.load() && - std::chrono::steady_clock::now() < wait_until) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(protocol_->torque_on_started_.load()); - - api::EmergencyStopRequest stop_request; - *stop_request.mutable_target() = makeTarget(); - grpc::ServerContext stop_context; - api::MotorCommandResponse stop_response; - const auto stop_status = service_->emergencyStop( - &stop_context, &stop_request, &stop_response); - - enabling.join(); - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_EQ(enable_status.error_code(), grpc::StatusCode::ABORTED); - EXPECT_TRUE(stop_response.status().emergency_stopped()); - EXPECT_GT(protocol_->last_quick_stop_order_.load(), - protocol_->torque_on_order_.load()); - EXPECT_GE(protocol_->torque_off_count_.load(), 1); -} - -TEST_F(MotorServiceTest, SuccessfulEnablePreemptCleanupFailureReturnsInternal) -{ - protocol_->torque_on_delay_ms_ = 100; - protocol_->torque_off_success_ = false; - - api::SetMotorEnabledRequest enable_request; - *enable_request.mutable_target() = makeTarget(); - enable_request.set_enabled(true); - grpc::ServerContext enable_context; - api::MotorCommandResponse enable_response; - grpc::Status enable_status; - std::thread enabling([&]() { - enable_status = service_->setEnabled( - &enable_context, &enable_request, &enable_response); - }); - const auto dispatch_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->torque_on_started_.load() && - std::chrono::steady_clock::now() < dispatch_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(protocol_->torque_on_started_.load()); - - api::EmergencyStopRequest stop_request; - *stop_request.mutable_target() = makeTarget(); - grpc::ServerContext stop_context; - api::MotorCommandResponse stop_response; - const auto stop_status = service_->emergencyStop( - &stop_context, &stop_request, &stop_response); - - enabling.join(); - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_EQ(enable_status.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_GE(protocol_->torque_off_count_.load(), 1); -} - -TEST_F(MotorServiceTest, ConcurrentEmergencyStopsCannotBeClearedByEnable) -{ - protocol_->quick_stop_delay_ms_ = 60; - - api::EmergencyStopRequest first_request; - *first_request.mutable_target() = makeTarget(); - api::EmergencyStopRequest second_request; - *second_request.mutable_target() = makeTarget(); - grpc::ServerContext first_context; - grpc::ServerContext second_context; - api::MotorCommandResponse first_response; - api::MotorCommandResponse second_response; - grpc::Status first_status; - grpc::Status second_status; - - std::thread first([&]() { - first_status = service_->emergencyStop( - &first_context, &first_request, &first_response); - }); - const auto first_stop_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (protocol_->quick_stop_count_.load() < 1 && - std::chrono::steady_clock::now() < first_stop_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - if (protocol_->quick_stop_count_.load() < 1) { - first.join(); - FAIL() << "first emergency stop did not dispatch"; - return; - } - - std::thread second([&]() { - second_status = service_->emergencyStop( - &second_context, &second_request, &second_response); - }); - first.join(); - - const auto second_stop_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (protocol_->quick_stop_count_.load() < 3 && - std::chrono::steady_clock::now() < second_stop_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - if (protocol_->quick_stop_count_.load() < 3) { - second.join(); - FAIL() << "second emergency stop did not dispatch"; - return; - } - - api::SetMotorEnabledRequest enable_request; - *enable_request.mutable_target() = makeTarget(); - enable_request.set_enabled(true); - grpc::ServerContext enable_context; - api::MotorCommandResponse enable_response; - const auto enable_status = service_->setEnabled( - &enable_context, &enable_request, &enable_response); - - second.join(); - ASSERT_TRUE(first_status.ok()) << first_status.error_message(); - ASSERT_TRUE(second_status.ok()) << second_status.error_message(); - EXPECT_EQ(enable_status.error_code(), grpc::StatusCode::ABORTED); - - api::ProfilePositionRequest motion_request; - *motion_request.mutable_target() = makeTarget(); - motion_request.set_target_position_rad(1.0); - motion_request.set_max_velocity_rad_s(1.0); - motion_request.set_acceleration_rad_s2(1.0); - grpc::ServerContext motion_context; - api::MotorCommandResponse motion_response; - const auto motion_status = service_->profilePosition( - &motion_context, &motion_request, &motion_response); - EXPECT_EQ(motion_status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_TRUE(motion_response.status().emergency_stopped()); -} - -TEST_F(MotorServiceTest, ProfileCancellationInterruptsLongPollAndQuickStops) -{ - ASSERT_TRUE(startGrpcServer()); - protocol_->hold_position_ = true; - - api::ProfilePositionRequest request; - *request.mutable_target() = makeTarget(); - request.set_target_position_rad(1.0); - request.set_max_velocity_rad_s(1.0); - request.set_acceleration_rad_s2(1.0); - request.mutable_wait()->set_timeout_ms(5000); - request.mutable_wait()->set_poll_period_ms(1000); - - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::milliseconds(75)); - api::MotorCommandResponse response; - const auto started = std::chrono::steady_clock::now(); - const auto status = stub_->profilePosition(&context, request, &response); - EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); - - const auto stop_deadline = - std::chrono::steady_clock::now() + std::chrono::milliseconds(250); - while (protocol_->quick_stop_count_.load() == 0 && - std::chrono::steady_clock::now() < stop_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(2)); - } - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); - EXPECT_LT(std::chrono::steady_clock::now() - started, - std::chrono::milliseconds(350)); -} - -TEST_F(MotorServiceTest, CyclicPositionStreamAppliesSetpointAndStopsOnWritesDone) -{ - ASSERT_TRUE(startGrpcServer()); - - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::seconds(2)); - auto stream = stub_->streamCyclicPosition(&context); - ASSERT_NE(stream, nullptr); - - api::CyclicPositionRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget(); - open_request.mutable_open()->set_watchdog_timeout_ms(500); - ASSERT_TRUE(stream->Write(open_request)); - - api::CyclicControlResponse response; - ASSERT_TRUE(stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); - - api::CyclicPositionRequest setpoint_request; - auto* setpoint = setpoint_request.mutable_setpoint(); - setpoint->set_sequence(1); - setpoint->set_target_position_rad(0.75); - setpoint->set_target_velocity_rad_s(0.2); - ASSERT_TRUE(stream->Write(setpoint_request)); - - response.Clear(); - ASSERT_TRUE(stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED); - EXPECT_EQ(response.sequence(), 1); - EXPECT_FALSE(response.has_status()); - - ASSERT_TRUE(stream->WritesDone()); - response.Clear(); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_TRUE(response.header().success()); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_STOPPED); - EXPECT_EQ(response.sequence(), 1); - EXPECT_FALSE(stream->Read(&response)); - - const auto finish = stream->Finish(); - EXPECT_TRUE(finish.ok()) << finish.error_message(); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); -} - -TEST_F(MotorServiceTest, CyclicVelocityStreamAppliesAndStopsOnWritesDone) -{ - ASSERT_TRUE(startGrpcServer()); - - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::seconds(2)); - auto stream = stub_->streamCyclicVelocity(&context); - ASSERT_NE(stream, nullptr); - - api::CyclicVelocityRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget(); - open_request.mutable_open()->set_watchdog_timeout_ms(500); - ASSERT_TRUE(stream->Write(open_request)); - - api::CyclicControlResponse response; - ASSERT_TRUE(stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); - EXPECT_TRUE(response.has_status()); - - api::CyclicVelocityRequest setpoint_request; - auto* setpoint = setpoint_request.mutable_setpoint(); - setpoint->set_sequence(1); - setpoint->set_target_velocity_rad_s(0.6); - ASSERT_TRUE(stream->Write(setpoint_request)); - - response.Clear(); - ASSERT_TRUE(stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED); - EXPECT_EQ(response.sequence(), 1); - EXPECT_FALSE(response.has_status()); - - ASSERT_TRUE(stream->WritesDone()); - response.Clear(); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_TRUE(response.header().success()); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_STOPPED); - EXPECT_EQ(response.sequence(), 1); - EXPECT_TRUE(response.has_status()); - EXPECT_FALSE(stream->Read(&response)); - - const auto finish = stream->Finish(); - EXPECT_TRUE(finish.ok()) << finish.error_message(); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); -} - -TEST_F(MotorServiceTest, EmergencyStopDuringCyclicDispatchCannotPublishApplied) -{ - ASSERT_TRUE(startGrpcServer()); - protocol_->cyclic_position_delay_ms_ = 100; - - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::seconds(3)); - auto stream = stub_->streamCyclicPosition(&context); - ASSERT_NE(stream, nullptr); - - api::CyclicPositionRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget(); - ASSERT_TRUE(stream->Write(open_request)); - - api::CyclicControlResponse response; - ASSERT_TRUE(stream->Read(&response)); - ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); - - api::CyclicPositionRequest setpoint_request; - setpoint_request.mutable_setpoint()->set_sequence(1); - setpoint_request.mutable_setpoint()->set_target_position_rad(0.5); - ASSERT_TRUE(stream->Write(setpoint_request)); - - const auto dispatch_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->cyclic_position_started_.load() && - std::chrono::steady_clock::now() < dispatch_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(protocol_->cyclic_position_started_.load()); - - api::EmergencyStopRequest stop_request; - *stop_request.mutable_target() = makeTarget(); - grpc::ServerContext stop_context; - api::MotorCommandResponse stop_response; - grpc::Status stop_status; - std::thread stopping([&]() { - stop_status = service_->emergencyStop( - &stop_context, &stop_request, &stop_response); - }); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - stream->WritesDone(); - stopping.join(); - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - - response.Clear(); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_NE(response.phase(), api::CYCLIC_STREAM_APPLIED); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_FAILED); - EXPECT_FALSE(response.header().success()); - EXPECT_FALSE(stream->Read(&response)); - const auto finish = stream->Finish(); - EXPECT_EQ(finish.error_code(), grpc::StatusCode::ABORTED); -} - -TEST_F(MotorServiceTest, CyclicOpenSetModeExceptionReturnsInternalAndQuickStops) -{ - ASSERT_TRUE(startGrpcServer()); - protocol_->throw_set_mode_ = true; - - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::seconds(2)); - auto stream = stub_->streamCyclicPosition(&context); - ASSERT_NE(stream, nullptr); - - api::CyclicPositionRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget(); - ASSERT_TRUE(stream->Write(open_request)); - stream->WritesDone(); - - api::CyclicControlResponse response; - EXPECT_FALSE(stream->Read(&response)); - const auto finish = stream->Finish(); - EXPECT_EQ(finish.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_NE(finish.error_message().find("injected setMode exception"), - std::string::npos); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); -} - -TEST_F(MotorServiceTest, CyclicStreamReportsFailureWhenQuickStopFails) -{ - ASSERT_TRUE(startGrpcServer()); - protocol_->quick_stop_success_ = false; - - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::seconds(2)); - auto stream = stub_->streamCyclicPosition(&context); - ASSERT_NE(stream, nullptr); - - api::CyclicPositionRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget(); - open_request.mutable_open()->set_watchdog_timeout_ms(500); - ASSERT_TRUE(stream->Write(open_request)); - - api::CyclicControlResponse response; - ASSERT_TRUE(stream->Read(&response)); - ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); - ASSERT_TRUE(stream->WritesDone()); - - response.Clear(); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_FAILED); - EXPECT_FALSE(stream->Read(&response)); - const auto finish = stream->Finish(); - EXPECT_EQ(finish.error_code(), grpc::StatusCode::INTERNAL); - - api::ProfilePositionRequest profile_request; - *profile_request.mutable_target() = makeTarget(); - profile_request.set_target_position_rad(1.0); - profile_request.set_max_velocity_rad_s(1.0); - profile_request.set_acceleration_rad_s2(1.0); - grpc::ServerContext profile_context; - api::MotorCommandResponse profile_response; - const auto profile_status = service_->profilePosition( - &profile_context, &profile_request, &profile_response); - EXPECT_EQ(profile_status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_TRUE(profile_response.status().emergency_stopped()); -} - -TEST_F(MotorServiceTest, CyclicDriverExceptionReturnsInternalWithoutTerminating) -{ - ASSERT_TRUE(startGrpcServer()); - protocol_->throw_cyclic_position_ = true; - - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::seconds(2)); - auto stream = stub_->streamCyclicPosition(&context); - ASSERT_NE(stream, nullptr); - - api::CyclicPositionRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget(); - ASSERT_TRUE(stream->Write(open_request)); - - api::CyclicControlResponse response; - ASSERT_TRUE(stream->Read(&response)); - ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); - - api::CyclicPositionRequest setpoint_request; - setpoint_request.mutable_setpoint()->set_sequence(1); - setpoint_request.mutable_setpoint()->set_target_position_rad(0.5); - ASSERT_TRUE(stream->Write(setpoint_request)); - - response.Clear(); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_FAILED); - EXPECT_TRUE(stream->WritesDone()); - EXPECT_FALSE(stream->Read(&response)); - const auto finish = stream->Finish(); - EXPECT_EQ(finish.error_code(), grpc::StatusCode::INTERNAL); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); -} - -TEST_F(MotorServiceTest, CyclicPositionStreamWatchdogStopsSilentClient) -{ - ASSERT_TRUE(startGrpcServer()); - - grpc::ClientContext context; - context.set_deadline( - std::chrono::system_clock::now() + std::chrono::seconds(2)); - auto stream = stub_->streamCyclicPosition(&context); - ASSERT_NE(stream, nullptr); - - api::CyclicPositionRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget(); - open_request.mutable_open()->set_watchdog_timeout_ms(25); - ASSERT_TRUE(stream->Write(open_request)); - - api::CyclicControlResponse response; - ASSERT_TRUE(stream->Read(&response)); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); - - response.Clear(); - ASSERT_TRUE(stream->Read(&response)); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_WATCHDOG_EXPIRED); - EXPECT_FALSE(stream->Read(&response)); - - const auto finish = stream->Finish(); - EXPECT_TRUE( - finish.error_code() == grpc::StatusCode::DEADLINE_EXCEEDED || - finish.error_code() == grpc::StatusCode::CANCELLED) - << finish.error_message(); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); -} - -TEST_F(MotorServiceTest, SystemStopAllPreemptsProfileAndResumesControl) -{ - protocol_->hold_position_ = true; - - api::ProfilePositionRequest motion_request; - *motion_request.mutable_target() = makeTarget(); - motion_request.set_target_position_rad(2.0); - motion_request.set_max_velocity_rad_s(1.0); - motion_request.set_acceleration_rad_s2(1.0); - motion_request.mutable_wait()->set_timeout_ms(5000); - motion_request.mutable_wait()->set_poll_period_ms(1); - - grpc::ServerContext motion_context; - api::MotorCommandResponse motion_response; - grpc::Status motion_status; - std::thread motion([&]() { - motion_status = service_->profilePosition( - &motion_context, &motion_request, &motion_response); - }); - - const auto command_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (!protocol_->command_started_.load() && - std::chrono::steady_clock::now() < command_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - if (!protocol_->command_started_.load()) { - motion_context.TryCancel(); - motion.join(); - FAIL() << "profile command did not start"; - return; - } - - auto& coordinator = globalMotorActivityCoordinator(); - const auto ticket = coordinator.beginStopAll(); - ASSERT_TRUE(ticket.valid()); - std::string stop_error; - EXPECT_TRUE(coordinator.stopAndWait( - ticket, std::chrono::seconds(1), &stop_error)) - << stop_error; - EXPECT_TRUE(coordinator.finishStopAll(ticket, true)); - - motion.join(); - EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED); - EXPECT_FALSE(motion_response.header().success()); - EXPECT_FALSE(motion_response.status().emergency_stopped()); - EXPECT_GE(protocol_->quick_stop_count_.load(), 1); - EXPECT_GT(protocol_->last_quick_stop_order_.load(), - protocol_->profile_command_order_.load()); - - protocol_->hold_position_ = false; - api::ProfilePositionRequest resumed_request; - *resumed_request.mutable_target() = makeTarget(); - resumed_request.set_target_position_rad(0.5); - resumed_request.set_max_velocity_rad_s(1.0); - resumed_request.set_acceleration_rad_s2(1.0); - resumed_request.mutable_wait()->set_settle_sample_count(1); - grpc::ServerContext resumed_context; - api::MotorCommandResponse resumed_response; - const auto resumed_status = service_->profilePosition( - &resumed_context, &resumed_request, &resumed_response); - ASSERT_TRUE(resumed_status.ok()) << resumed_status.error_message(); - EXPECT_TRUE(resumed_response.header().success()); - EXPECT_FALSE(resumed_response.status().emergency_stopped()); -} - -TEST_F(MotorServiceTest, FailedSystemStopAllRemainsFailClosedUntilRecovery) -{ - // Resolve the motor once so its control state is registered even though no - // motion RPC is active when StopAll starts. - api::GetMotorStatusRequest status_request; - *status_request.mutable_target() = makeTarget(); - grpc::ServerContext status_context; - api::GetMotorStatusResponse status_response; - ASSERT_TRUE(service_->getStatus( - &status_context, &status_request, &status_response).ok()); - - auto& coordinator = globalMotorActivityCoordinator(); - protocol_->quick_stop_success_ = false; - const auto failed_ticket = coordinator.beginStopAll(); - ASSERT_TRUE(failed_ticket.valid()); - std::string stop_error; - EXPECT_FALSE(coordinator.stopAndWait( - failed_ticket, std::chrono::seconds(1), &stop_error)); - EXPECT_FALSE(stop_error.empty()); - EXPECT_FALSE(coordinator.finishStopAll(failed_ticket, false)); - - api::ProfilePositionRequest blocked_request; - *blocked_request.mutable_target() = makeTarget(); - blocked_request.set_target_position_rad(1.0); - blocked_request.set_max_velocity_rad_s(1.0); - blocked_request.set_acceleration_rad_s2(1.0); - grpc::ServerContext blocked_context; - api::MotorCommandResponse blocked_response; - const auto blocked_status = service_->profilePosition( - &blocked_context, &blocked_request, &blocked_response); - EXPECT_EQ(blocked_status.error_code(), grpc::StatusCode::ABORTED); - EXPECT_NE(blocked_status.error_message().find("StopAll"), - std::string::npos); - - protocol_->quick_stop_success_ = true; - const auto recovery_ticket = coordinator.beginStopAll(); - ASSERT_TRUE(recovery_ticket.valid()); - stop_error.clear(); - ASSERT_TRUE(coordinator.stopAndWait( - recovery_ticket, std::chrono::seconds(1), &stop_error)) - << stop_error; - ASSERT_TRUE(coordinator.finishStopAll(recovery_ticket, true)); - - blocked_request.mutable_wait()->set_settle_sample_count(1); - grpc::ServerContext resumed_context; - api::MotorCommandResponse resumed_response; - const auto resumed_status = service_->profilePosition( - &resumed_context, &blocked_request, &resumed_response); - ASSERT_TRUE(resumed_status.ok()) << resumed_status.error_message(); - EXPECT_TRUE(resumed_response.header().success()); - EXPECT_FALSE(resumed_response.status().emergency_stopped()); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_robot_arm_teleop_backend_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_robot_arm_teleop_backend_test.cpp deleted file mode 100644 index b320b3b4..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_robot_arm_teleop_backend_test.cpp +++ /dev/null @@ -1,525 +0,0 @@ -#include "service/grpc/server/include/grpc_robot_arm_teleop_backend.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace cmvr::service { -namespace { - -class FakeRobotArm final : public device::RobotArm { -public: - FakeRobotArm() - { - id_ = "right_arm"; - model_.name = id_; - model_.dof = 2; - model_.joint_names = {"joint_1", "joint_2"}; - model_.joint_limits = { - {-1.0, 1.0, 2.0, 5.0, 10.0}, - {-0.5, 0.5, 3.0, 6.0, 10.0}}; - - state_.connected = true; - state_.powered_on = true; - state_.robot_mode = device::RobotMode::Idle; - state_.safety_mode = device::SafetyMode::Normal; - state_.actual_joint_state.position = {0.0, 0.0}; - state_.actual_joint_state.velocity = {0.0, 0.0}; - state_.actual_joint_state.effort = {1.0, 2.0}; - state_.actual_joint_state.sequence = 7; - state_.actual_joint_state.position_valid = true; - state_.actual_joint_state.velocity_valid = true; - state_.actual_joint_state.effort_valid = true; - } - - std::string typeName() const override { return "FakeRobotArm"; } - device::RobotModel getRobotModel() const override { return model_; } - std::size_t getDof() const override { return model_.dof; } - device::ArmState getRobotState() const override - { - ++get_state_calls; - return state_; - } - device::JointGroupState getJointState() const override - { - return state_.actual_joint_state; - } - device::CartesianPose getTcpPose( - device::FrameType = device::FrameType::Base) const override - { - return {}; - } - device::RobotMode getRobotMode() const override - { - return state_.robot_mode; - } - device::SafetyMode getSafetyMode() const override - { - return state_.safety_mode; - } - device::ControlMode getControlMode() const override - { - return device::ControlMode::Servo; - } - bool supportsTeleopGroupServo() const noexcept override - { - return group_servo_capability; - } - device::JointEffortSource jointEffortSource() const noexcept override - { - return effort_source; - } - - device::Result torqueOn() override - { - ++torque_on_calls; - return device::Result::success(); - } - device::Result torqueOff() override { return device::Result::success(); } - device::Result calibrateZeroQ(const std::string&) override - { - return device::Result::success(); - } - device::Result emergencyStop() override - { - return device::Result::success(); - } - device::Result protectiveStop() override - { - return device::Result::success(); - } - device::Result setSpeedScaling(double) override - { - return device::Result::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 state_.fault; } - - device::Result moveJ( - const device::JointPositionCommand&, - const device::MotionOptions&) override - { - return device::Result::success(); - } - device::Result speedJ( - const device::JointVelocityCommand&, double, double) override - { - return device::Result::success(); - } - device::Result stopJ(double) override - { - return device::Result::success(); - } - device::Result moveL( - const device::CartesianPose&, - const device::MotionOptions&, - device::FrameType = device::FrameType::Base) override - { - return device::Result::success(); - } - device::Result speedL( - const device::CartesianVelocity&, - double, - double, - device::FrameType = device::FrameType::Base) override - { - return device::Result::success(); - } - device::Result stopL(std::optional = std::nullopt) override - { - return device::Result::success(); - } - device::Result stopMotion() override - { - ++stop_motion_calls; - return stop_motion_result; - } - - device::Result startServoMode( - const device::ServoOptions& options) override - { - ++start_servo_calls; - last_servo_period = options.period; - return start_servo_result; - } - device::Result servoJ( - const device::JointPositionCommand& target) override - { - ++servo_j_calls; - last_command = target.position; - if (servo_sleep.count() > 0) { - std::this_thread::sleep_for(servo_sleep); - } - return servo_j_result; - } - device::Result servoL( - const device::CartesianPose&, - device::FrameType = device::FrameType::Base) override - { - return device::Result::success(); - } - device::Result servoSpeedJ( - const device::JointVelocityCommand&) override - { - return device::Result::success(); - } - device::Result servoSpeedL( - const device::CartesianVelocity&, - device::FrameType = device::FrameType::Base) override - { - return device::Result::success(); - } - device::Result stopServoMode() override - { - ++stop_servo_calls; - return stop_servo_result; - } - - device::Result connect(const std::string&, int) override - { - return device::Result::success(); - } - device::Result disconnect() override - { - return device::Result::success(); - } - bool isConnected() const override { return state_.connected; } - device::Result powerOn() override { return torqueOn(); } - device::Result powerOff() override { return torqueOff(); } - device::Result brakeRelease() override - { - return device::Result::success(); - } - device::Result shutdown() override - { - return device::Result::success(); - } - device::Result clearFault() override - { - return device::Result::success(); - } - device::Result unlockProtectiveStop() override - { - return device::Result::success(); - } - device::Result loadProgram(const std::string&) override - { - return device::Result::success(); - } - device::Result playProgram() override - { - return device::Result::success(); - } - device::Result pauseProgram() override - { - return device::Result::success(); - } - device::Result stopProgram() override - { - return device::Result::success(); - } - std::vector ik( - const std::string&, - const std::string&, - const device::CartesianPose&) override - { - return {}; - } - std::shared_ptr kinematicsSolver() const override - { - return nullptr; - } - device::CartesianPose fk( - const std::string&, const std::string&) override - { - return {}; - } - device::CartesianPose fk(bool = true) override { return {}; } - device::CartesianVelocity getSpeedLCommandTwistBase() const override - { - return {}; - } - bool busy() const override { return false; } - - bool group_servo_capability{true}; - device::JointEffortSource effort_source{ - device::JointEffortSource::Unspecified}; - mutable int get_state_calls{0}; - int torque_on_calls{0}; - int start_servo_calls{0}; - int servo_j_calls{0}; - int stop_motion_calls{0}; - int stop_servo_calls{0}; - double last_servo_period{0.0}; - std::vector last_command; - std::chrono::microseconds servo_sleep{0}; - device::Result start_servo_result{device::Result::success()}; - device::Result servo_j_result{device::Result::success()}; - device::Result stop_motion_result{device::Result::success()}; - device::Result stop_servo_result{device::Result::success()}; - device::RobotModel model_; - device::ArmState state_; -}; - -config::ArmTeleopBackendConfig validConfig() -{ - config::ArmTeleopBackendConfig config; - config.set_enable(true); - config.set_device_id("right_arm"); - config.set_model_sha256(std::string(64, 'a')); - config.set_calibration_sha256(std::string(64, 'b')); - config.set_base_frame("base_link"); - config.set_tool_frame("tool_link"); - config.set_servo_period_s(0.01); - config.set_max_apply_duration_us(5000); - config.set_require_powered(true); - config.set_max_initial_position_step_rad(0.2); - config.set_max_position_step_rad(0.015); - return config; -} - -arm_teleop::JointSetpoint setpoint( - const double first, - const double second, - const double first_velocity = 0.0, - const double second_velocity = 0.0) -{ - arm_teleop::JointSetpoint value; - value.set_sequence(1); - value.add_position_rad(first); - value.add_position_rad(second); - value.add_velocity_rad_s(first_velocity); - value.add_velocity_rad_s(second_velocity); - value.set_valid_for_us(5000); - return value; -} - -arm_teleop::OpenSession openRequest() -{ - arm_teleop::OpenSession request; - request.set_requested_command_rate_hz(100); - return request; -} - -std::chrono::steady_clock::time_point liveDeadline() -{ - return std::chrono::steady_clock::now() + - std::chrono::seconds(1); -} - -TEST(RobotArmTeleopBackendTest, RejectsArmWithoutExplicitGroupServoCapability) -{ - auto arm = std::make_shared(); - arm->group_servo_capability = false; - const auto backend = - makeRobotArmTeleopBackend(arm, validConfig()); - - EXPECT_FALSE(backend->available()); - EXPECT_NE( - backend->unavailableReason().find("capability"), - std::string::npos); - EXPECT_EQ(arm->start_servo_calls, 0); -} - -TEST(RobotArmTeleopBackendTest, OpenStartsServoButNeverPowersArm) -{ - auto arm = std::make_shared(); - const auto backend = - makeRobotArmTeleopBackend(arm, validConfig()); - - ASSERT_TRUE(backend->available()) - << backend->unavailableReason(); - EXPECT_TRUE(backend->open(openRequest()).success); - EXPECT_EQ(arm->start_servo_calls, 1); - EXPECT_DOUBLE_EQ(arm->last_servo_period, 0.01); - EXPECT_EQ(arm->torque_on_calls, 0); -} - -TEST(RobotArmTeleopBackendTest, AppliesSetpointAndAlwaysRunsBothStopPaths) -{ - auto arm = std::make_shared(); - const auto backend = - makeRobotArmTeleopBackend(arm, validConfig()); - ASSERT_TRUE(backend->open(openRequest()).success); - - ASSERT_TRUE( - backend->applySetpoint( - setpoint(0.1, 0.1), liveDeadline()).success); - EXPECT_EQ(arm->servo_j_calls, 1); - EXPECT_EQ(arm->last_command, (std::vector{0.1, 0.1})); - - arm->stop_motion_result = device::Result::failure( - device::ArmErrorCode::CommandFailed, "motion stop failed"); - const auto result = backend->stop( - arm_teleop::STOP_REASON_OPERATOR_REQUEST, "test"); - EXPECT_FALSE(result.success); - EXPECT_EQ(arm->stop_motion_calls, 1); - EXPECT_EQ(arm->stop_servo_calls, 1); -} - -TEST(RobotArmTeleopBackendTest, RejectsInitialAndContinuousPositionSteps) -{ - auto arm = std::make_shared(); - const auto backend = - makeRobotArmTeleopBackend(arm, validConfig()); - ASSERT_TRUE(backend->open(openRequest()).success); - - EXPECT_EQ( - backend->applySetpoint( - setpoint(0.21, 0.0), liveDeadline()).status_code, - grpc::StatusCode::OUT_OF_RANGE); - ASSERT_TRUE( - backend->applySetpoint( - setpoint(0.1, 0.1), liveDeadline()).success); - EXPECT_EQ( - backend->applySetpoint( - setpoint(0.12, 0.1), liveDeadline()).status_code, - grpc::StatusCode::OUT_OF_RANGE); - EXPECT_EQ(arm->servo_j_calls, 1); -} - -TEST(RobotArmTeleopBackendTest, RejectsJointPositionAndVelocityLimits) -{ - auto arm = std::make_shared(); - const auto backend = - makeRobotArmTeleopBackend(arm, validConfig()); - ASSERT_TRUE(backend->open(openRequest()).success); - - EXPECT_EQ( - backend->applySetpoint( - setpoint(1.01, 0.0), liveDeadline()).status_code, - grpc::StatusCode::OUT_OF_RANGE); - EXPECT_EQ( - backend->applySetpoint( - setpoint(0.0, 0.0, 2.01, 0.0), liveDeadline()) - .status_code, - grpc::StatusCode::OUT_OF_RANGE); - EXPECT_EQ(arm->servo_j_calls, 0); -} - -TEST(RobotArmTeleopBackendTest, DetectsServoApplyTimeout) -{ - auto arm = std::make_shared(); - auto config = validConfig(); - config.set_max_apply_duration_us(500); - const auto backend = - makeRobotArmTeleopBackend(arm, config); - ASSERT_TRUE(backend->open(openRequest()).success); - arm->servo_sleep = std::chrono::microseconds(1500); - - const auto result = - backend->applySetpoint( - setpoint(0.1, 0.1), liveDeadline()); - EXPECT_FALSE(result.success); - EXPECT_EQ( - result.status_code, - grpc::StatusCode::DEADLINE_EXCEEDED); - EXPECT_EQ(arm->servo_j_calls, 1); -} - -TEST(RobotArmTeleopBackendTest, RejectsInsufficientDeadlineBudgetBeforeDispatch) -{ - auto arm = std::make_shared(); - const auto backend = - makeRobotArmTeleopBackend(arm, validConfig()); - ASSERT_TRUE(backend->open(openRequest()).success); - - const auto result = backend->applySetpoint( - setpoint(0.1, 0.1), - std::chrono::steady_clock::now() + - std::chrono::microseconds(100)); - EXPECT_FALSE(result.success); - EXPECT_EQ( - result.status_code, - grpc::StatusCode::DEADLINE_EXCEEDED); - EXPECT_EQ(arm->servo_j_calls, 0); -} - -TEST(RobotArmTeleopBackendTest, ExpiredDeadlineNeverDispatchesServoCommand) -{ - auto arm = std::make_shared(); - const auto backend = - makeRobotArmTeleopBackend(arm, validConfig()); - ASSERT_TRUE(backend->open(openRequest()).success); - - const auto result = backend->applySetpoint( - setpoint(0.1, 0.1), - std::chrono::steady_clock::now()); - EXPECT_FALSE(result.success); - EXPECT_EQ( - result.status_code, - grpc::StatusCode::DEADLINE_EXCEEDED); - EXPECT_EQ(arm->servo_j_calls, 0); -} - -TEST(RobotArmTeleopBackendTest, RejectsUnsupportedRateAndEarlyRedispatch) -{ - auto arm = std::make_shared(); - const auto backend = - makeRobotArmTeleopBackend(arm, validConfig()); - auto too_fast = openRequest(); - too_fast.set_requested_command_rate_hz(101); - EXPECT_EQ( - backend->open(too_fast).status_code, - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ(arm->start_servo_calls, 0); - - ASSERT_TRUE(backend->open(openRequest()).success); - ASSERT_TRUE( - backend->applySetpoint( - setpoint(0.1, 0.1), liveDeadline()).success); - EXPECT_EQ( - backend->applySetpoint( - setpoint(0.105, 0.105), liveDeadline()).status_code, - grpc::StatusCode::RESOURCE_EXHAUSTED); - EXPECT_EQ(arm->servo_j_calls, 1); -} - -TEST(RobotArmTeleopBackendTest, SnapshotUsesCacheAndHidesUnverifiedEffort) -{ - auto arm = std::make_shared(); - const auto backend = - makeRobotArmTeleopBackend(arm, validConfig()); - ASSERT_TRUE(backend->open(openRequest()).success); - const int calls_after_open = arm->get_state_calls; - - const auto first = backend->snapshot(); - const auto second = backend->snapshot(); - EXPECT_EQ(arm->get_state_calls, calls_after_open); - EXPECT_TRUE(first.joint_state.position_valid()); - EXPECT_TRUE(first.joint_state.velocity_valid()); - EXPECT_FALSE(first.joint_state.effort_valid()); - EXPECT_EQ(first.joint_state.effort_nm_size(), 0); - EXPECT_EQ( - second.joint_state.effort_source(), - arm_teleop::EFFORT_SOURCE_UNSPECIFIED); -} - -TEST(RobotArmTeleopBackendTest, PublishesEffortOnlyWithExplicitSource) -{ - auto arm = std::make_shared(); - arm->effort_source = device::JointEffortSource::JointSensor; - const auto backend = - makeRobotArmTeleopBackend(arm, validConfig()); - ASSERT_TRUE(backend->open(openRequest()).success); - - const auto snapshot = backend->snapshot(); - EXPECT_TRUE(backend->supportsForceFeedback()); - EXPECT_TRUE(snapshot.joint_state.effort_valid()); - EXPECT_EQ(snapshot.joint_state.effort_nm_size(), 2); - EXPECT_EQ( - snapshot.joint_state.effort_source(), - arm_teleop::EFFORT_SOURCE_JOINT_SENSOR); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_security_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_security_test.cpp deleted file mode 100644 index 17b1e1f7..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_security_test.cpp +++ /dev/null @@ -1,294 +0,0 @@ -#include "service/grpc/server/include/grpc_security.h" - -#include -#include -#include -#include - -#include -#include - -#include "cmvr/api/agv_service.pb.h" -#include "cmvr/api/arm_service.pb.h" -#include "cmvr/api/arm_teleop_v1.pb.h" -#include "cmvr/api/biohead_service.pb.h" -#include "cmvr/api/camera_service.pb.h" -#include "cmvr/api/dexhand_service.pb.h" -#include "cmvr/api/hlc_service.pb.h" -#include "cmvr/api/microphone_service.pb.h" -#include "cmvr/api/motor_service.pb.h" -#include "cmvr/api/speaker_service.pb.h" -#include "cmvr/api/system_service.pb.h" -#include "cmvr/api/test_service.pb.h" - -namespace cmvr::service { -namespace { - -GrpcMethodPolicy readPolicy() -{ - return { - "/cmvr.api.SystemService/GetSystemInfo", - GrpcAccessClass::Read, - GrpcRole::Observer}; -} - -GrpcMethodPolicy recoveryPolicy() -{ - return { - "/cmvr.api.SystemService/RecoverSafetyState", - GrpcAccessClass::Recover, - GrpcRole::SafetyAdmin}; -} - -class SafetyAdminProvider final : public GrpcAuthenticationProvider { -public: - GrpcAuthenticationResult authenticate(const GrpcCallFacts&) const override - { - GrpcPrincipal principal; - principal.id = "maintenance"; - principal.method = GrpcAuthenticationMethod::StaticToken; - principal.authenticated = true; - principal.roles = {GrpcRole::SafetyAdmin}; - return {std::move(principal), grpc::Status::OK}; - } -}; - -TEST(GrpcSecurityConfigTest, - LegacyConfigPreservesAnonymousAccessButDisablesRecovery) -{ - config::GRPCServerConfig config; - const auto result = resolveGrpcSecurityConfig(config, "0.0.0.0"); - - ASSERT_TRUE(result.valid) << result.error; - EXPECT_TRUE(result.config.legacy_compatibility); - EXPECT_TRUE(result.config.insecure_non_loopback); - EXPECT_EQ( - result.config.authentication, GrpcAuthenticationMethod::Disabled); - EXPECT_EQ( - result.config.recovery_exposure, GrpcRecoveryExposure::Disabled); - EXPECT_FALSE(result.warnings.empty()); -} - -TEST(GrpcSecurityConfigTest, - ExplicitNonLoopbackInsecureRequiresAcknowledgement) -{ - config::GRPCServerConfig config; - auto* security = config.mutable_security(); - security->set_transport_mode(config::GRPCSecurityConfig::INSECURE); - security->set_authentication_mode(config::GRPCSecurityConfig::DISABLED); - security->set_recovery_exposure( - config::GRPCSecurityConfig::RECOVERY_DISABLED); - - const auto rejected = resolveGrpcSecurityConfig(config, "0.0.0.0"); - EXPECT_FALSE(rejected.valid); - - security->set_allow_insecure_non_loopback(true); - const auto accepted = resolveGrpcSecurityConfig(config, "0.0.0.0"); - ASSERT_TRUE(accepted.valid) << accepted.error; - EXPECT_TRUE(accepted.config.insecure_non_loopback); -} - -TEST(GrpcSecurityConfigTest, - UnsupportedAuthenticationNeverFallsBackToDisabled) -{ - config::GRPCServerConfig config; - auto* security = config.mutable_security(); - security->set_transport_mode(config::GRPCSecurityConfig::INSECURE); - security->set_authentication_mode( - config::GRPCSecurityConfig::STATIC_TOKEN); - security->set_recovery_exposure( - config::GRPCSecurityConfig::RECOVERY_AUTHORIZED); - - const auto result = resolveGrpcSecurityConfig(config, "127.0.0.1"); - EXPECT_FALSE(result.valid); - EXPECT_NE(result.error.find("DISABLED"), std::string::npos); -} - -TEST(GrpcSecurityConfigTest, EnabledRecoveryRequiresPersistentAuditFile) -{ - config::GRPCServerConfig config; - auto* security = config.mutable_security(); - security->set_transport_mode(config::GRPCSecurityConfig::INSECURE); - security->set_authentication_mode(config::GRPCSecurityConfig::DISABLED); - security->set_recovery_exposure( - config::GRPCSecurityConfig::RECOVERY_LOCAL_ONLY); - - const auto rejected = - resolveGrpcSecurityConfig(config, "127.0.0.1"); - EXPECT_FALSE(rejected.valid); - EXPECT_NE(rejected.error.find("audit_file"), std::string::npos); - - security->set_audit_file("/var/lib/cmvr-es/recovery-audit.jsonl"); - const auto accepted = - resolveGrpcSecurityConfig(config, "127.0.0.1"); - ASSERT_TRUE(accepted.valid) << accepted.error; - EXPECT_EQ( - accepted.config.recovery_audit_file, - "/var/lib/cmvr-es/recovery-audit.jsonl"); -} - -TEST(GrpcSecurityGatewayTest, DisabledProviderDoesNotTrustIdentityMetadata) -{ - auto gateway = makeDefaultGrpcSecurityGateway(); - GrpcCallFacts facts; - facts.peer = "ipv4:10.0.0.5:12345"; - facts.metadata.emplace("principal", "safety-admin"); - facts.metadata.emplace("role", "SafetyAdmin"); - - const auto call = gateway->beginCall(std::move(facts), readPolicy()); - - ASSERT_TRUE(call.allowed()) << call.status().error_message(); - EXPECT_EQ(call.context().principal.id, "anonymous"); - EXPECT_FALSE(call.context().principal.authenticated); - ASSERT_EQ(call.context().principal.roles.size(), 1U); - EXPECT_EQ(call.context().principal.roles.front(), GrpcRole::Anonymous); -} - -TEST(GrpcSecurityGatewayTest, RecoveryIsDisabledBeforeLedgerAdmission) -{ - auto gateway = makeDefaultGrpcSecurityGateway(); - const auto call = gateway->beginCall(GrpcCallFacts{}, recoveryPolicy()); - - EXPECT_FALSE(call.allowed()); - EXPECT_EQ( - call.status().error_code(), grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ(call.status().error_message(), "RECOVERY_RPC_DISABLED"); -} - -TEST(GrpcSecurityGatewayTest, LocalOnlyRecoveryUsesActualPeerClassification) -{ - GrpcSecurityRuntimeConfig config; - config.recovery_exposure = GrpcRecoveryExposure::LocalOnly; - auto gateway = makeGrpcSecurityGateway(config); - - GrpcCallFacts remote; - remote.peer = "ipv4:192.168.1.20:42000"; - EXPECT_FALSE(gateway->beginCall(remote, recoveryPolicy()).allowed()); - - GrpcCallFacts local; - local.peer = "ipv4:127.0.0.1:42000"; - EXPECT_TRUE(gateway->beginCall(local, recoveryPolicy()).allowed()); - - EXPECT_TRUE(isLocalGrpcPeer("ipv6:[::1]")); - EXPECT_TRUE(isLocalGrpcPeer("unix:/run/cmvr-es.sock")); - EXPECT_FALSE(isLocalGrpcPeer("ipv4:10.0.0.1:50051")); -} - -TEST(GrpcSecurityGatewayTest, AuthorizedRecoveryAcceptsSafetyAdminProvider) -{ - GrpcSecurityRuntimeConfig config; - config.authentication = GrpcAuthenticationMethod::StaticToken; - config.recovery_exposure = GrpcRecoveryExposure::Authorized; - auto gateway = std::make_shared( - config, - std::make_shared(), - std::make_shared( - GrpcRecoveryExposure::Authorized)); - - const auto call = gateway->beginCall(GrpcCallFacts{}, recoveryPolicy()); - ASSERT_TRUE(call.allowed()) << call.status().error_message(); - EXPECT_EQ(call.context().principal.id, "maintenance"); -} - -TEST(GrpcSecurityGatewayTest, AuditUsesEffectiveServerPrincipal) -{ - std::vector records; - GrpcSecurityRuntimeConfig config; - auto gateway = makeGrpcSecurityGateway( - config, - [&records](const GrpcSecurityAuditRecord& record) { - records.push_back(record); - }); - - GrpcCallFacts facts; - facts.metadata.emplace("x-correlation-id", "request-42"); - ASSERT_TRUE(gateway->beginCall(std::move(facts), readPolicy()).allowed()); - - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records.front().correlation_id, "request-42"); - EXPECT_EQ(records.front().principal_id, "anonymous"); - EXPECT_FALSE(records.front().authenticated); -} - -TEST(GrpcMethodPolicyRegistryTest, RejectsDuplicatesAndSortsSnapshot) -{ - GrpcMethodPolicyRegistry registry; - EXPECT_TRUE(registry.registerPolicy(recoveryPolicy())); - EXPECT_TRUE(registry.registerPolicy(readPolicy())); - EXPECT_FALSE(registry.registerPolicy(readPolicy())); - - const auto policies = registry.snapshot(); - ASSERT_EQ(policies.size(), 2U); - EXPECT_LT(policies[0].full_method_name, policies[1].full_method_name); - EXPECT_TRUE(registry.find(readPolicy().full_method_name).has_value()); - EXPECT_FALSE(registry.find("/unknown/method").has_value()); -} - -TEST(GrpcMethodPolicyRegistryTest, - DefaultRegistryCoversEveryCompiledApiMethod) -{ - const auto& registry = defaultGrpcMethodPolicyRegistry(); - const auto* pool = google::protobuf::DescriptorPool::generated_pool(); - const std::vector services{ - "cmvr.api.AgvService", - "cmvr.api.ArmService", - "cmvr.api.armteleop.v1.ArmTeleopService", - "cmvr.api.BioHeadService", - "cmvr.api.CameraService", - "cmvr.api.DexHandService", - "cmvr.api.HlcService", - "cmvr.api.MicPhoneService", - "cmvr.api.MotorService", - "cmvr.api.SpeakerService", - "cmvr.api.SystemService", - "cmvr.api.TestService"}; - - std::size_t method_count = 0; - for (const auto& service_name : services) { - const auto* service = pool->FindServiceByName(service_name); - ASSERT_NE(service, nullptr) << service_name; - for (int index = 0; index < service->method_count(); ++index) { - const auto* method = service->method(index); - const std::string full_name = - "/" + service_name + "/" + - std::string(method->name()); - const auto policy = registry.find(full_name); - ASSERT_TRUE(policy.has_value()) << full_name; - EXPECT_EQ( - policy->mutating, - policy->access != GrpcAccessClass::Read) << full_name; - if (policy->mutating) { - EXPECT_NE( - policy->command_intent, - safety::CommandIntent::Observe) << full_name; - } - ++method_count; - } - } - EXPECT_EQ(registry.snapshot().size(), method_count); -} - -TEST(GrpcMethodPolicyRegistryTest, - TouchIsAControlActuationRatherThanAReadOnlySensorCall) -{ - const auto policy = defaultGrpcMethodPolicyRegistry().find( - "/cmvr.api.HlcService/touch"); - ASSERT_TRUE(policy.has_value()); - EXPECT_EQ(policy->access, GrpcAccessClass::Mutate); - EXPECT_EQ(policy->command_intent, safety::CommandIntent::Actuate); - EXPECT_EQ(policy->policy_family, safety::SafetyPolicyFamily::Control); - EXPECT_TRUE(policy->mutating); - EXPECT_FALSE(policy->safety_lane); -} - -TEST(GrpcMethodPolicyRegistryTest, UnregisteredMethodFailsClosed) -{ - const auto call = beginRegisteredGrpcCall( - makeDefaultGrpcSecurityGateway(), nullptr, - "/cmvr.api.UnknownService/Mutate"); - EXPECT_FALSE(call.allowed()); - EXPECT_EQ(call.status().error_code(), grpc::StatusCode::INTERNAL); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp deleted file mode 100644 index 3b3ba689..00000000 --- a/cmvr-es/service/grpc/server/tests/grpc_system_service_test.cpp +++ /dev/null @@ -1,3435 +0,0 @@ -#include "service/grpc/server/include/grpc_system_service.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "cmvr/config/task_manager_config/task_manager_config.pb.h" -#include "devices/agv/abstract_agv.h" -#include "devices/arm/robot_arm.h" -#include "devices/camera/abstract_camera.h" -#include "devices/microphone/abstract_microphone.h" -#include "devices/speaker/abstract_speaker.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" -#include "manager/device_manager/include/device_manager.h" -#include "manager/media_source_manager/include/device_media_source_adapter.h" -#include "manager/task_manager/include/task_manager.h" -#include "service/grpc/action/include/action_queue_executor.h" -#include "service/grpc/server/include/camera_operational_activity_registry.h" -#include "service/grpc/server/include/camera_ptz_activity_registry.h" -#include "service/grpc/server/include/grpc_camera_service.h" -#include "service/grpc/server/include/grpc_recovery_audit.h" -#include "service/grpc/server/include/grpc_security.h" -#include "service/grpc/server/include/media_activity_coordinator.h" -#include "service/grpc/server/include/motor_activity_coordinator.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" -#include "task/task_factory.h" - -namespace cmvr::service { -namespace { - -class AllowAllAuthorizationPolicy final : public GrpcAuthorizationPolicy { -public: - GrpcAuthorizationDecision authorize( - const GrpcRequestContext&, - const GrpcMethodPolicy&) const override - { - return {true, grpc::Status::OK}; - } -}; - -class MemoryRecoveryAuditSink final : public RecoveryAuditSink { -public: - bool append( - const RecoveryAuditRecord& record, - std::string* error) noexcept override - { - if (fail) { - if (error) { - *error = "injected audit failure"; - } - return false; - } - records.push_back(record); - return true; - } - - bool fail{false}; - std::vector records; -}; - -std::shared_ptr makeAllowAllRecoveryGateway() -{ - GrpcSecurityRuntimeConfig config; - config.recovery_exposure = GrpcRecoveryExposure::LocalOnly; - return std::make_shared( - config, - std::make_shared(), - std::make_shared()); -} - -class SnapshotDevice final : public device::AbstractDevice { -public: - SnapshotDevice(std::string id, - device::DeviceKind kind, - std::string type_name, - device::DeviceHealthSnapshot health = {}) - : AbstractDevice(std::move(id)), - kind_(kind), - type_name_(std::move(type_name)), - health_(std::move(health)) - { - } - - device::DeviceKind kind() const noexcept override { return kind_; } - std::string typeName() const override { return type_name_; } - device::DeviceHealthSnapshot healthSnapshot() override - { - return health_; - } - bool stop() override - { - std::unique_lock lock(stop_mutex_); - ++stop_calls_; - if (block_next_stop_) { - block_next_stop_ = false; - stop_started_ = true; - stop_started_cv_.notify_all(); - stop_release_cv_.wait( - lock, - [this]() { return release_stop_; }); - } - return true; - } - int stopCalls() const - { - std::lock_guard lock(stop_mutex_); - return stop_calls_; - } - void blockNextStop() - { - std::lock_guard lock(stop_mutex_); - block_next_stop_ = true; - stop_started_ = false; - release_stop_ = false; - } - bool waitForStop(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(stop_mutex_); - return stop_started_cv_.wait_for( - lock, timeout, [this]() { return stop_started_; }); - } - void releaseStop() - { - { - std::lock_guard lock(stop_mutex_); - release_stop_ = true; - } - stop_release_cv_.notify_all(); - } - -private: - device::DeviceKind kind_; - std::string type_name_; - device::DeviceHealthSnapshot health_; - mutable std::mutex stop_mutex_; - std::condition_variable stop_started_cv_; - std::condition_variable stop_release_cv_; - int stop_calls_{0}; - bool block_next_stop_{false}; - bool stop_started_{false}; - bool release_stop_{false}; -}; - -class StopAllTestCamera final : public device::AbstractCamera { -public: - StopAllTestCamera(std::string id, - const bool stop_clears_recording = true, - const bool stop_throws = false) - : stop_clears_recording_(stop_clears_recording), - stop_throws_(stop_throws) - { - id_ = std::move(id); - state_.is_initialized = true; - state_.is_recording = true; - } - - std::string typeName() const override { return "StopAllTestCamera"; } - - void getState(device::CameraState& state) override - { - std::lock_guard lock(mutex_); - state = state_; - } - - void stopRecording() override - { - { - std::lock_guard lock(mutex_); - ++stop_recording_calls_; - if (stop_throws_) { - stop_recording_condition_.notify_all(); - throw std::runtime_error( - "injected camera recording stop failure"); - } - if (stop_clears_recording_) { - state_.is_recording = false; - } - } - stop_recording_condition_.notify_all(); - } - - bool startOperationalActivity() override - { - std::lock_guard lock(mutex_); - operational_active_ = true; - return true; - } - - bool stopOperationalActivity() override - { - std::lock_guard lock(mutex_); - ++operational_stop_calls_; - if (state_.is_recording) { - return false; - } - operational_active_ = false; - return true; - } - - bool controlPtz(device::PtzCommand, - const bool stop, - int) override - { - std::lock_guard lock(mutex_); - ++ptz_calls_; - ptz_active_ = !stop; - return true; - } - - bool stop() override - { - std::lock_guard lock(mutex_); - ++lifecycle_stop_calls_; - return true; - } - - bool isRecording() const - { - std::lock_guard lock(mutex_); - return state_.is_recording; - } - - void setRecording(const bool recording) - { - std::lock_guard lock(mutex_); - state_.is_recording = recording; - } - - void startRecording(const std::string&) override - { - { - std::unique_lock lock(start_recording_control_mutex_); - if (block_next_start_recording_) { - block_next_start_recording_ = false; - start_recording_entered_ = true; - start_recording_entered_condition_.notify_all(); - start_recording_release_condition_.wait( - lock, [this] { return release_start_recording_; }); - } - } - { - std::lock_guard lock(mutex_); - state_.is_recording = true; - ++start_recording_calls_; - } - start_recording_condition_.notify_all(); - } - - void blockNextStartRecording() - { - std::lock_guard lock(start_recording_control_mutex_); - block_next_start_recording_ = true; - start_recording_entered_ = false; - release_start_recording_ = false; - } - - bool waitForStartRecordingEntered( - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(start_recording_control_mutex_); - return start_recording_entered_condition_.wait_for( - lock, timeout, [this] { return start_recording_entered_; }); - } - - void releaseBlockedStartRecording() - { - { - std::lock_guard lock(start_recording_control_mutex_); - release_start_recording_ = true; - } - start_recording_release_condition_.notify_all(); - } - - bool waitForStartRecordingCalls( - const int expected, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return start_recording_condition_.wait_for( - lock, - timeout, - [this, expected] { - return start_recording_calls_ >= expected; - }); - } - - int stopRecordingCalls() const - { - std::lock_guard lock(mutex_); - return stop_recording_calls_; - } - - bool waitForStopRecordingCalls( - const int expected, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return stop_recording_condition_.wait_for( - lock, - timeout, - [this, expected] { - return stop_recording_calls_ >= expected; - }); - } - - int lifecycleStopCalls() const - { - std::lock_guard lock(mutex_); - return lifecycle_stop_calls_; - } - - int operationalStopCalls() const - { - std::lock_guard lock(mutex_); - return operational_stop_calls_; - } - - bool operationalActive() const - { - std::lock_guard lock(mutex_); - return operational_active_; - } - - bool ptzActive() const - { - std::lock_guard lock(mutex_); - return ptz_active_; - } - - int ptzCalls() const - { - std::lock_guard lock(mutex_); - return ptz_calls_; - } - -private: - mutable std::mutex mutex_; - std::condition_variable stop_recording_condition_; - std::condition_variable start_recording_condition_; - mutable std::mutex start_recording_control_mutex_; - std::condition_variable start_recording_entered_condition_; - std::condition_variable start_recording_release_condition_; - int start_recording_calls_{0}; - int stop_recording_calls_{0}; - int lifecycle_stop_calls_{0}; - int operational_stop_calls_{0}; - int ptz_calls_{0}; - bool operational_active_{false}; - bool ptz_active_{false}; - bool block_next_start_recording_{false}; - bool start_recording_entered_{false}; - bool release_start_recording_{false}; - bool stop_clears_recording_{true}; - bool stop_throws_{false}; -}; - -class StopAllTestMicrophone final : public device::AbstractMicrophone { -public: - StopAllTestMicrophone(std::string id, - const bool stop_clears_recording = true, - const bool stop_throws = false) - : stop_clears_recording_(stop_clears_recording), - stop_throws_(stop_throws) - { - id_ = std::move(id); - state_.is_initialized = true; - state_.is_recording = true; - } - - std::string typeName() const override - { - return "StopAllTestMicrophone"; - } - - void getState(device::MicrophoneState& state) override - { - std::lock_guard lock(mutex_); - state = state_; - } - - void stopRecording() override - { - std::lock_guard lock(mutex_); - ++stop_recording_calls_; - if (stop_throws_) { - throw std::runtime_error( - "injected microphone recording stop failure"); - } - if (stop_clears_recording_) { - state_.is_recording = false; - } - } - - bool stop() override - { - std::lock_guard lock(mutex_); - ++lifecycle_stop_calls_; - return true; - } - - bool isRecording() const - { - std::lock_guard lock(mutex_); - return state_.is_recording; - } - - int stopRecordingCalls() const - { - std::lock_guard lock(mutex_); - return stop_recording_calls_; - } - - int lifecycleStopCalls() const - { - std::lock_guard lock(mutex_); - return lifecycle_stop_calls_; - } - -private: - mutable std::mutex mutex_; - int stop_recording_calls_{0}; - int lifecycle_stop_calls_{0}; - bool stop_clears_recording_{true}; - bool stop_throws_{false}; -}; - -class StopAllTestSpeaker final : public device::AbstractSpeaker { -public: - explicit StopAllTestSpeaker(std::string id, - const bool playback_stop_result = true) - : playback_stop_result_(playback_stop_result) - { - id_ = std::move(id); - state_.is_initialized = true; - state_.is_running = true; - state_.is_decoding = true; - } - - std::string typeName() const override { return "StopAllTestSpeaker"; } - - void getState(device::SpeakerState& state) override - { - std::lock_guard lock(mutex_); - state = state_; - } - - bool stopPlayback() override - { - std::lock_guard lock(mutex_); - ++stop_playback_calls_; - if (playback_stop_result_) { - state_.is_running = false; - state_.is_decoding = false; - } - return playback_stop_result_; - } - - bool stop() override - { - std::lock_guard lock(mutex_); - ++lifecycle_stop_calls_; - return true; - } - - int stopPlaybackCalls() const - { - std::lock_guard lock(mutex_); - return stop_playback_calls_; - } - - int lifecycleStopCalls() const - { - std::lock_guard lock(mutex_); - return lifecycle_stop_calls_; - } - -private: - mutable std::mutex mutex_; - int stop_playback_calls_{0}; - int lifecycle_stop_calls_{0}; - bool playback_stop_result_{true}; -}; - -class BlockingStopTask final : public task::Task { -public: - explicit BlockingStopTask(std::string id) - : id_(std::move(id)) - { - } - - const std::string& id() const override { return id_; } - task::TaskRunMode runMode() const override - { - return task::TaskRunMode::BLOCKING_SERVICE; - } - bool init() override { return true; } - bool step(double) override { return true; } - void stop() override { lifecycle_stop_calls_.fetch_add(1); } - bool stopActivity() override - { - std::unique_lock lock(mutex_); - ++stop_activity_calls_; - stop_entered_ = true; - condition_.notify_all(); - condition_.wait(lock, [this] { return release_stop_; }); - return true; - } - task::TaskState state() const override - { - return task::TaskState::IDLE; - } - bool isBusy() const override { return false; } - bool isFinished() const override { return false; } - bool isFailed() const override { return false; } - std::string stateString() const override { return "IDLE"; } - std::string detailStatusString() const override { return "IDLE"; } - - bool waitForStopActivity(const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [this] { return stop_entered_; }); - } - - void releaseStopActivity() - { - { - std::lock_guard lock(mutex_); - release_stop_ = true; - } - condition_.notify_all(); - } - - int stopActivityCalls() const - { - std::lock_guard lock(mutex_); - return stop_activity_calls_; - } - - int lifecycleStopCalls() const - { - return lifecycle_stop_calls_.load(); - } - -private: - std::string id_; - mutable std::mutex mutex_; - std::condition_variable condition_; - std::atomic lifecycle_stop_calls_{0}; - int stop_activity_calls_{0}; - bool stop_entered_{false}; - bool release_stop_{false}; -}; - -std::shared_ptr blocking_stop_task; - -class ActionTrace final { -public: - using TimePoint = std::chrono::steady_clock::time_point; - - struct Entry { - std::string name; - TimePoint time; - }; - - void add(std::string name) - { - std::lock_guard lock(mutex_); - entries_.push_back({std::move(name), std::chrono::steady_clock::now()}); - } - - std::vector names() const - { - std::lock_guard lock(mutex_); - std::vector result; - result.reserve(entries_.size()); - for (const auto& entry : entries_) { - result.push_back(entry.name); - } - return result; - } - - std::optional firstTime(const std::string& name) const - { - std::lock_guard lock(mutex_); - const auto found = std::find_if( - entries_.begin(), entries_.end(), - [&name](const Entry& entry) { return entry.name == name; }); - return found == entries_.end() - ? std::nullopt - : std::optional(found->time); - } - -private: - mutable std::mutex mutex_; - std::vector entries_; -}; - -class ActionTestArm final : public device::RobotArm { -public: - ActionTestArm(std::string id, std::shared_ptr trace) - : trace_(std::move(trace)) - { - id_ = std::move(id); - } - - std::string typeName() const override { return "ActionTestArm"; } - bool stop() override - { - lifecycle_stop_calls_.fetch_add(1, std::memory_order_relaxed); - return true; - } - device::DeviceHealthSnapshot healthSnapshot() override - { - std::unique_lock lock(health_mutex_); - ++health_snapshot_calls_; - health_snapshot_entered_ = true; - health_condition_.notify_all(); - health_condition_.wait( - lock, [this] { return !block_health_snapshot_; }); - return {device::DeviceHealthState::Healthy, {}}; - } - bool supportsActionQueueMotion() const noexcept override { return true; } - device::RobotModel getRobotModel() const override - { - device::RobotModel model; - model.name = "ActionTestArm"; - model.dof = kDof; - model.joint_names.assign(kDof, "joint"); - return model; - } - std::size_t getDof() const override { return kDof; } - device::ArmState getRobotState() const override { return {}; } - device::JointGroupState getJointState() const override { return {}; } - device::CartesianPose getTcpPose( - device::FrameType = device::FrameType::Base) const override - { - return {}; - } - device::RobotMode getRobotMode() const override - { - return device::RobotMode::Idle; - } - device::SafetyMode getSafetyMode() const override - { - return device::SafetyMode::Normal; - } - device::ControlMode getControlMode() const override - { - return device::ControlMode::Position; - } - - device::Result torqueOn() override { return device::Result::success(); } - device::Result torqueOff() override { return device::Result::success(); } - device::Result calibrateZeroQ(const std::string&) override - { - return device::Result::success(); - } - device::Result emergencyStop() override - { - return device::Result::success(); - } - device::Result protectiveStop() override - { - return device::Result::success(); - } - device::Result setSpeedScaling(double) override - { - return device::Result::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; } - - device::Result moveJ(const device::JointPositionCommand&, - const device::MotionOptions& options) override - { - return performMotion("arm:J", options); - } - device::Result speedJ(const device::JointVelocityCommand&, - double, - double) override - { - return device::Result::success(); - } - device::Result stopJ(double) override { return stopMotion(); } - device::Result moveL( - const device::CartesianPose& target, - const device::MotionOptions& options, - device::FrameType = device::FrameType::Base) override - { - return performMotion( - "arm:L:" + std::to_string(static_cast(target.x)), - options); - } - device::Result speedL( - const device::CartesianVelocity&, - double, - double, - device::FrameType = device::FrameType::Base) override - { - return device::Result::success(); - } - device::Result stopL(std::optional = std::nullopt) override - { - return stopMotion(); - } - device::Result stopMotion() override - { - bool fail = false; - { - std::unique_lock lock(mutex_); - ++stop_motion_calls_; - stop_requested_ = true; - fail = stop_motion_fails_; - motion_condition_.notify_all(); - if (block_next_stop_motion_) { - block_next_stop_motion_ = false; - stop_motion_blocked_ = true; - stop_motion_blocked_condition_.notify_all(); - stop_motion_release_condition_.wait( - lock, - [this]() { return release_blocked_stop_motion_; }); - } - } - trace_->add("arm:stop:" + id_); - motion_condition_.notify_all(); - return fail - ? device::Result::failure( - device::ArmErrorCode::CommandFailed, - "injected RobotArm stop failure") - : device::Result::success(); - } - - device::Result startServoMode(const device::ServoOptions&) override - { - return device::Result::success(); - } - device::Result servoJ(const device::JointPositionCommand&) override - { - return device::Result::success(); - } - device::Result servoL( - const device::CartesianPose&, - device::FrameType = device::FrameType::Base) override - { - return device::Result::success(); - } - device::Result servoSpeedJ( - const device::JointVelocityCommand&) override - { - return device::Result::success(); - } - device::Result servoSpeedL( - const device::CartesianVelocity&, - device::FrameType = device::FrameType::Base) override - { - return device::Result::success(); - } - device::Result stopServoMode() override - { - return device::Result::success(); - } - - device::Result connect(const std::string&, int) override - { - return device::Result::success(); - } - device::Result disconnect() override { return device::Result::success(); } - bool isConnected() const override { return true; } - device::Result powerOn() override { return device::Result::success(); } - device::Result powerOff() override { return device::Result::success(); } - device::Result brakeRelease() override { return device::Result::success(); } - device::Result shutdown() override - { - shutdown_calls_.fetch_add(1, std::memory_order_relaxed); - return device::Result::success(); - } - device::Result clearFault() override { return device::Result::success(); } - device::Result unlockProtectiveStop() override - { - return device::Result::success(); - } - device::Result loadProgram(const std::string&) override - { - return device::Result::success(); - } - device::Result playProgram() override { return device::Result::success(); } - device::Result pauseProgram() override { return device::Result::success(); } - device::Result stopProgram() override { return device::Result::success(); } - std::vector ik(const std::string&, - const std::string&, - const device::CartesianPose&) override - { - return {}; - } - std::shared_ptr kinematicsSolver() const override - { - return nullptr; - } - device::CartesianPose fk(const std::string&, - const std::string&) override - { - return {}; - } - device::CartesianPose fk(bool = true) override { return {}; } - device::CartesianVelocity getSpeedLCommandTwistBase() const override - { - return {}; - } - bool busy() const override - { - std::lock_guard lock(mutex_); - if (busy_throws_after_stop_ && stop_motion_calls_ != 0) { - throw std::runtime_error( - "injected RobotArm busy-state failure"); - } - return active_motions_ != 0; - } - - void blockNextMotion() - { - std::lock_guard lock(mutex_); - block_next_motion_ = true; - release_blocked_motion_ = false; - stop_requested_ = false; - } - - void blockCanceledMotionReturn() - { - std::lock_guard lock(mutex_); - block_canceled_motion_return_ = true; - canceled_motion_return_blocked_ = false; - release_canceled_motion_return_ = false; - } - - bool waitForCanceledMotionReturn( - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return canceled_motion_return_condition_.wait_for( - lock, - timeout, - [this]() { return canceled_motion_return_blocked_; }); - } - - void releaseCanceledMotionReturn() - { - { - std::lock_guard lock(mutex_); - release_canceled_motion_return_ = true; - } - motion_condition_.notify_all(); - } - - void releaseBlockedMotion() - { - { - std::lock_guard lock(mutex_); - release_blocked_motion_ = true; - } - motion_condition_.notify_all(); - } - - void failOnMotionCall(const int call_index) - { - std::lock_guard lock(mutex_); - fail_on_motion_call_ = call_index; - } - - void failStopAndBusyConfirmation() - { - std::lock_guard lock(mutex_); - stop_motion_fails_ = true; - busy_throws_after_stop_ = true; - } - - void blockNextStopMotion() - { - std::lock_guard lock(mutex_); - block_next_stop_motion_ = true; - stop_motion_blocked_ = false; - release_blocked_stop_motion_ = false; - } - - bool waitForBlockedStopMotion( - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return stop_motion_blocked_condition_.wait_for( - lock, timeout, [this]() { return stop_motion_blocked_; }); - } - - void releaseBlockedStopMotion() - { - { - std::lock_guard lock(mutex_); - release_blocked_stop_motion_ = true; - } - stop_motion_release_condition_.notify_all(); - } - - bool waitForMotionCalls( - const int expected, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return motion_started_condition_.wait_for( - lock, timeout, - [this, expected]() { return motion_calls_ >= expected; }); - } - - int motionCalls() const - { - std::lock_guard lock(mutex_); - return motion_calls_; - } - - int stopMotionCalls() const - { - std::lock_guard lock(mutex_); - return stop_motion_calls_; - } - - int lifecycleStopCalls() const - { - return lifecycle_stop_calls_.load(std::memory_order_relaxed); - } - - int shutdownCalls() const - { - return shutdown_calls_.load(std::memory_order_relaxed); - } - - void blockHealthSnapshot() - { - std::lock_guard lock(health_mutex_); - block_health_snapshot_ = true; - health_snapshot_entered_ = false; - } - - bool waitForHealthSnapshot( - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(health_mutex_); - return health_condition_.wait_for( - lock, timeout, [this] { return health_snapshot_entered_; }); - } - - void releaseHealthSnapshot() - { - { - std::lock_guard lock(health_mutex_); - block_health_snapshot_ = false; - } - health_condition_.notify_all(); - } - - int healthSnapshotCalls() const - { - std::lock_guard lock(health_mutex_); - return health_snapshot_calls_; - } - - bool waitForStopMotionCalls( - const int expected, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return motion_condition_.wait_for( - lock, timeout, - [this, expected]() { return stop_motion_calls_ >= expected; }); - } - - int maxActiveMotions() const - { - std::lock_guard lock(mutex_); - return max_active_motions_; - } - -private: - device::Result performMotion( - const std::string& event, - const device::MotionOptions& options) - { - bool block = false; - bool fail = false; - { - std::lock_guard lock(mutex_); - ++motion_calls_; - ++active_motions_; - max_active_motions_ = std::max( - max_active_motions_, active_motions_); - block = block_next_motion_; - block_next_motion_ = false; - fail = motion_calls_ == fail_on_motion_call_; - } - trace_->add(event); - motion_started_condition_.notify_all(); - - bool canceled = false; - bool safety_timeout = false; - if (block) { - const auto safety_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - std::unique_lock lock(mutex_); - while (!release_blocked_motion_ && !stop_requested_ && - !(options.cancellation_requested && - options.cancellation_requested())) { - if (std::chrono::steady_clock::now() >= safety_deadline) { - safety_timeout = true; - break; - } - motion_condition_.wait_for(lock, std::chrono::milliseconds(1)); - } - canceled = stop_requested_ || - (options.cancellation_requested && - options.cancellation_requested()); - } - - { - std::unique_lock lock(mutex_); - --active_motions_; - if (canceled && block_canceled_motion_return_) { - block_canceled_motion_return_ = false; - canceled_motion_return_blocked_ = true; - canceled_motion_return_condition_.notify_all(); - motion_condition_.wait( - lock, - [this]() { return release_canceled_motion_return_; }); - } - } - if (safety_timeout) { - return device::Result::failure( - device::ArmErrorCode::Timeout, - "test arm safety wait expired"); - } - if (canceled) { - return device::Result::failure( - device::ArmErrorCode::CommandRejected, - "test arm motion canceled"); - } - if (fail) { - return device::Result::failure( - device::ArmErrorCode::CommandFailed, - "injected RobotArm failure"); - } - return device::Result::success(); - } - - static constexpr std::size_t kDof = 6U; - std::shared_ptr trace_; - mutable std::mutex mutex_; - std::condition_variable motion_condition_; - std::condition_variable motion_started_condition_; - std::condition_variable canceled_motion_return_condition_; - std::condition_variable stop_motion_blocked_condition_; - std::condition_variable stop_motion_release_condition_; - mutable std::mutex health_mutex_; - std::condition_variable health_condition_; - std::atomic lifecycle_stop_calls_{0}; - std::atomic shutdown_calls_{0}; - int motion_calls_{0}; - int stop_motion_calls_{0}; - int active_motions_{0}; - int max_active_motions_{0}; - int fail_on_motion_call_{-1}; - bool block_next_motion_{false}; - bool release_blocked_motion_{false}; - bool stop_requested_{false}; - bool block_canceled_motion_return_{false}; - bool canceled_motion_return_blocked_{false}; - bool release_canceled_motion_return_{false}; - bool stop_motion_fails_{false}; - bool busy_throws_after_stop_{false}; - bool block_next_stop_motion_{false}; - bool stop_motion_blocked_{false}; - bool release_blocked_stop_motion_{false}; - int health_snapshot_calls_{0}; - bool block_health_snapshot_{false}; - bool health_snapshot_entered_{false}; -}; - -class ActionTestAgv final : public device::AbstractAGV { -public: - ActionTestAgv(std::string id, std::shared_ptr trace) - : trace_(std::move(trace)) - { - id_ = std::move(id); - } - - std::string typeName() const override { return "ActionTestAgv"; } - bool supportsSynchronousAction(device::AgvActionKind) const noexcept override - { - return supports_synchronous_.load(std::memory_order_acquire); - } - device::AgvRuntimeState runtimeState() const override { return {}; } - device::AgvNavigationStatus navigationStatus() const override { return {}; } - device::AgvResult navigateToPose( - const math::Pose2d& pose, - const device::AgvMotionOptions&, - const device::AgvAdapterParams&) override - { - { - std::lock_guard lock(parameters_mutex_); - last_pose_ = pose; - } - return record("agv:pose"); - } - device::AgvResult navigateToStation( - const std::string& station_id, - const device::AgvMotionOptions&, - const device::AgvAdapterParams&) override - { - return record("agv:station:" + station_id); - } - device::AgvResult followPath( - const std::vector& path, - const device::AgvMotionOptions&) override - { - { - std::lock_guard lock(parameters_mutex_); - last_path_ = path; - } - return record("agv:path"); - } - device::AgvResult cancelNavigation() override - { - cancel_calls_.fetch_add(1, std::memory_order_relaxed); - trace_->add("agv:cancel"); - return device::AgvResult::success(); - } - device::AgvResult confirmMotionStopped() override - { - return stopped_confirmed_.load(std::memory_order_acquire) - ? device::AgvResult::success() - : device::AgvResult::failure( - device::AgvErrorCode::Timeout, - "test AGV stopped state is unconfirmed"); - } - void setSupportsSynchronous(const bool value) - { - supports_synchronous_.store(value, std::memory_order_release); - } - - void setNavigationFailure(const bool value) - { - fail_navigation_.store(value, std::memory_order_release); - } - - void setStoppedConfirmed(const bool value) - { - stopped_confirmed_.store(value, std::memory_order_release); - } - - int navigationCalls() const - { - return navigation_calls_.load(std::memory_order_relaxed); - } - - math::Pose2d lastPose() const - { - std::lock_guard lock(parameters_mutex_); - return last_pose_; - } - - std::vector lastPath() const - { - std::lock_guard lock(parameters_mutex_); - return last_path_; - } - -private: - device::AgvResult record(std::string event) - { - navigation_calls_.fetch_add(1, std::memory_order_relaxed); - trace_->add(std::move(event)); - if (fail_navigation_.load(std::memory_order_acquire)) { - return device::AgvResult::failure( - device::AgvErrorCode::TaskFailed, - "injected AGV navigation failure"); - } - return device::AgvResult::success(); - } - - std::shared_ptr trace_; - mutable std::mutex parameters_mutex_; - math::Pose2d last_pose_{}; - std::vector last_path_; - std::atomic supports_synchronous_{true}; - std::atomic fail_navigation_{false}; - std::atomic stopped_confirmed_{true}; - std::atomic navigation_calls_{0}; - std::atomic cancel_calls_{0}; -}; - -api::ActionStep* addMoveLStep( - api::ActionQueueCommand_Request& request, - const std::string& step_id, - const std::string& device_id, - const double marker, - const std::uint32_t timeout_ms = 0U, - const bool asynchronous = false) -{ - auto* step = request.add_steps(); - step->set_step_id(step_id); - step->set_timeout_ms(timeout_ms); - auto* command = step->mutable_arm_move_l(); - command->mutable_header()->set_device_id(device_id); - command->mutable_target()->set_x(marker); - command->set_frame(api::ARM_FRAME_BASE); - command->mutable_options()->set_asynchronous(asynchronous); - return step; -} - -api::ActionStep* addMoveJStep( - api::ActionQueueCommand_Request& request, - const std::string& step_id, - const std::string& device_id, - const bool asynchronous = false) -{ - auto* step = request.add_steps(); - step->set_step_id(step_id); - auto* command = step->mutable_arm_move_j(); - command->mutable_header()->set_device_id(device_id); - for (std::size_t index = 0; index < 6U; ++index) { - command->mutable_target()->add_position( - static_cast(index) * 0.1); - } - command->mutable_options()->set_asynchronous(asynchronous); - return step; -} - -api::ActionStep* addAgvStationStep( - api::ActionQueueCommand_Request& request, - const std::string& step_id, - const std::string& device_id, - const std::string& station_id, - const bool asynchronous = false) -{ - auto* step = request.add_steps(); - step->set_step_id(step_id); - auto* command = step->mutable_agv_navigate_to_station(); - command->mutable_header()->set_device_id(device_id); - command->set_station_id(station_id); - command->mutable_options()->set_asynchronous(asynchronous); - return step; -} - -api::ActionStep* addAgvPoseStep( - api::ActionQueueCommand_Request& request, - const std::string& step_id, - const std::string& device_id, - const double x, - const double y, - const double theta) -{ - auto* step = request.add_steps(); - step->set_step_id(step_id); - auto* command = step->mutable_agv_navigate_to_pose(); - command->mutable_header()->set_device_id(device_id); - command->mutable_pose()->set_x(x); - command->mutable_pose()->set_y(y); - command->mutable_pose()->set_theta(theta); - return step; -} - -api::ActionStep* addAgvPathStep( - api::ActionQueueCommand_Request& request, - const std::string& step_id, - const std::string& device_id) -{ - auto* step = request.add_steps(); - step->set_step_id(step_id); - auto* command = step->mutable_agv_follow_path(); - command->mutable_header()->set_device_id(device_id); - auto* first = command->add_path(); - first->set_source_station("start"); - first->set_target_station("middle"); - auto* second = command->add_path(); - second->set_source_station("middle"); - second->set_target_station("finish"); - return step; -} - -api::ActionStep* addDelayStep( - api::ActionQueueCommand_Request& request, - const std::string& step_id, - const std::uint32_t duration_ms) -{ - auto* step = request.add_steps(); - step->set_step_id(step_id); - step->mutable_delay()->set_duration_ms(duration_ms); - return step; -} - -std::uint64_t currentUnixTimeMs() -{ - const auto elapsed = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - return static_cast(elapsed.count()); -} - -const api::SystemDeviceInfo* findDevice( - const api::GetDeviceListCommand_Feedback& response, - const std::string& id) -{ - for (const auto& device : response.device_list()) { - if (device.device_id() == id) { - return &device; - } - } - return nullptr; -} - -const api::SafetyOperationTargetResult* findSafetyTarget( - const api::StopAllCommand_Feedback& response, - const std::string& id) -{ - for (const auto& target : response.targets()) { - if (target.target_id() == id) { - return ⌖ - } - } - return nullptr; -} - -class GrpcSystemServiceTest : public ::testing::Test { -protected: - void SetUp() override - { - task::TaskManager::destroyInstance(); - blocking_stop_task.reset(); - (void)media::globalMediaSourceManager().stopAllSources(); - control::ControlAuthorityManager::instance().clear(); - globalStopAllAdmissionGate().clearForTesting(); - globalCameraOperationalActivityRegistry().clearForTesting(); - globalCameraPtzActivityRegistry().clearForTesting(); - globalMediaActivityCoordinator().clearForTesting(); - globalMotorActivityCoordinator().clearForTesting(); - device::DeviceManager::destroyInstance(); - } - - void TearDown() override - { - if (blocking_stop_task) { - blocking_stop_task->releaseStopActivity(); - } - service_.reset(); - task::TaskManager::destroyInstance(); - blocking_stop_task.reset(); - action_agv_.reset(); - action_arm_.reset(); - action_trace_.reset(); - owned_devices_.clear(); - device::DeviceManager::destroyInstance(); - control::ControlAuthorityManager::instance().clear(); - globalStopAllAdmissionGate().clearForTesting(); - globalCameraOperationalActivityRegistry().clearForTesting(); - globalCameraPtzActivityRegistry().clearForTesting(); - globalMediaActivityCoordinator().clearForTesting(); - globalMotorActivityCoordinator().clearForTesting(); - (void)media::globalMediaSourceManager().stopAllSources(); - } - - api::GetDeviceListCommand_Feedback getDeviceList() - { - api::GetDeviceListCommand_Request request; - api::GetDeviceListCommand_Feedback response; - grpc::ServerContext context; - - const auto status = service_->GetDeviceList( - &context, &request, &response); - EXPECT_TRUE(status.ok()) << status.error_message(); - return response; - } - - void registerDevice(device::DeviceManager& manager, - std::shared_ptr device) - { - manager.registerDevice(device); - owned_devices_.push_back(std::move(device)); - } - - void initializeActionDevices() - { - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - action_trace_ = std::make_shared(); - action_arm_ = std::make_shared( - "action-arm", action_trace_); - action_agv_ = std::make_shared( - "action-agv", action_trace_); - manager.registerDevice(action_arm_); - manager.registerDevice(action_agv_); - service_ = std::make_unique(); - api::GetSystemInfoCommand_Request info_request; - api::GetSystemInfoCommand_Feedback info_response; - grpc::ServerContext info_context; - const auto info_status = service_->GetSystemInfo( - &info_context, &info_request, &info_response); - ASSERT_TRUE(info_status.ok()) << info_status.error_message(); - service_instance_id_ = info_response.action_service_instance_id(); - ASSERT_FALSE(service_instance_id_.empty()); - } - - api::ActionQueueCommand_Feedback executeAction( - const api::ActionQueueCommand_Request& request) - { - auto bound_request = request; - if (bound_request.expected_service_instance_id().empty()) { - bound_request.set_expected_service_instance_id( - service_instance_id_); - } - api::ActionQueueCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->ExecuteActionQueue( - &context, &bound_request, &response); - EXPECT_TRUE(status.ok()) << status.error_message(); - return response; - } - - std::unique_ptr service_; - std::vector> owned_devices_; - std::shared_ptr action_trace_; - std::shared_ptr action_arm_; - std::shared_ptr action_agv_; - std::string service_instance_id_; -}; - -TEST_F(GrpcSystemServiceTest, - ReturnsOnlyEnabledDevicesAndPreservesEnabledErrors) -{ - config::DeviceManagerConfig config; - config.set_name("system-service-test"); - config.set_version("9.2"); - config.set_description("GetDeviceList snapshot test"); - - auto* disabled = config.add_devices(); - disabled->set_id("b_disabled_camera"); - disabled->set_type(config::DeviceConfigEntry::DEVICE_TYPE_CAMERA); - disabled->set_enable(false); - - // An enabled unsupported entry remains in the manager snapshot as ERROR. - // GetDeviceList must expose it rather than filtering by lifecycle state. - auto* enabled_error = config.add_devices(); - enabled_error->set_id("c_enabled_error"); - enabled_error->set_type(config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN); - enabled_error->set_enable(true); - - auto& manager = device::DeviceManager::getInstance(config); - registerDevice( - manager, - std::make_shared( - "z_camera", - device::DeviceKind::Camera, - "VendorCamera", - device::DeviceHealthSnapshot{ - device::DeviceHealthState::Healthy, {}})); - registerDevice( - manager, - std::make_shared( - "a_unknown", - device::DeviceKind::Unknown, - "MysteryDriver", - device::DeviceHealthSnapshot{ - device::DeviceHealthState::Degraded, - "device health is degraded"})); - service_ = std::make_unique(); - - const auto response = getDeviceList(); - - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_TRUE(response.header().has_timestamp()); - EXPECT_GT(response.header().timestamp().seconds(), 0); - EXPECT_EQ(response.manager_name(), "system-service-test"); - EXPECT_EQ(response.manager_version(), "9.2"); - EXPECT_EQ(response.manager_description(), "GetDeviceList snapshot test"); - ASSERT_EQ(response.device_list_size(), 3); - - // DeviceManager::snapshot() supplies stable device-id ordering, and the - // service must preserve it while filtering disabled entries. - EXPECT_EQ(response.device_list(0).device_id(), "a_unknown"); - EXPECT_EQ(response.device_list(1).device_id(), "c_enabled_error"); - EXPECT_EQ(response.device_list(2).device_id(), "z_camera"); - EXPECT_EQ(findDevice(response, "b_disabled_camera"), nullptr); - - const auto* unknown = findDevice(response, "a_unknown"); - ASSERT_NE(unknown, nullptr); - EXPECT_TRUE(unknown->enabled()); - EXPECT_EQ(unknown->type_name(), "MysteryDriver"); - EXPECT_EQ(unknown->device_type(), - api::SYSTEM_DEVICE_TYPE_UNSPECIFIED); - EXPECT_NE(unknown->device_type(), api::SYSTEM_DEVICE_TYPE_AGV); - EXPECT_EQ(unknown->manager_state(), - api::SYSTEM_DEVICE_STATE_REGISTERED); - EXPECT_EQ(unknown->health(), api::SYSTEM_DEVICE_HEALTH_DEGRADED); - EXPECT_TRUE(unknown->has_error()); - EXPECT_EQ(unknown->error_message(), "device health is degraded"); - EXPECT_GT(unknown->status_updated_at_unix_ms(), 0U); - - const auto* error = findDevice(response, "c_enabled_error"); - ASSERT_NE(error, nullptr); - EXPECT_TRUE(error->enabled()); - EXPECT_EQ(error->type_name(), "Unknown"); - EXPECT_EQ(error->device_type(), api::SYSTEM_DEVICE_TYPE_UNSPECIFIED); - EXPECT_EQ(error->manager_state(), api::SYSTEM_DEVICE_STATE_ERROR); - EXPECT_EQ(error->health(), api::SYSTEM_DEVICE_HEALTH_UNSPECIFIED); - EXPECT_TRUE(error->has_error()); - EXPECT_FALSE(error->error_message().empty()); - EXPECT_GT(error->status_updated_at_unix_ms(), 0U); - - const auto* camera = findDevice(response, "z_camera"); - ASSERT_NE(camera, nullptr); - EXPECT_TRUE(camera->enabled()); - EXPECT_EQ(camera->type_name(), "VendorCamera"); - EXPECT_EQ(camera->device_type(), api::SYSTEM_DEVICE_TYPE_CAMERA); - EXPECT_EQ(camera->manager_state(), - api::SYSTEM_DEVICE_STATE_REGISTERED); - EXPECT_EQ(camera->health(), api::SYSTEM_DEVICE_HEALTH_HEALTHY); - EXPECT_FALSE(camera->has_error()); - EXPECT_TRUE(camera->error_message().empty()); - EXPECT_GT(camera->status_updated_at_unix_ms(), 0U); -} - -TEST_F(GrpcSystemServiceTest, EmptyListReturnsMetadataAndTimestamps) -{ - config::DeviceManagerConfig config; - config.set_name("empty-manager"); - config.set_version("1.2.3"); - config.set_description("manager without devices"); - device::DeviceManager::getInstance(config); - service_ = std::make_unique(); - - const auto before_ms = currentUnixTimeMs(); - const auto response = getDeviceList(); - const auto after_ms = currentUnixTimeMs(); - - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_TRUE(response.header().has_timestamp()); - EXPECT_GT(response.header().timestamp().seconds(), 0); - EXPECT_EQ(response.device_list_size(), 0); - EXPECT_EQ(response.manager_name(), "empty-manager"); - EXPECT_EQ(response.manager_version(), "1.2.3"); - EXPECT_EQ(response.manager_description(), "manager without devices"); - EXPECT_GE(response.sampled_at_unix_ms(), before_ms); - EXPECT_LE(response.sampled_at_unix_ms(), after_ms); -} - -TEST_F(GrpcSystemServiceTest, SafetyStateUsesCoordinatorMemorySnapshot) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - registerDevice( - manager, - std::make_shared( - "safety-camera", - device::DeviceKind::Camera, - "SafetyCamera", - device::DeviceHealthSnapshot{ - device::DeviceHealthState::Healthy, {}})); - service_ = std::make_unique(); - - api::GetSafetyStateCommand_Request request; - api::GetSafetyStateCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->GetSafetyState( - &context, &request, &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_FALSE(response.control_service_instance_id().empty()); - EXPECT_EQ( - response.header().service_instance_id(), - response.control_service_instance_id()); - EXPECT_GT(response.safety_epoch(), 0U); - ASSERT_EQ(response.devices_size(), 1); - EXPECT_EQ(response.devices(0).device_id(), "safety-camera"); - EXPECT_EQ(response.devices(0).policy_family(), "Sensor"); - ASSERT_GT(response.participants_size(), 0); - EXPECT_FALSE(response.participants(0).participant_id().empty()); - EXPECT_FALSE(response.participants(0).phase().empty()); - EXPECT_TRUE(response.participants(0).registered()); - - api::GetSystemInfoCommand_Request info_request; - api::GetSystemInfoCommand_Feedback info_response; - grpc::ServerContext info_context; - ASSERT_TRUE(service_->GetSystemInfo( - &info_context, &info_request, &info_response).ok()); - EXPECT_EQ( - info_response.control_service_instance_id(), - response.control_service_instance_id()); - EXPECT_EQ(info_response.safety_schema_version(), 1U); -} - -TEST_F(GrpcSystemServiceTest, RecoveryIsDisabledByDefault) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - service_ = std::make_unique(); - - api::RecoverSafetyStateCommand_Request request; - request.set_recovery_id("recovery-disabled"); - request.mutable_scope()->set_all_devices(true); - request.set_expected_safety_epoch( - manager.safetyManager().snapshot().safety_epoch); - request.set_mode(api::RecoverSafetyStateCommand::VERIFY_ONLY); - request.set_reason("diagnostic verification"); - api::RecoverSafetyStateCommand_Feedback response; - grpc::ServerContext context; - - const auto status = service_->RecoverSafetyState( - &context, &request, &response); - EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ(status.error_message(), "RECOVERY_RPC_DISABLED"); -} - -TEST_F(GrpcSystemServiceTest, RecoveryRequiresDurableAuditBeforeCoordinator) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - manager.registerDevice(std::make_shared( - "audit-camera", device::DeviceKind::Camera, "AuditCamera")); - auto audit = std::make_shared(); - service_ = std::make_unique( - std::chrono::seconds(1), makeAllowAllRecoveryGateway(), audit); - - api::RecoverSafetyStateCommand_Request request; - request.set_recovery_id("recovery-audited"); - request.mutable_scope()->set_all_devices(true); - request.set_expected_safety_epoch( - manager.safetyManager().snapshot().safety_epoch); - request.set_mode(api::RecoverSafetyStateCommand::VERIFY_ONLY); - request.set_reason("verify the local work cell"); - request.set_timeout_ms(500); - api::RecoverSafetyStateCommand_Feedback response; - grpc::ServerContext context; - - const auto status = service_->RecoverSafetyState( - &context, &request, &response); - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_EQ(audit->records.size(), 2U); - EXPECT_EQ(audit->records.front().stage, "accepted"); - EXPECT_EQ(audit->records.back().stage, "completed"); - EXPECT_EQ(response.recovery_id(), "recovery-audited"); - - const auto epoch_before_failure = - manager.safetyManager().snapshot().safety_epoch; - audit->fail = true; - request.set_recovery_id("recovery-audit-fails"); - request.set_expected_safety_epoch(epoch_before_failure); - response.Clear(); - grpc::ServerContext failed_context; - const auto failed = service_->RecoverSafetyState( - &failed_context, &request, &response); - EXPECT_EQ(failed.error_code(), grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_EQ( - response.header().reason_code(), - api::COMMAND_REASON_CODE_RECOVERY_AUDIT_FAILED); - EXPECT_EQ( - manager.safetyManager().snapshot().safety_epoch, - epoch_before_failure); -} - -TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind) -{ - struct ExpectedMapping { - const char* id; - device::DeviceKind source; - api::SystemDeviceType destination; - }; - constexpr std::array mappings{{ - {"01_agv", device::DeviceKind::AGV, - api::SYSTEM_DEVICE_TYPE_AGV}, - {"02_arm", device::DeviceKind::Arm, - api::SYSTEM_DEVICE_TYPE_ARM}, - {"03_battery", device::DeviceKind::Battery, - api::SYSTEM_DEVICE_TYPE_BATTERY}, - {"04_bio_head", device::DeviceKind::BioHead, - api::SYSTEM_DEVICE_TYPE_BIO_HEAD}, - {"05_camera", device::DeviceKind::Camera, - api::SYSTEM_DEVICE_TYPE_CAMERA}, - {"06_can_bus", device::DeviceKind::CanBus, - api::SYSTEM_DEVICE_TYPE_CAN_BUS}, - {"07_dex_hand", device::DeviceKind::DexHand, - api::SYSTEM_DEVICE_TYPE_DEX_HAND}, - {"08_gripper", device::DeviceKind::Gripper, - api::SYSTEM_DEVICE_TYPE_GRIPPER}, - {"09_microphone", device::DeviceKind::Microphone, - api::SYSTEM_DEVICE_TYPE_MICROPHONE}, - {"10_motor", device::DeviceKind::Motor, - api::SYSTEM_DEVICE_TYPE_MOTOR}, - {"11_motor_system", device::DeviceKind::MotorSystem, - api::SYSTEM_DEVICE_TYPE_MOTOR_SYSTEM}, - {"12_mujoco_viewer", device::DeviceKind::MujocoViewer, - api::SYSTEM_DEVICE_TYPE_MUJOCO_VIEWER}, - {"13_mujoco_world", device::DeviceKind::MujocoWorld, - api::SYSTEM_DEVICE_TYPE_MUJOCO_WORLD}, - {"14_robot", device::DeviceKind::Robot, - api::SYSTEM_DEVICE_TYPE_ROBOT}, - {"15_speaker", device::DeviceKind::Speaker, - api::SYSTEM_DEVICE_TYPE_SPEAKER}, - }}; - - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - for (const auto& mapping : mappings) { - registerDevice( - manager, - std::make_shared( - mapping.id, mapping.source, "MappedBackend")); - } - service_ = std::make_unique(); - - const auto response = getDeviceList(); - - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - ASSERT_EQ(response.device_list_size(), - static_cast(mappings.size())); - for (std::size_t index = 0; index < mappings.size(); ++index) { - SCOPED_TRACE(mappings[index].id); - const auto& actual = response.device_list( - static_cast(index)); - EXPECT_EQ(actual.device_id(), mappings[index].id); - EXPECT_EQ(actual.device_type(), mappings[index].destination); - EXPECT_NE(actual.device_type(), - api::SYSTEM_DEVICE_TYPE_UNSPECIFIED); - } -} - -TEST_F(GrpcSystemServiceTest, ActionQueueExecutesFourMoveLStepsSerially) -{ - initializeActionDevices(); - api::ActionQueueCommand_Request request; - request.set_action_id("four-movel"); - for (int marker = 1; marker <= 4; ++marker) { - addMoveLStep( - request, - "move-" + std::to_string(marker), - action_arm_->id(), - static_cast(marker)); - } - - const auto response = executeAction(request); - - EXPECT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(response.completed_steps(), 4U); - EXPECT_FALSE(response.has_failed_step_index()); - EXPECT_EQ(action_arm_->motionCalls(), 4); - EXPECT_EQ(action_arm_->maxActiveMotions(), 1); - EXPECT_EQ( - action_trace_->names(), - (std::vector{ - "arm:L:1", "arm:L:2", "arm:L:3", "arm:L:4"})); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueRejectsStaleDeviceGenerationBeforeStepDispatch) -{ - config::DeviceManagerConfig config; - config.mutable_safety()->set_mode( - config::SafetyManagerConfig::ENFORCE_ALL); - auto& manager = device::DeviceManager::getInstance(config); - action_trace_ = std::make_shared(); - action_arm_ = std::make_shared( - "action-arm", action_trace_); - manager.registerDevice(action_arm_); - manager.safetyManager().markStartupComplete(); - std::optional current_device_generation; - const auto snapshot_deadline = - std::chrono::steady_clock::now() + std::chrono::seconds(1); - while (std::chrono::steady_clock::now() < snapshot_deadline) { - const auto safety = manager.safetyManager().snapshot(); - ASSERT_EQ( - safety.system_state, safety::SystemAdmissionState::Open); - const auto safety_device = std::find_if( - safety.devices.begin(), safety.devices.end(), - [this](const auto& device) { - return device.descriptor.device_id == action_arm_->id(); - }); - if (safety_device != safety.devices.end() && - safety_device->safety.fresh) { - current_device_generation = - safety_device->safety.snapshot.device_generation; - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - } - ASSERT_TRUE(current_device_generation.has_value()); - service_ = std::make_unique(); - - api::GetSystemInfoCommand_Request info_request; - api::GetSystemInfoCommand_Feedback info_response; - grpc::ServerContext info_context; - ASSERT_TRUE(service_->GetSystemInfo( - &info_context, &info_request, &info_response).ok()); - - api::ActionQueueCommand_Request request; - request.set_action_id("stale-step-generation"); - request.set_expected_service_instance_id( - info_response.action_service_instance_id()); - auto* step = addMoveLStep( - request, "move", action_arm_->id(), 1.0); - step->mutable_arm_move_l() - ->mutable_header() - ->set_expected_device_generation( - *current_device_generation + 1U); - - api::ActionQueueCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->ExecuteActionQueue( - &context, &request, &response); - - EXPECT_TRUE(status.ok()) << status.error_message(); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_EQ(response.completed_steps(), 0U); - ASSERT_TRUE(response.has_failed_step_index()); - EXPECT_EQ(response.failed_step_index(), 0U); - EXPECT_NE( - response.header().error_message().find("generation"), - std::string::npos) - << response.header().error_message(); - EXPECT_EQ(action_arm_->motionCalls(), 0); - EXPECT_TRUE(action_trace_->names().empty()); -} - -TEST_F(GrpcSystemServiceTest, ActionQueuePreservesArmDelayAgvOrder) -{ - initializeActionDevices(); - api::ActionQueueCommand_Request request; - request.set_action_id("arm-delay-agv"); - addMoveJStep(request, "arm", action_arm_->id()); - addDelayStep(request, "settle", 25U); - addAgvStationStep( - request, "agv", action_agv_->id(), "dock"); - - const auto response = executeAction(request); - const auto arm_time = action_trace_->firstTime("arm:J"); - const auto agv_time = action_trace_->firstTime("agv:station:dock"); - - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(response.completed_steps(), 3U); - EXPECT_EQ( - action_trace_->names(), - (std::vector{"arm:J", "agv:station:dock"})); - ASSERT_TRUE(arm_time.has_value()); - ASSERT_TRUE(agv_time.has_value()); - EXPECT_GE( - std::chrono::duration_cast( - *agv_time - *arm_time), - std::chrono::milliseconds(15)); -} - -TEST_F(GrpcSystemServiceTest, ActionQueueExecutesAgvPoseAndPathSteps) -{ - initializeActionDevices(); - api::ActionQueueCommand_Request request; - request.set_action_id("agv-pose-and-path"); - addAgvPoseStep( - request, "pose", action_agv_->id(), 1.0, 2.0, 0.5); - addAgvPathStep(request, "path", action_agv_->id()); - - const auto response = executeAction(request); - - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(response.completed_steps(), 2U); - EXPECT_EQ(action_agv_->navigationCalls(), 2); - const auto pose = action_agv_->lastPose(); - EXPECT_DOUBLE_EQ(pose.x, 1.0); - EXPECT_DOUBLE_EQ(pose.y, 2.0); - EXPECT_DOUBLE_EQ(pose.theta, 0.5); - const auto path = action_agv_->lastPath(); - ASSERT_EQ(path.size(), 2U); - EXPECT_EQ(path[0].source_station, "start"); - EXPECT_EQ(path[0].target_station, "middle"); - EXPECT_EQ(path[1].source_station, "middle"); - EXPECT_EQ(path[1].target_station, "finish"); - EXPECT_EQ( - action_trace_->names(), - (std::vector{"agv:pose", "agv:path"})); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueuePrevalidatesAllStepsBeforeAnyDispatch) -{ - initializeActionDevices(); - api::ActionQueueCommand_Request request; - request.set_action_id("prevalidate-all"); - addMoveLStep(request, "valid-first", action_arm_->id(), 1.0); - addAgvStationStep( - request, "missing-second", "missing-agv", "dock"); - - const auto response = executeAction(request); - - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_EQ(response.completed_steps(), 0U); - ASSERT_TRUE(response.has_failed_step_index()); - EXPECT_EQ(response.failed_step_index(), 1U); - EXPECT_EQ(action_arm_->motionCalls(), 0); - EXPECT_EQ(action_agv_->navigationCalls(), 0); - EXPECT_TRUE(action_trace_->names().empty()); -} - -TEST_F(GrpcSystemServiceTest, ActionQueueRejectsAsynchronousMotion) -{ - initializeActionDevices(); - api::ActionQueueCommand_Request arm_request; - arm_request.set_action_id("async-arm"); - addMoveLStep( - arm_request, "arm", action_arm_->id(), 1.0, 0U, true); - const auto arm_response = executeAction(arm_request); - - api::ActionQueueCommand_Request agv_request; - agv_request.set_action_id("async-agv"); - addAgvStationStep( - agv_request, "agv", action_agv_->id(), "dock", true); - const auto agv_response = executeAction(agv_request); - - EXPECT_EQ(arm_response.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_EQ(agv_response.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_EQ(action_arm_->motionCalls(), 0); - EXPECT_EQ(action_agv_->navigationCalls(), 0); - EXPECT_TRUE(action_trace_->names().empty()); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueStopsAfterFailureAndReportsFailedStep) -{ - initializeActionDevices(); - action_arm_->failOnMotionCall(2); - api::ActionQueueCommand_Request request; - request.set_action_id("fail-fast"); - addMoveLStep(request, "first", action_arm_->id(), 1.0); - addMoveLStep(request, "fails", action_arm_->id(), 2.0); - addMoveLStep(request, "must-not-run", action_arm_->id(), 3.0); - - const auto response = executeAction(request); - - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_FAILED); - EXPECT_EQ(response.completed_steps(), 1U); - ASSERT_TRUE(response.has_failed_step_index()); - EXPECT_EQ(response.failed_step_index(), 1U); - EXPECT_EQ(action_arm_->motionCalls(), 2); - const auto trace = action_trace_->names(); - ASSERT_GE(trace.size(), 2U); - EXPECT_EQ(trace[0], "arm:L:1"); - EXPECT_EQ(trace[1], "arm:L:2"); - EXPECT_GE(action_arm_->stopMotionCalls(), 1); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueQuarantinesAgvAfterUnconfirmedFailureStop) -{ - initializeActionDevices(); - action_agv_->setNavigationFailure(true); - action_agv_->setStoppedConfirmed(false); - api::ActionQueueCommand_Request request; - request.set_action_id("agv-unconfirmed-stop"); - addAgvStationStep( - request, "fails", action_agv_->id(), "dock"); - - const auto response = executeAction(request); - - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_FAILED); - ASSERT_TRUE(response.has_failed_step_index()); - EXPECT_EQ(response.failed_step_index(), 0U); - EXPECT_NE( - response.header().error_message().find("remain quarantined"), - std::string::npos); - auto& authority = control::ControlAuthorityManager::instance(); - const auto lease = authority.tryAcquire( - action_agv_->id(), - "normal-control-after-unconfirmed-action-stop", - std::chrono::hours(1)); - EXPECT_FALSE(lease.acquired); - - const auto recovery = authority.preemptAcquire( - action_agv_->id(), - "confirmed-agv-recovery", - std::chrono::hours(1)); - ASSERT_TRUE(recovery.acquired) << recovery.detail; - ASSERT_TRUE(authority.waitForPreemptedRelease( - recovery.token, std::chrono::milliseconds::zero())); - ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token)); - authority.release(recovery.token); - - const auto recovered_lease = authority.tryAcquire( - action_agv_->id(), - "normal-control-after-confirmed-recovery", - std::chrono::hours(1)); - EXPECT_TRUE(recovered_lease.acquired) << recovered_lease.detail; - authority.release(recovered_lease.token); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueIsIdempotentAndRejectsConflictingPayload) -{ - initializeActionDevices(); - api::ActionQueueCommand_Request request; - request.set_action_id("idempotent-action"); - addMoveLStep(request, "only-step", action_arm_->id(), 1.0); - - const auto first = executeAction(request); - const auto retry = executeAction(request); - auto conflicting = request; - conflicting.mutable_steps(0) - ->mutable_arm_move_l()->mutable_target()->set_x(2.0); - const auto conflict = executeAction(conflicting); - - EXPECT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ( - first.deduplication_status(), - api::ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW); - EXPECT_EQ(retry.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ( - retry.deduplication_status(), - api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT); - EXPECT_EQ(retry.completed_steps(), 1U); - EXPECT_EQ(conflict.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_EQ( - conflict.deduplication_status(), - api::ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT); - EXPECT_EQ(action_arm_->motionCalls(), 1); - EXPECT_EQ( - action_trace_->names(), - (std::vector{"arm:L:1"})); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueRetryAfterTerminalCacheChurnDoesNotRedispatch) -{ - initializeActionDevices(); - api::ActionQueueCommand_Request original; - original.set_action_id("idempotent-after-cache-churn"); - addMoveLStep(original, "only-step", action_arm_->id(), 1.0); - - const auto first = executeAction(original); - ASSERT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED); - ASSERT_EQ(action_arm_->motionCalls(), 1); - - constexpr int kActionsBeyondTerminalCacheCapacity = 257; - for (int index = 0; index < kActionsBeyondTerminalCacheCapacity; ++index) { - SCOPED_TRACE(index); - api::ActionQueueCommand_Request filler; - filler.set_action_id("terminal-cache-filler-" + std::to_string(index)); - addDelayStep(filler, "delay", 0U); - - const auto response = executeAction(filler); - ASSERT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED) - << response.header().error_message(); - } - - const auto retry = executeAction(original); - - EXPECT_EQ(retry.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(retry.completed_steps(), 1U); - EXPECT_EQ(action_arm_->motionCalls(), 1); - - // submitAndWait() may wake before the worker completes terminal-cache - // rotation for the final filler. Advance one additional terminal action so - // the original ID is deterministically present in the retired-ID filter. - constexpr int kTotalActionsToRetireOriginal = 4353; - for (int index = kActionsBeyondTerminalCacheCapacity; - index < kTotalActionsToRetireOriginal; ++index) { - SCOPED_TRACE(index); - api::ActionQueueCommand_Request filler; - filler.set_action_id("terminal-cache-filler-" + std::to_string(index)); - addDelayStep(filler, "delay", 0U); - - const auto response = executeAction(filler); - ASSERT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED) - << response.header().error_message(); - } - - const auto retired_retry = executeAction(original); - - EXPECT_EQ(retired_retry.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_EQ( - retired_retry.deduplication_status(), - api::ACTION_DEDUPLICATION_STATUS_RESULT_EVICTED); - EXPECT_NE( - retired_retry.header().error_message().find("no longer cached"), - std::string::npos); - EXPECT_EQ(action_arm_->motionCalls(), 1); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueRejectsMissingOrStaleServiceInstance) -{ - initializeActionDevices(); - api::ActionQueueCommand_Request request; - request.set_action_id("service-instance-check"); - addMoveLStep(request, "move", action_arm_->id(), 1.0); - - api::ActionQueueCommand_Feedback missing_response; - grpc::ServerContext missing_context; - const auto missing_status = service_->ExecuteActionQueue( - &missing_context, &request, &missing_response); - - request.set_expected_service_instance_id("stale-instance"); - api::ActionQueueCommand_Feedback stale_response; - grpc::ServerContext stale_context; - const auto stale_status = service_->ExecuteActionQueue( - &stale_context, &request, &stale_response); - - EXPECT_TRUE(missing_status.ok()) << missing_status.error_message(); - EXPECT_EQ( - missing_response.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_EQ(missing_response.service_instance_id(), service_instance_id_); - EXPECT_TRUE(stale_status.ok()) << stale_status.error_message(); - EXPECT_EQ(stale_response.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_EQ( - stale_response.deduplication_status(), - api::ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH); - EXPECT_EQ(stale_response.service_instance_id(), service_instance_id_); - EXPECT_EQ(action_arm_->motionCalls(), 0); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueServiceRestartRejectsRetryBoundToPriorInstance) -{ - initializeActionDevices(); - api::ActionQueueCommand_Request request; - request.set_action_id("prior-instance-retry"); - request.set_expected_service_instance_id(service_instance_id_); - addMoveLStep(request, "move", action_arm_->id(), 1.0); - const std::string prior_instance = service_instance_id_; - - const auto first = executeAction(request); - ASSERT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED) - << first.header().error_message(); - ASSERT_EQ(action_arm_->motionCalls(), 1); - - service_.reset(); - service_ = std::make_unique(); - api::GetSystemInfoCommand_Request info_request; - api::GetSystemInfoCommand_Feedback info_response; - grpc::ServerContext info_context; - ASSERT_TRUE(service_->GetSystemInfo( - &info_context, &info_request, &info_response).ok()); - service_instance_id_ = info_response.action_service_instance_id(); - ASSERT_FALSE(service_instance_id_.empty()); - ASSERT_NE(service_instance_id_, prior_instance); - - api::ActionQueueCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->ExecuteActionQueue( - &context, &request, &response); - - EXPECT_TRUE(status.ok()) << status.error_message(); - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_EQ( - response.deduplication_status(), - api::ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH); - EXPECT_EQ(response.service_instance_id(), service_instance_id_); - EXPECT_EQ(action_arm_->motionCalls(), 1); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueLedgerCapacityRejectsOnlyNewIds) -{ - initializeActionDevices(); - ActionQueueExecutor executor( - device::DeviceManager::getInstance(), 2U); - const auto make_request = [&executor](const std::string& action_id) { - api::ActionQueueCommand_Request request; - request.set_action_id(action_id); - request.set_expected_service_instance_id(executor.instanceId()); - addDelayStep(request, "delay", 0U); - return request; - }; - const auto first_request = make_request("ledger-first"); - const auto second_request = make_request("ledger-second"); - const auto rejected_request = make_request("ledger-third"); - api::ActionQueueCommand_Feedback first; - api::ActionQueueCommand_Feedback second; - api::ActionQueueCommand_Feedback rejected; - api::ActionQueueCommand_Feedback retry; - - EXPECT_EQ( - executor.submitAndWait(first_request, first), - ActionQueueExecutor::WaitResult::Terminal); - EXPECT_EQ( - executor.submitAndWait(second_request, second), - ActionQueueExecutor::WaitResult::Terminal); - EXPECT_EQ( - executor.submitAndWait(rejected_request, rejected), - ActionQueueExecutor::WaitResult::Terminal); - EXPECT_EQ( - executor.submitAndWait(first_request, retry), - ActionQueueExecutor::WaitResult::Terminal); - - EXPECT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(second.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(rejected.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_EQ( - rejected.deduplication_status(), - api::ACTION_DEDUPLICATION_STATUS_LEDGER_EXHAUSTED); - EXPECT_EQ(retry.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ( - retry.deduplication_status(), - api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueCanceledWaiterLeavesAcceptedActionRunning) -{ - initializeActionDevices(); - ActionQueueExecutor executor(device::DeviceManager::getInstance()); - action_arm_->blockNextMotion(); - - api::ActionQueueCommand_Request request; - request.set_action_id("canceled-waiter-action-continues"); - request.set_expected_service_instance_id(executor.instanceId()); - request.set_total_timeout_ms(1000U); - addMoveLStep(request, "move", action_arm_->id(), 1.0); - - std::atomic waiter_canceled{false}; - api::ActionQueueCommand_Feedback abandoned_feedback; - auto waiter = std::async( - std::launch::async, - [&executor, &request, &abandoned_feedback, &waiter_canceled]() { - return executor.submitAndWait( - request, - abandoned_feedback, - [&waiter_canceled]() { - return waiter_canceled.load(std::memory_order_acquire); - }); - }); - ASSERT_TRUE(action_arm_->waitForMotionCalls( - 1, std::chrono::milliseconds(500))); - - waiter_canceled.store(true, std::memory_order_release); - const auto waiter_status = waiter.wait_for(std::chrono::milliseconds(500)); - if (waiter_status != std::future_status::ready) { - action_arm_->releaseBlockedMotion(); - } - ASSERT_EQ(waiter_status, std::future_status::ready); - EXPECT_EQ( - waiter.get(), - ActionQueueExecutor::WaitResult::CanceledAfterAdmission); - EXPECT_EQ(action_arm_->motionCalls(), 1); - - action_arm_->releaseBlockedMotion(); - ASSERT_TRUE(executor.waitForIdle(std::chrono::milliseconds(500))); - - api::ActionQueueCommand_Feedback retry_feedback; - const auto retry_result = executor.submitAndWait(request, retry_feedback); - - EXPECT_EQ(retry_result, ActionQueueExecutor::WaitResult::Terminal); - EXPECT_TRUE(retry_feedback.header().success()) - << retry_feedback.header().error_message(); - EXPECT_EQ( - retry_feedback.result(), - api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(retry_feedback.completed_steps(), 1U); - EXPECT_EQ(action_arm_->motionCalls(), 1); -} - -TEST_F(GrpcSystemServiceTest, - ConcurrentIdenticalActionIdJoinsInFlightExecution) -{ - initializeActionDevices(); - ActionQueueExecutor executor(device::DeviceManager::getInstance()); - action_arm_->blockNextMotion(); - - api::ActionQueueCommand_Request request; - request.set_action_id("join-identical-in-flight-action"); - request.set_expected_service_instance_id(executor.instanceId()); - request.set_total_timeout_ms(1000U); - addMoveLStep(request, "move", action_arm_->id(), 1.0); - - api::ActionQueueCommand_Feedback first_feedback; - api::ActionQueueCommand_Feedback joined_feedback; - auto first = std::async( - std::launch::async, - [&executor, &request, &first_feedback]() { - return executor.submitAndWait(request, first_feedback); - }); - const bool motion_started = action_arm_->waitForMotionCalls( - 1, std::chrono::milliseconds(500)); - - std::atomic joined_wait_polls{0}; - auto joined = std::async( - std::launch::async, - [&executor, &request, &joined_feedback, &joined_wait_polls]() { - return executor.submitAndWait( - request, - joined_feedback, - [&joined_wait_polls]() { - joined_wait_polls.fetch_add( - 1, std::memory_order_acq_rel); - return false; - }); - }); - const auto poll_deadline = - std::chrono::steady_clock::now() + std::chrono::milliseconds(500); - while (joined_wait_polls.load(std::memory_order_acquire) < 2 && - std::chrono::steady_clock::now() < poll_deadline) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - const bool joined_is_waiting = - joined_wait_polls.load(std::memory_order_acquire) >= 2; - action_arm_->releaseBlockedMotion(); - - const auto first_result = first.get(); - const auto joined_result = joined.get(); - - ASSERT_TRUE(motion_started); - ASSERT_TRUE(joined_is_waiting); - EXPECT_EQ(first_result, ActionQueueExecutor::WaitResult::Terminal); - EXPECT_EQ(joined_result, ActionQueueExecutor::WaitResult::Terminal); - EXPECT_EQ(first_feedback.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(joined_feedback.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ( - first_feedback.deduplication_status(), - api::ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW); - EXPECT_EQ( - joined_feedback.deduplication_status(), - api::ACTION_DEDUPLICATION_STATUS_JOINED_IN_FLIGHT); - EXPECT_EQ(action_arm_->motionCalls(), 1); -} - -TEST_F(GrpcSystemServiceTest, - ConcurrentActionQueuesRemainFifoAndHoldTheControlLease) -{ - initializeActionDevices(); - action_arm_->blockNextMotion(); - - api::ActionQueueCommand_Request first_request; - first_request.set_action_id("fifo-first"); - addMoveLStep(first_request, "first-1", action_arm_->id(), 10.0); - addMoveLStep(first_request, "first-2", action_arm_->id(), 11.0); - api::ActionQueueCommand_Request second_request; - second_request.set_action_id("fifo-second"); - addMoveLStep(second_request, "second-1", action_arm_->id(), 20.0); - - auto first = std::async( - std::launch::async, - [this, first_request]() { return executeAction(first_request); }); - const bool started = action_arm_->waitForMotionCalls( - 1, std::chrono::milliseconds(500)); - auto second = std::async( - std::launch::async, - [this, second_request]() { return executeAction(second_request); }); - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - - const auto competing = - control::ControlAuthorityManager::instance().tryAcquire( - action_arm_->id(), "ordinary-control", - std::chrono::seconds(1)); - if (competing.acquired) { - // Keep a failed assertion from stranding the worker behind this test - // lease and turning the diagnostic into a long timeout. - control::ControlAuthorityManager::instance().release( - competing.token); - } - action_arm_->releaseBlockedMotion(); - const auto first_response = first.get(); - const auto second_response = second.get(); - - EXPECT_TRUE(started); - EXPECT_FALSE(competing.acquired); - EXPECT_EQ(first_response.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(second_response.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(action_arm_->maxActiveMotions(), 1); - EXPECT_EQ( - action_trace_->names(), - (std::vector{ - "arm:L:10", "arm:L:11", "arm:L:20"})); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueTotalAndStepTimeoutsIssueTypedArmStop) -{ - initializeActionDevices(); - - action_arm_->blockNextMotion(); - api::ActionQueueCommand_Request total_timeout; - total_timeout.set_action_id("total-timeout"); - total_timeout.set_total_timeout_ms(40U); - addMoveLStep( - total_timeout, "total", action_arm_->id(), 1.0); - const auto total_response = executeAction(total_timeout); - const int stops_after_total = action_arm_->stopMotionCalls(); - - action_arm_->blockNextMotion(); - api::ActionQueueCommand_Request step_timeout; - step_timeout.set_action_id("step-timeout"); - step_timeout.set_total_timeout_ms(500U); - addMoveLStep( - step_timeout, "step", action_arm_->id(), 2.0, 30U); - const auto step_response = executeAction(step_timeout); - - EXPECT_EQ(total_response.result(), api::ACTION_RESULT_CODE_TIMED_OUT); - EXPECT_EQ(total_response.completed_steps(), 0U); - ASSERT_TRUE(total_response.has_failed_step_index()); - EXPECT_EQ(total_response.failed_step_index(), 0U); - EXPECT_GE(stops_after_total, 1); - EXPECT_EQ(step_response.result(), api::ACTION_RESULT_CODE_TIMED_OUT); - EXPECT_EQ(step_response.completed_steps(), 0U); - ASSERT_TRUE(step_response.has_failed_step_index()); - EXPECT_EQ(step_response.failed_step_index(), 0U); - EXPECT_GT(action_arm_->stopMotionCalls(), stops_after_total); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueRetiresUnconfirmedTimedOutArmStopForRecovery) -{ - initializeActionDevices(); - action_arm_->blockNextMotion(); - action_arm_->failStopAndBusyConfirmation(); - - api::ActionQueueCommand_Request request; - request.set_action_id("unconfirmed-timeout-stop"); - request.set_total_timeout_ms(40U); - addMoveLStep(request, "times-out", action_arm_->id(), 1.0); - - const auto response = executeAction(request); - - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_TIMED_OUT); - EXPECT_NE( - response.header().error_message().find("remains quarantined"), - std::string::npos); - auto& authority = control::ControlAuthorityManager::instance(); - EXPECT_FALSE(authority.tryAcquire( - action_arm_->id(), "move-before-recovery", std::chrono::hours(1)) - .acquired); - - const auto recovery = authority.preemptAcquire( - action_arm_->id(), - "confirmed-arm-recovery", - std::chrono::hours(1)); - ASSERT_TRUE(recovery.acquired) << recovery.detail; - ASSERT_TRUE(authority.waitForPreemptedRelease( - recovery.token, std::chrono::milliseconds::zero())); - ASSERT_TRUE(authority.recoverRetiredSafetyHolders(recovery.token)); - authority.release(recovery.token); - - const auto recovered_lease = authority.tryAcquire( - action_arm_->id(), "move-after-recovery", std::chrono::hours(1)); - EXPECT_TRUE(recovered_lease.acquired) << recovered_lease.detail; - authority.release(recovered_lease.token); -} - -TEST_F(GrpcSystemServiceTest, - DelayedActionCancellationCannotStopSuccessorControlLease) -{ - initializeActionDevices(); - action_arm_->blockNextMotion(); - action_arm_->blockCanceledMotionReturn(); - - api::ActionQueueCommand_Request request; - request.set_action_id("stale-action-stop-must-not-preempt-successor"); - request.set_total_timeout_ms(2000U); - addMoveLStep(request, "active", action_arm_->id(), 1.0); - - auto action = std::async( - std::launch::async, - [this, request]() { return executeAction(request); }); - const bool motion_started = action_arm_->waitForMotionCalls( - 1, std::chrono::milliseconds(500)); - - auto& authority = control::ControlAuthorityManager::instance(); - control::ControlAcquireResult direct_stop; - if (motion_started) { - direct_stop = authority.preemptAcquire( - action_arm_->id(), - "direct-stop-before-successor", - std::chrono::seconds(30)); - } - if (direct_stop.acquired) { - (void)action_arm_->stopMotion(); - } else { - action_arm_->releaseBlockedMotion(); - } - const bool old_driver_ready_to_return = - action_arm_->waitForCanceledMotionReturn( - std::chrono::milliseconds(500)); - const auto successor_before_handler_exit = authority.tryAcquire( - action_arm_->id(), - "successor-before-old-handler-exit", - std::chrono::seconds(30)); - action_arm_->releaseCanceledMotionReturn(); - - const auto action_status = action.wait_for(std::chrono::seconds(1)); - if (action_status != std::future_status::ready) { - service_->prepareForShutdown(); - } - ASSERT_EQ(action_status, std::future_status::ready); - const auto response = action.get(); - const bool old_handler_released = direct_stop.acquired && - authority.waitForPreemptedRelease( - direct_stop.token, std::chrono::seconds(1)); - if (direct_stop.acquired) { - authority.release(direct_stop.token); - } - const auto successor = authority.tryAcquire( - action_arm_->id(), - "successor-move", - std::chrono::seconds(30)); - const bool successor_still_current = - successor.acquired && authority.validate(successor.token); - if (successor.acquired) { - authority.release(successor.token); - } - - ASSERT_TRUE(motion_started); - ASSERT_TRUE(direct_stop.acquired) << direct_stop.detail; - ASSERT_TRUE(old_driver_ready_to_return); - EXPECT_FALSE(successor_before_handler_exit.acquired); - EXPECT_TRUE(old_handler_released); - ASSERT_TRUE(successor.acquired) << successor.detail; - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_CANCELED); - EXPECT_TRUE(successor_still_current); - EXPECT_EQ(action_arm_->stopMotionCalls(), 1); -} - -TEST_F(GrpcSystemServiceTest, - StopAllCancelsActiveActionAndSkipsRemainingSteps) -{ - initializeActionDevices(); - action_arm_->blockNextMotion(); - api::ActionQueueCommand_Request action_request; - action_request.set_action_id("stop-all-action"); - action_request.set_total_timeout_ms(500U); - addMoveLStep(action_request, "active", action_arm_->id(), 1.0); - addMoveLStep(action_request, "must-not-run", action_arm_->id(), 2.0); - - auto action = std::async( - std::launch::async, - [this, action_request]() { return executeAction(action_request); }); - const bool started = action_arm_->waitForMotionCalls( - 1, std::chrono::milliseconds(500)); - - api::StopAllCommand_Request stop_request; - api::StopAllCommand_Feedback stop_response; - grpc::ServerContext stop_context; - const auto stop_status = service_->StopAll( - &stop_context, &stop_request, &stop_response); - const auto action_response = action.get(); - - api::ActionQueueCommand_Request resumed_request; - resumed_request.set_action_id("action-after-stop-all"); - addMoveLStep( - resumed_request, "resumed", action_arm_->id(), 3.0); - const auto resumed_response = executeAction(resumed_request); - - EXPECT_TRUE(started); - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_TRUE(stop_response.header().success()) - << stop_response.header().error_message(); - EXPECT_EQ(action_response.result(), api::ACTION_RESULT_CODE_CANCELED); - EXPECT_EQ(action_response.completed_steps(), 0U); - ASSERT_TRUE(action_response.has_failed_step_index()); - EXPECT_EQ(action_response.failed_step_index(), 0U); - EXPECT_TRUE(resumed_response.header().success()) - << resumed_response.header().error_message(); - EXPECT_EQ( - resumed_response.result(), api::ACTION_RESULT_CODE_COMPLETED); - EXPECT_EQ(resumed_response.completed_steps(), 1U); - EXPECT_EQ( - resumed_response.service_instance_id(), service_instance_id_); - EXPECT_EQ(action_arm_->motionCalls(), 2); - EXPECT_GE(action_arm_->stopMotionCalls(), 1); -} - -TEST_F(GrpcSystemServiceTest, - StopAllRequestsMotionStopBeforeWaitingForMediaHandlers) -{ - initializeActionDevices(); - auto media_session = globalMediaActivityCoordinator().beginSession(); - ASSERT_TRUE(media_session); - - api::StopAllCommand_Request request; - auto stop_all = std::async(std::launch::async, [this, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll(&context, &request, &response); - return std::make_pair(status, response); - }); - - const bool motion_stop_requested = action_arm_->waitForStopMotionCalls( - 1, std::chrono::milliseconds(500)); - const auto status_while_media_active = - stop_all.wait_for(std::chrono::milliseconds(20)); - media_session.reset(); - - ASSERT_EQ( - stop_all.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - const auto [status, response] = stop_all.get(); - - EXPECT_TRUE(motion_stop_requested); - EXPECT_EQ( - status_while_media_active, - std::future_status::timeout); - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()) - << response.header().error_message(); -} - -TEST_F(GrpcSystemServiceTest, - StopAllDoesNotReportSuccessWhenAnotherAdmissionParticipantFails) -{ - initializeActionDevices(); - auto media_session = globalMediaActivityCoordinator().beginSession(); - ASSERT_TRUE(media_session); - - api::StopAllCommand_Request request; - auto stop_all = std::async(std::launch::async, [this, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll(&context, &request, &response); - return std::make_pair(status, response); - }); - - ASSERT_TRUE(action_arm_->waitForStopMotionCalls( - 1, std::chrono::milliseconds(500))); - auto& admission = globalStopAllAdmissionGate(); - const auto failed_participant = admission.beginStopAll(); - ASSERT_TRUE(failed_participant.valid()); - EXPECT_FALSE(admission.finishStopAll(failed_participant, false)); - media_session.reset(); - - ASSERT_EQ( - stop_all.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - const auto [status, response] = stop_all.get(); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_FALSE(response.header().success()); - EXPECT_NE( - response.header().error_message().find( - "could not safely resume system admission"), - std::string::npos); - EXPECT_FALSE(admission.lockAdmission().accepting()); -} - -TEST_F(GrpcSystemServiceTest, - StopAllDoesNotWaitForADeviceHealthSnapshot) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - auto trace = std::make_shared(); - auto arm = std::make_shared( - "health-blocked-arm", trace); - arm->blockHealthSnapshot(); - auto registration = std::async( - std::launch::async, [&manager, arm] { - manager.registerDevice(arm); - }); - const bool health_call_blocked = arm->waitForHealthSnapshot( - std::chrono::milliseconds(500)); - service_ = std::make_unique(); - - api::StopAllCommand_Request request; - auto stop_all = std::async(std::launch::async, [this, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll( - &context, &request, &response); - return std::make_pair(status, response); - }); - - const bool stop_dispatched_while_health_blocked = - arm->waitForStopMotionCalls( - 1, std::chrono::milliseconds(500)); - arm->releaseHealthSnapshot(); - - ASSERT_EQ( - registration.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - registration.get(); - ASSERT_EQ( - stop_all.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - const auto [status, response] = stop_all.get(); - - EXPECT_TRUE(health_call_blocked); - EXPECT_TRUE(stop_dispatched_while_health_blocked); - EXPECT_EQ(arm->healthSnapshotCalls(), 1); - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()) - << response.header().error_message(); -} - -TEST_F(GrpcSystemServiceTest, - StopAllDispatchesDeviceStopsWhileTaskActivityStopIsBlocked) -{ - task::TaskFactory::registerCreator( - config::TaskConfigEntry::TASK_TYPE_UME_TELEOP, - [](const config::TaskConfigEntry& entry) { - blocking_stop_task = - std::make_shared(entry.id()); - return blocking_stop_task; - }); - config::TaskManagerConfig task_config; - auto* task_entry = task_config.add_tasks(); - task_entry->set_id("blocking-stop-task"); - task_entry->set_type( - config::TaskConfigEntry::TASK_TYPE_UME_TELEOP); - task_entry->set_enable(true); - task_entry->set_run_mode( - config::TaskConfigEntry::TASK_RUN_MODE_BLOCKING_SERVICE); - auto& task_manager = task::TaskManager::getInstance(task_config); - ASSERT_TRUE(task_manager.initialized()); - ASSERT_NE(blocking_stop_task, nullptr); - - initializeActionDevices(); - auto camera = std::make_shared( - "camera-while-task-stop-blocked"); - device::DeviceManager::getInstance().registerDevice(camera); - api::StopAllCommand_Request request; - auto stop_all = std::async(std::launch::async, [this, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll( - &context, &request, &response); - return std::make_pair(status, response); - }); - - const bool task_stop_entered = - blocking_stop_task->waitForStopActivity( - std::chrono::milliseconds(500)); - const bool arm_stop_dispatched = action_arm_->waitForStopMotionCalls( - 1, std::chrono::milliseconds(500)); - const bool camera_stop_dispatched = camera->waitForStopRecordingCalls( - 1, std::chrono::milliseconds(500)); - const auto status_while_task_blocked = - stop_all.wait_for(std::chrono::milliseconds::zero()); - blocking_stop_task->releaseStopActivity(); - - ASSERT_EQ( - stop_all.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - const auto [status, response] = stop_all.get(); - - EXPECT_TRUE(task_stop_entered); - EXPECT_TRUE(arm_stop_dispatched); - EXPECT_TRUE(camera_stop_dispatched); - EXPECT_EQ(status_while_task_blocked, std::future_status::timeout); - EXPECT_EQ(blocking_stop_task->stopActivityCalls(), 1); - EXPECT_EQ(blocking_stop_task->lifecycleStopCalls(), 0); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()) - << response.header().error_message(); -} - -TEST_F(GrpcSystemServiceTest, - StopAllDispatchesPeerArmStopWhileAnotherDriverIsBlocked) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - auto trace = std::make_shared(); - auto blocking_arm = std::make_shared( - "a-blocking-arm", trace); - auto peer_arm = std::make_shared( - "b-peer-arm", trace); - auto peer_camera = std::make_shared( - "c-peer-camera"); - manager.registerDevice(blocking_arm); - manager.registerDevice(peer_arm); - manager.registerDevice(peer_camera); - blocking_arm->blockNextStopMotion(); - service_ = std::make_unique(); - - api::StopAllCommand_Request request; - auto stop_all = std::async(std::launch::async, [this, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll( - &context, &request, &response); - return std::make_pair(status, response); - }); - - const bool blocking_stop_entered = - blocking_arm->waitForBlockedStopMotion( - std::chrono::milliseconds(500)); - const bool peer_stop_dispatched = peer_arm->waitForStopMotionCalls( - 1, std::chrono::milliseconds(500)); - const bool peer_camera_stop_dispatched = - peer_camera->waitForStopRecordingCalls( - 1, std::chrono::milliseconds(500)); - const auto status_before_release = - stop_all.wait_for(std::chrono::milliseconds(0)); - blocking_arm->releaseBlockedStopMotion(); - const auto completion_status = - stop_all.wait_for(std::chrono::seconds(1)); - const auto [status, response] = stop_all.get(); - - EXPECT_TRUE(blocking_stop_entered); - EXPECT_TRUE(peer_stop_dispatched); - EXPECT_TRUE(peer_camera_stop_dispatched); - EXPECT_EQ(status_before_release, std::future_status::timeout); - EXPECT_EQ(completion_status, std::future_status::ready); - EXPECT_TRUE(status.ok()) << status.error_message(); - EXPECT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_GE(blocking_arm->stopMotionCalls(), 2); - EXPECT_GE(peer_arm->stopMotionCalls(), 2); - EXPECT_EQ(peer_camera->lifecycleStopCalls(), 0); -} - -TEST_F(GrpcSystemServiceTest, - StopAllRoundsAreSerializedAcrossSystemServiceInstances) -{ - initializeActionDevices(); - auto second_service = std::make_unique(); - action_arm_->blockNextStopMotion(); - - api::StopAllCommand_Request request; - auto first_stop = std::async(std::launch::async, [this, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll(&context, &request, &response); - return std::make_pair(status, response); - }); - - const bool first_stop_entered = action_arm_->waitForBlockedStopMotion( - std::chrono::milliseconds(500)); - auto second_stop = std::async( - std::launch::async, - [&second_service, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = second_service->StopAll( - &context, &request, &response); - return std::make_pair(status, response); - }); - - const auto second_before_release = - second_stop.wait_for(std::chrono::milliseconds(50)); - const int stop_calls_before_release = action_arm_->stopMotionCalls(); - action_arm_->releaseBlockedStopMotion(); - - EXPECT_TRUE(first_stop_entered); - EXPECT_EQ(second_before_release, std::future_status::timeout); - EXPECT_EQ(stop_calls_before_release, 1); - - const auto [first_status, first_response] = first_stop.get(); - const auto [second_status, second_response] = second_stop.get(); - EXPECT_TRUE(first_status.ok()) << first_status.error_message(); - EXPECT_TRUE(first_response.header().success()) - << first_response.header().error_message(); - EXPECT_TRUE(second_status.ok()) << second_status.error_message(); - EXPECT_TRUE(second_response.header().success()) - << second_response.header().error_message(); - EXPECT_EQ(first_response.operation_id(), second_response.operation_id()); - EXPECT_EQ( - first_response.current_safety_epoch(), - second_response.current_safety_epoch()); - EXPECT_EQ(action_arm_->stopMotionCalls(), 2); -} - -TEST_F(GrpcSystemServiceTest, - PrepareForShutdownDoesNotWaitForBlockedBusinessStopAll) -{ - initializeActionDevices(); - auto media_session = globalMediaActivityCoordinator().beginSession(); - ASSERT_TRUE(media_session); - - api::StopAllCommand_Request request; - auto stop_all = std::async(std::launch::async, [this, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll(&context, &request, &response); - return std::make_pair(status, response); - }); - - ASSERT_TRUE(action_arm_->waitForStopMotionCalls( - 1, std::chrono::milliseconds(500))); - ASSERT_EQ( - stop_all.wait_for(std::chrono::milliseconds(20)), - std::future_status::timeout); - - auto shutdown = std::async(std::launch::async, [this] { - service_->prepareForShutdown(); - }); - EXPECT_EQ( - shutdown.wait_for(std::chrono::milliseconds(500)), - std::future_status::ready); - shutdown.get(); - - media_session.reset(); - ASSERT_EQ( - stop_all.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - const auto [status, response] = stop_all.get(); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_FALSE(response.header().success()); - EXPECT_NE( - response.header().error_message().find( - "could not safely resume ActionQueue admission"), - std::string::npos); -} - -TEST_F(GrpcSystemServiceTest, - StopAllFailsClosedWhenAgvStoppedStateIsUnconfirmed) -{ - initializeActionDevices(); - action_agv_->setStoppedConfirmed(false); - - api::StopAllCommand_Request request; - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll( - &context, &request, &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_FALSE(response.header().success()); - const auto* agv_target = findSafetyTarget(response, action_agv_->id()); - ASSERT_NE(agv_target, nullptr); - EXPECT_EQ( - agv_target->reason_code(), - api::COMMAND_REASON_CODE_DEVICE_STILL_MOVING); - EXPECT_EQ( - agv_target->result(), api::SAFETY_OPERATION_RESULT_FAILED); - const auto lease = - control::ControlAuthorityManager::instance().tryAcquire( - action_agv_->id(), - "normal-control-after-unconfirmed-stop-all", - std::chrono::hours(1)); - EXPECT_FALSE(lease.acquired); -} - -TEST_F(GrpcSystemServiceTest, - StopAllStopsActiveMediaWithoutStoppingDeviceLifecycles) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - auto camera = std::make_shared("recording-camera"); - auto microphone = - std::make_shared("recording-microphone"); - auto speaker = std::make_shared("playing-speaker"); - manager.registerDevice(camera); - manager.registerDevice(microphone); - manager.registerDevice(speaker); - ASSERT_EQ( - globalCameraOperationalActivityRegistry().start( - camera->id(), camera), - CameraOperationalActivityRegistry::DispatchResult::Success); - service_ = std::make_unique(); - - api::StopAllCommand_Request request; - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll( - &context, &request, &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(camera->stopRecordingCalls(), 1); - EXPECT_FALSE(camera->isRecording()); - EXPECT_EQ(microphone->stopRecordingCalls(), 2); - EXPECT_FALSE(microphone->isRecording()); - EXPECT_EQ(speaker->stopPlaybackCalls(), 2); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - EXPECT_GE(camera->operationalStopCalls(), 2); - EXPECT_FALSE(camera->operationalActive()); - EXPECT_EQ(microphone->lifecycleStopCalls(), 0); - EXPECT_EQ(speaker->lifecycleStopCalls(), 0); -} - -TEST_F(GrpcSystemServiceTest, - StopAllStopsUntrackedInventoryCameraOperationalActivity) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - auto camera = std::make_shared( - "untracked-inventory-camera"); - camera->setRecording(false); - ASSERT_TRUE(camera->startOperationalActivity()); - manager.registerDevice(camera); - service_ = std::make_unique(); - - api::StopAllCommand_Request request; - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll( - &context, &request, &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(camera->operationalStopCalls(), 2); - EXPECT_FALSE(camera->operationalActive()); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); -} - -TEST_F(GrpcSystemServiceTest, - StopAllRepeatsOperationalStopAfterAnOldMediaStartFinishes) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - auto camera = std::make_shared("raced-recording-camera"); - camera->blockNextStartRecording(); - manager.registerDevice(camera); - service_ = std::make_unique(); - - gRPCCameraServiceImpl camera_service; - api::StartCameraRecordingCommand_Request start_request; - start_request.mutable_header()->set_device_id(camera->id()); - start_request.set_video_path("raced.mp4"); - auto old_start = std::async( - std::launch::async, - [&] { - api::StartCameraRecordingCommand_Feedback response; - grpc::ServerContext context; - const auto status = camera_service.StartRecording( - &context, &start_request, &response); - return std::make_pair(status, response); - }); - ASSERT_TRUE(camera->waitForStartRecordingEntered( - std::chrono::milliseconds(500))); - - api::StopAllCommand_Request request; - auto stop_all = std::async(std::launch::async, [this, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll(&context, &request, &response); - return std::make_pair(status, response); - }); - - ASSERT_TRUE(camera->waitForStopRecordingCalls( - 1, std::chrono::milliseconds(500))); - EXPECT_EQ( - stop_all.wait_for(std::chrono::milliseconds(20)), - std::future_status::timeout); - camera->releaseBlockedStartRecording(); - - ASSERT_EQ( - old_start.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - const auto [start_status, start_response] = old_start.get(); - ASSERT_TRUE(start_status.ok()) << start_status.error_message(); - EXPECT_TRUE(start_response.header().success()) - << start_response.header().error_message(); - ASSERT_TRUE(camera->waitForStartRecordingCalls( - 1, std::chrono::milliseconds(500))); - ASSERT_EQ( - stop_all.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - const auto [status, response] = stop_all.get(); - - ASSERT_TRUE(status.ok()) << status.error_message(); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(camera->stopRecordingCalls(), 2); - EXPECT_FALSE(camera->isRecording()); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); -} - -TEST_F(GrpcSystemServiceTest, - StopAllStopsMediaActivitiesAbsentFromDeviceManager) -{ - config::DeviceManagerConfig config; - device::DeviceManager::getInstance(config); - - constexpr const char* hub_source_id = "orphan-hub-camera"; - constexpr const char* hub_track_id = "orphan-hub-camera/video/color"; - media::TrackDescriptor::Config track_config; - track_config.id = hub_track_id; - track_config.source_id = hub_source_id; - track_config.kind = media::MediaKind::VIDEO; - track_config.time_base = {1, 90000}; - auto descriptor = media::makeTrackDescriptor(std::move(track_config)); - auto hub_stop_calls = std::make_shared>(0); - media::MediaSourceManager::SourceCallbacks callbacks; - callbacks.start = []( - const media::MediaSourceManager::FrameSink&, - const media::MediaSourceManager::CancelPredicate&) { - return true; - }; - callbacks.stop_confirmed = [hub_stop_calls] { - hub_stop_calls->fetch_add(1, std::memory_order_relaxed); - return true; - }; - auto& hub = media::globalMediaSourceManager(); - ASSERT_TRUE(hub.registerSource( - descriptor, std::move(callbacks), 2)); - auto subscription = hub.subscribe(hub_track_id); - ASSERT_TRUE(subscription.valid()); - - auto operational_camera = std::make_shared( - "orphan-operational-camera"); - operational_camera->setRecording(false); - ASSERT_EQ( - globalCameraOperationalActivityRegistry().start( - operational_camera->id(), operational_camera), - CameraOperationalActivityRegistry::DispatchResult::Success); - - auto ptz_camera = std::make_shared( - "orphan-ptz-camera"); - ASSERT_EQ( - globalCameraPtzActivityRegistry().control( - ptz_camera->id(), - ptz_camera, - device::PtzCommand::PanLeft, - false, - 3), - CameraPtzActivityRegistry::DispatchResult::Success); - - service_ = std::make_unique(); - api::StopAllCommand_Request request; - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll( - &context, &request, &response); - - ASSERT_TRUE(status.ok()) << status.error_message(); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(hub_stop_calls->load(std::memory_order_relaxed), 1); - EXPECT_FALSE(subscription.valid()); - EXPECT_TRUE(hub.trackedSourceIds().empty()); - EXPECT_EQ(operational_camera->operationalStopCalls(), 1); - EXPECT_FALSE(operational_camera->operationalActive()); - EXPECT_EQ(operational_camera->lifecycleStopCalls(), 0); - EXPECT_EQ(operational_camera->stopRecordingCalls(), 0); - EXPECT_EQ(ptz_camera->ptzCalls(), 2); - EXPECT_FALSE(ptz_camera->ptzActive()); - EXPECT_EQ(ptz_camera->lifecycleStopCalls(), 0); - EXPECT_EQ(ptz_camera->stopRecordingCalls(), 0); -} - -TEST_F(GrpcSystemServiceTest, - StopAllMediaFailuresLeaveActionQueuePaused) -{ - initializeActionDevices(); - auto& manager = device::DeviceManager::getInstance(); - auto camera = std::make_shared( - "camera-remains-recording", false); - auto microphone = std::make_shared( - "microphone-stop-throws", true, true); - auto speaker = std::make_shared( - "speaker-stop-fails", false); - manager.registerDevice(camera); - manager.registerDevice(microphone); - manager.registerDevice(speaker); - - api::StopAllCommand_Request stop_request; - api::StopAllCommand_Feedback stop_response; - grpc::ServerContext stop_context; - const auto stop_status = service_->StopAll( - &stop_context, &stop_request, &stop_response); - - api::ActionQueueCommand_Request action_request; - action_request.set_action_id("action-after-media-stop-failure"); - addMoveLStep( - action_request, "must-not-run", action_arm_->id(), 1.0); - const auto action_response = executeAction(action_request); - - ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); - EXPECT_FALSE(stop_response.header().success()); - const auto* camera_target = findSafetyTarget( - stop_response, camera->id()); - ASSERT_NE(camera_target, nullptr); - EXPECT_EQ( - camera_target->reason_code(), - api::COMMAND_REASON_CODE_STOP_UNCONFIRMED); - EXPECT_EQ(camera->stopRecordingCalls(), 2); - EXPECT_TRUE(camera->isRecording()); - EXPECT_EQ(microphone->stopRecordingCalls(), 2); - EXPECT_TRUE(microphone->isRecording()); - EXPECT_EQ(speaker->stopPlaybackCalls(), 2); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - EXPECT_EQ(microphone->lifecycleStopCalls(), 0); - EXPECT_EQ(speaker->lifecycleStopCalls(), 0); - EXPECT_FALSE(action_response.header().success()); - EXPECT_EQ( - action_response.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_NE( - action_response.header().error_message().find( - "temporarily paused by StopAll"), - std::string::npos); - EXPECT_EQ(action_arm_->motionCalls(), 0); -} - -TEST_F(GrpcSystemServiceTest, - ActionQueueCancellationStopsActiveLaterArmBeforeEarlierArm) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - action_trace_ = std::make_shared(); - auto earlier_arm = std::make_shared( - "earlier-arm", action_trace_); - auto active_arm = std::make_shared( - "active-arm", action_trace_); - manager.registerDevice(earlier_arm); - manager.registerDevice(active_arm); - service_ = std::make_unique(); - api::GetSystemInfoCommand_Request info_request; - api::GetSystemInfoCommand_Feedback info_response; - grpc::ServerContext info_context; - ASSERT_TRUE(service_->GetSystemInfo( - &info_context, &info_request, &info_response).ok()); - service_instance_id_ = info_response.action_service_instance_id(); - ASSERT_FALSE(service_instance_id_.empty()); - - active_arm->blockNextMotion(); - api::ActionQueueCommand_Request request; - request.set_action_id("cancel-active-later-arm-first"); - request.set_total_timeout_ms(1000U); - addMoveLStep(request, "earlier-completes", earlier_arm->id(), 1.0); - addMoveLStep(request, "active-blocks", active_arm->id(), 2.0); - - auto action = std::async( - std::launch::async, - [this, request]() { return executeAction(request); }); - const bool active_started = active_arm->waitForMotionCalls( - 1, std::chrono::milliseconds(500)); - - service_->prepareForShutdown(); - const auto response = action.get(); - - api::ActionQueueCommand_Request rejected_request; - rejected_request.set_action_id("action-after-service-shutdown"); - addMoveLStep( - rejected_request, "must-not-run", earlier_arm->id(), 3.0); - const auto rejected_response = executeAction(rejected_request); - - const auto trace = action_trace_->names(); - const auto active_stop = std::find( - trace.begin(), trace.end(), "arm:stop:active-arm"); - const auto earlier_stop = std::find( - trace.begin(), trace.end(), "arm:stop:earlier-arm"); - - ASSERT_TRUE(active_started); - EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_CANCELED); - EXPECT_EQ(response.completed_steps(), 1U); - ASSERT_TRUE(response.has_failed_step_index()); - EXPECT_EQ(response.failed_step_index(), 1U); - ASSERT_NE(active_stop, trace.end()); - ASSERT_NE(earlier_stop, trace.end()); - EXPECT_LT(active_stop, earlier_stop); - EXPECT_FALSE(rejected_response.header().success()); - EXPECT_EQ( - rejected_response.result(), api::ACTION_RESULT_CODE_REJECTED); - EXPECT_NE( - rejected_response.header().error_message().find("shutting down"), - std::string::npos); - EXPECT_EQ(earlier_arm->motionCalls(), 1); - EXPECT_EQ(active_arm->motionCalls(), 1); -} - -TEST_F(GrpcSystemServiceTest, - RetriedStopAllReusesTimedOutArmBarrierAndCanRecover) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - auto trace = std::make_shared(); - auto arm = std::make_shared( - "preempted-handler-arm", trace); - manager.registerDevice(arm); - - auto& authority = control::ControlAuthorityManager::instance(); - const auto old_handler = authority.tryAcquire( - arm->id(), "blocked-control-handler", std::chrono::seconds(30)); - ASSERT_TRUE(old_handler.acquired) << old_handler.detail; - service_ = std::make_unique( - std::chrono::milliseconds(300)); - - api::StopAllCommand_Request request; - auto stop_all = std::async(std::launch::async, [this, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll( - &context, &request, &response); - return std::make_pair(status, response); - }); - - const bool initial_stop_completed = arm->waitForStopMotionCalls( - 1, std::chrono::milliseconds(500)); - if (initial_stop_completed) { - arm->blockNextStopMotion(); - } - const auto completion = stop_all.wait_for(std::chrono::seconds(1)); - const bool final_stop_started = initial_stop_completed && - arm->waitForBlockedStopMotion(std::chrono::milliseconds(500)); - - ASSERT_EQ(completion, std::future_status::ready); - const auto [status, response] = stop_all.get(); - const auto stop_calls_before_retry = arm->stopMotionCalls(); - const auto control_while_failed_closed = authority.tryAcquire( - arm->id(), "control-after-timed-out-stop-all", - std::chrono::seconds(30)); - - // Let the deadline-overrunning final typed stop finish before retrying the - // retained barrier. The old control owner is then allowed to drain. - arm->releaseBlockedStopMotion(); - authority.release(old_handler.token); - - auto second_service = std::make_unique( - std::chrono::seconds(1)); - api::StopAllCommand_Feedback second_response; - grpc::ServerContext second_context; - const auto second_status = second_service->StopAll( - &second_context, &request, &second_response); - const auto recovered_control = authority.tryAcquire( - arm->id(), "control-after-retried-stop-all", - std::chrono::seconds(30)); - if (recovered_control.acquired) { - authority.release(recovered_control.token); - } - - EXPECT_TRUE(initial_stop_completed); - EXPECT_TRUE(final_stop_started); - ASSERT_TRUE(status.ok()) << status.error_message(); - EXPECT_FALSE(response.header().success()); - const auto* failed_arm = findSafetyTarget(response, arm->id()); - ASSERT_NE(failed_arm, nullptr); - EXPECT_EQ( - failed_arm->reason_code(), - api::COMMAND_REASON_CODE_PARTICIPANT_TIMEOUT); - ASSERT_TRUE(second_status.ok()) << second_status.error_message(); - EXPECT_TRUE(second_response.header().success()) - << second_response.header().error_message(); - EXPECT_EQ(stop_calls_before_retry, 2); - EXPECT_GE(arm->stopMotionCalls(), 4); - EXPECT_FALSE(control_while_failed_closed.acquired); - EXPECT_TRUE(recovered_control.acquired) << recovered_control.detail; -} - -TEST_F(GrpcSystemServiceTest, - StopAllDoesNotStopDeviceLifecyclesAndRevokesOnlyArmLease) -{ - config::DeviceManagerConfig config; - auto& manager = device::DeviceManager::getInstance(config); - auto trace = std::make_shared(); - auto arm = std::make_shared("leased_arm", trace); - auto camera = std::make_shared("leased_camera"); - auto already_stopping_arm = std::make_shared( - "already_stopping_arm", trace); - manager.registerDevice(arm); - manager.registerDevice(camera); - manager.registerDevice(already_stopping_arm); - - auto& authority = control::ControlAuthorityManager::instance(); - const auto arm_lease = authority.tryAcquire( - arm->id(), "arm-session", std::chrono::seconds(30)); - const auto camera_lease = authority.tryAcquire( - camera->id(), "camera-session", std::chrono::seconds(30)); - const auto existing_stop_barrier = authority.preemptAcquire( - already_stopping_arm->id(), - "existing-stop", - std::chrono::seconds(30)); - ASSERT_TRUE(arm_lease.acquired) << arm_lease.detail; - ASSERT_TRUE(camera_lease.acquired) << camera_lease.detail; - ASSERT_TRUE(existing_stop_barrier.acquired) - << existing_stop_barrier.detail; - ASSERT_TRUE(authority.validate(arm_lease.token)); - ASSERT_TRUE(authority.validate(camera_lease.token)); - - service_ = std::make_unique(); - api::StopAllCommand_Request request; - auto stop_all = std::async(std::launch::async, [this, &request] { - api::StopAllCommand_Feedback response; - grpc::ServerContext context; - const auto status = service_->StopAll( - &context, &request, &response); - return std::make_pair(status, response); - }); - ASSERT_TRUE(arm->waitForStopMotionCalls( - 1, std::chrono::milliseconds(500))); - EXPECT_FALSE(authority.validate(arm_lease.token)); - EXPECT_EQ( - stop_all.wait_for(std::chrono::milliseconds(20)), - std::future_status::timeout); - authority.release(arm_lease.token); - ASSERT_EQ( - stop_all.wait_for(std::chrono::seconds(1)), - std::future_status::ready); - const auto [status, response] = stop_all.get(); - - ASSERT_TRUE(status.ok()) << status.error_message(); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_GE(arm->stopMotionCalls(), 2); - EXPECT_EQ(arm->lifecycleStopCalls(), 0); - EXPECT_EQ(arm->shutdownCalls(), 0); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - EXPECT_EQ(camera->stopRecordingCalls(), 1); - EXPECT_FALSE(camera->isRecording()); - EXPECT_GE(already_stopping_arm->stopMotionCalls(), 2); - EXPECT_EQ(already_stopping_arm->lifecycleStopCalls(), 0); - EXPECT_EQ(already_stopping_arm->shutdownCalls(), 0); - EXPECT_FALSE(authority.validate(arm_lease.token)); - EXPECT_FALSE(authority.isLeased(arm->id())); - EXPECT_TRUE(authority.validate(camera_lease.token)); - EXPECT_TRUE(authority.isLeased(camera->id())); - EXPECT_TRUE(authority.validate(existing_stop_barrier.token)); - EXPECT_TRUE(authority.isLeased(already_stopping_arm->id())); - const auto move_after_stop = authority.tryAcquire( - arm->id(), "move-after-stop", std::chrono::seconds(30)); - EXPECT_TRUE(move_after_stop.acquired) << move_after_stop.detail; - authority.release(move_after_stop.token); - authority.release(existing_stop_barrier.token); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/media_activity_coordinator_test.cpp b/cmvr-es/service/grpc/server/tests/media_activity_coordinator_test.cpp deleted file mode 100644 index 17ca7db1..00000000 --- a/cmvr-es/service/grpc/server/tests/media_activity_coordinator_test.cpp +++ /dev/null @@ -1,279 +0,0 @@ -#include "service/grpc/server/include/media_activity_coordinator.h" - -#include -#include -#include -#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; - - { - cmvr::service::MediaActivityCoordinator dispatch_coordinator; - auto dispatch_session = dispatch_coordinator.beginSession(); - CHECK_TRUE(dispatch_session); - - std::promise dispatch_entered; - auto dispatch_entered_future = dispatch_entered.get_future(); - std::promise release_dispatch; - auto release_dispatch_future = release_dispatch.get_future(); - auto dispatch_future = std::async(std::launch::async, [&] { - return dispatch_session.runIfCurrent([&] { - dispatch_entered.set_value(); - release_dispatch_future.wait(); - }); - }); - dispatch_entered_future.wait(); - - const auto dispatch_ticket = dispatch_coordinator.beginStopAll(); - CHECK_TRUE(dispatch_ticket.valid()); - CHECK_TRUE(!dispatch_coordinator.waitForStopped( - dispatch_ticket, 20ms)); - - release_dispatch.set_value(); - CHECK_TRUE(dispatch_future.get()); - - dispatch_session.reset(); - CHECK_TRUE(dispatch_coordinator.waitForStopped(dispatch_ticket, 100ms)); - CHECK_TRUE(dispatch_coordinator.finishStopAll(dispatch_ticket, true)); - } - - { - cmvr::service::MediaActivityCoordinator generation_coordinator; - auto old_generation_session = generation_coordinator.beginSession(); - CHECK_TRUE(old_generation_session); - - const auto generation_ticket = generation_coordinator.beginStopAll(); - CHECK_TRUE(generation_ticket.valid()); - - std::atomic dispatched_operations{0}; - CHECK_TRUE(!old_generation_session.runIfCurrent( - [&] { ++dispatched_operations; })); - CHECK_TRUE(dispatched_operations.load(std::memory_order_acquire) == 0); - - old_generation_session.reset(); - CHECK_TRUE( - generation_coordinator.waitForStopped(generation_ticket, 100ms)); - CHECK_TRUE(generation_coordinator.finishStopAll(generation_ticket, true)); - - auto current_generation_session = generation_coordinator.beginSession(); - CHECK_TRUE(current_generation_session); - CHECK_TRUE(current_generation_session.runIfCurrent( - [&] { ++dispatched_operations; })); - CHECK_TRUE(dispatched_operations.load(std::memory_order_acquire) == 1); - } - - { - cmvr::service::MediaActivityCoordinator deferred_coordinator; - std::atomic deferred_cancel_calls{0}; - auto deferred_session = deferred_coordinator.beginSession( - [&] { ++deferred_cancel_calls; }); - CHECK_TRUE(deferred_session); - - const auto deferred_ticket = - deferred_coordinator.beginStopAll(true); - CHECK_TRUE(deferred_ticket.valid()); - CHECK_TRUE(deferred_session.cancelled()); - CHECK_TRUE(!deferred_session.runIfCurrent([] {})); - CHECK_TRUE( - deferred_cancel_calls.load(std::memory_order_acquire) == 0); - CHECK_TRUE( - deferred_coordinator.requestCancellation(deferred_ticket)); - CHECK_TRUE( - deferred_cancel_calls.load(std::memory_order_acquire) == 1); - CHECK_TRUE( - deferred_coordinator.requestCancellation(deferred_ticket)); - CHECK_TRUE( - deferred_cancel_calls.load(std::memory_order_acquire) == 1); - - deferred_session.reset(); - CHECK_TRUE( - deferred_coordinator.waitForStopped(deferred_ticket, 100ms)); - CHECK_TRUE( - deferred_coordinator.finishStopAll(deferred_ticket, true)); - CHECK_TRUE( - !deferred_coordinator.requestCancellation(deferred_ticket)); - } - - { - cmvr::service::MediaActivityCoordinator parallel_coordinator; - std::promise blocked_cancel_entered; - auto blocked_cancel_entered_future = - blocked_cancel_entered.get_future(); - std::promise release_blocked_cancel; - auto release_blocked_cancel_future = - release_blocked_cancel.get_future().share(); - std::promise peer_cancel_entered; - auto peer_cancel_entered_future = peer_cancel_entered.get_future(); - std::atomic blocked_cancel_calls{0}; - std::atomic peer_cancel_calls{0}; - - auto blocked_session = parallel_coordinator.beginSession([&] { - ++blocked_cancel_calls; - blocked_cancel_entered.set_value(); - release_blocked_cancel_future.wait(); - }); - auto peer_session = parallel_coordinator.beginSession([&] { - ++peer_cancel_calls; - peer_cancel_entered.set_value(); - }); - CHECK_TRUE(blocked_session && peer_session); - - const auto ticket = parallel_coordinator.beginStopAll(true); - std::vector operations; - std::string collect_error; - CHECK_TRUE(parallel_coordinator.collectCancellationOperations( - ticket, operations, &collect_error)); - CHECK_TRUE(collect_error.empty()); - CHECK_TRUE(operations.size() == 2); - CHECK_TRUE(operations[0].resource_key != operations[1].resource_key); - - auto first = std::async(std::launch::async, operations[0].operation); - auto second = std::async(std::launch::async, operations[1].operation); - CHECK_TRUE(blocked_cancel_entered_future.wait_for(100ms) == - std::future_status::ready); - CHECK_TRUE(peer_cancel_entered_future.wait_for(100ms) == - std::future_status::ready); - release_blocked_cancel.set_value(); - CHECK_TRUE(first.get().success); - CHECK_TRUE(second.get().success); - CHECK_TRUE(operations[0].operation().success); - CHECK_TRUE(operations[1].operation().success); - CHECK_TRUE(blocked_cancel_calls.load() == 1); - CHECK_TRUE(peer_cancel_calls.load() == 1); - - blocked_session.reset(); - peer_session.reset(); - CHECK_TRUE(parallel_coordinator.waitForStopped(ticket, 100ms)); - CHECK_TRUE(parallel_coordinator.finishStopAll(ticket, true)); - CHECK_TRUE(!parallel_coordinator.collectCancellationOperations( - ticket, operations, &collect_error)); - CHECK_TRUE(!collect_error.empty()); - } - - { - cmvr::service::MediaActivityCoordinator exception_coordinator; - std::atomic throwing_cancel_calls{0}; - auto throwing_session = exception_coordinator.beginSession([&] { - ++throwing_cancel_calls; - throw std::runtime_error("cancel failed"); - }); - CHECK_TRUE(throwing_session); - const auto ticket = exception_coordinator.beginStopAll(true); - std::vector operations; - CHECK_TRUE(exception_coordinator.collectCancellationOperations( - ticket, operations)); - CHECK_TRUE(operations.size() == 1); - const auto result = operations[0].operation(); - const auto cached_result = operations[0].operation(); - CHECK_TRUE(!result.success); - CHECK_TRUE(result.detail.find("cancel failed") != std::string::npos); - CHECK_TRUE(cached_result.detail == result.detail); - CHECK_TRUE(throwing_cancel_calls.load() == 1); - throwing_session.reset(); - CHECK_TRUE(exception_coordinator.waitForStopped(ticket, 100ms)); - CHECK_TRUE(!exception_coordinator.finishStopAll(ticket, false)); - } - - { - cmvr::service::DeferredStopOperation detached_operation; - cmvr::service::MediaActivityCoordinator::Session retained_session; - std::atomic detached_cancel_calls{0}; - { - cmvr::service::MediaActivityCoordinator ephemeral_coordinator; - retained_session = ephemeral_coordinator.beginSession( - [&] { ++detached_cancel_calls; }); - const auto ticket = - ephemeral_coordinator.beginStopAll(true); - std::vector operations; - CHECK_TRUE( - ephemeral_coordinator.collectCancellationOperations( - ticket, operations)); - CHECK_TRUE(operations.size() == 1); - detached_operation = std::move(operations[0]); - } - CHECK_TRUE(detached_operation.operation().success); - CHECK_TRUE(detached_cancel_calls.load() == 1); - retained_session.reset(); - } - - cmvr::service::MediaActivityCoordinator coordinator; - std::atomic cancel_calls{0}; - - auto old_session = coordinator.beginSession([&] { ++cancel_calls; }); - CHECK_TRUE(old_session); - CHECK_TRUE(old_session.claimExclusiveResource("speaker:test")); - - auto competing_session = coordinator.beginSession(); - CHECK_TRUE(competing_session); - CHECK_TRUE(!competing_session.claimExclusiveResource("speaker:test")); - competing_session.reset(); - - const auto ticket = coordinator.beginStopAll(); - CHECK_TRUE(ticket.valid()); - CHECK_TRUE(cancel_calls.load(std::memory_order_acquire) == 1); - CHECK_TRUE(old_session.cancelled()); - CHECK_TRUE(!old_session.runIfCurrent([] {})); - CHECK_TRUE(!coordinator.beginSession()); - CHECK_TRUE(!coordinator.waitForStopped(ticket, 5ms)); - - old_session.reset(); - CHECK_TRUE(coordinator.waitForStopped(ticket, 100ms)); - CHECK_TRUE(coordinator.finishStopAll(ticket, true)); - - auto resumed_session = coordinator.beginSession(); - CHECK_TRUE(resumed_session); - CHECK_TRUE(resumed_session.claimExclusiveResource("speaker:test")); - - const auto concurrent_ticket_a = coordinator.beginStopAll(); - const auto concurrent_ticket_b = coordinator.beginStopAll(); - resumed_session.reset(); - CHECK_TRUE(coordinator.waitForStopped(concurrent_ticket_a, 100ms)); - CHECK_TRUE(coordinator.finishStopAll(concurrent_ticket_a, true)); - CHECK_TRUE(!coordinator.beginSession()); - CHECK_TRUE(coordinator.finishStopAll(concurrent_ticket_b, true)); - CHECK_TRUE(coordinator.beginSession()); - - { - cmvr::service::MediaActivityCoordinator detailed_coordinator; - const auto first = detailed_coordinator.beginStopAll(true); - const auto second = detailed_coordinator.beginStopAll(true); - const auto first_result = - detailed_coordinator.finishStopAllDetailed(first, true); - CHECK_TRUE(first_result.ticket_consumed); - CHECK_TRUE(first_result.participant_stopped); - CHECK_TRUE(!first_result.admission_resumed); - const auto second_result = - detailed_coordinator.finishStopAllDetailed(second, true); - CHECK_TRUE(second_result.ticket_consumed); - CHECK_TRUE(second_result.participant_stopped); - CHECK_TRUE(second_result.admission_resumed); - CHECK_TRUE(detailed_coordinator.beginSession()); - } - - std::cout << "media_activity_coordinator_test: PASS\n"; - return 0; -} diff --git a/cmvr-es/service/grpc/server/tests/motor_activity_coordinator_test.cpp b/cmvr-es/service/grpc/server/tests/motor_activity_coordinator_test.cpp deleted file mode 100644 index 35d430f6..00000000 --- a/cmvr-es/service/grpc/server/tests/motor_activity_coordinator_test.cpp +++ /dev/null @@ -1,283 +0,0 @@ -#include "service/grpc/server/include/motor_activity_coordinator.h" - -#include -#include -#include -#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::MotorActivityCoordinator; - - MotorActivityCoordinator coordinator; - std::atomic cancel_calls{0}; - std::atomic quick_stop_calls{0}; - std::atomic busy{true}; - - auto registration = coordinator.registerControl( - [&] { ++cancel_calls; }, - [&] { - ++quick_stop_calls; - return true; - }, - [&] { return !busy.load(std::memory_order_acquire); }, - "test motor"); - CHECK_TRUE(registration); - - const auto ticket = coordinator.beginStopAll(); - CHECK_TRUE(ticket.valid()); - CHECK_TRUE(cancel_calls.load(std::memory_order_acquire) == 1); - CHECK_TRUE(!coordinator.lockAdmission().accepting()); - - std::string initial_error; - CHECK_TRUE(coordinator.requestStop(ticket, &initial_error)); - CHECK_TRUE(initial_error.empty()); - CHECK_TRUE(quick_stop_calls.load(std::memory_order_acquire) == 1); - - auto stop_future = std::async(std::launch::async, [&] { - std::string error; - return coordinator.waitForStopped(ticket, 1s, &error); - }); - CHECK_TRUE(stop_future.wait_for(20ms) == std::future_status::timeout); - - busy.store(false, std::memory_order_release); - coordinator.notifyStateChanged(); - CHECK_TRUE(stop_future.get()); - CHECK_TRUE(coordinator.finishStopAll(ticket, true)); - CHECK_TRUE(coordinator.lockAdmission().accepting()); - - std::atomic quick_stops_in_flight{0}; - std::atomic maximum_quick_stops{0}; - auto make_serial_registration = [&](const char* description) { - return coordinator.registerControl( - [] {}, - [&] { - const int active = ++quick_stops_in_flight; - int maximum = maximum_quick_stops.load(); - while (maximum < active && - !maximum_quick_stops.compare_exchange_weak( - maximum, active)) { - } - std::this_thread::sleep_for(5ms); - --quick_stops_in_flight; - return true; - }, - [] { return true; }, - description); - }; - auto serial_a = make_serial_registration("serial motor a"); - auto serial_b = make_serial_registration("serial motor b"); - CHECK_TRUE(serial_a && serial_b); - - const auto serial_ticket = coordinator.beginStopAll(); - CHECK_TRUE(serial_ticket.valid()); - CHECK_TRUE(coordinator.stopAndWait(serial_ticket, 1s)); - CHECK_TRUE(maximum_quick_stops.load(std::memory_order_acquire) == 1); - CHECK_TRUE(coordinator.finishStopAll(serial_ticket, true)); - - { - MotorActivityCoordinator parallel_coordinator; - std::promise blocked_stop_entered; - auto blocked_stop_entered_future = - blocked_stop_entered.get_future(); - std::promise release_blocked_stop; - auto release_blocked_stop_future = - release_blocked_stop.get_future().share(); - std::promise peer_stop_entered; - auto peer_stop_entered_future = peer_stop_entered.get_future(); - std::atomic blocked_cancel_calls{0}; - std::atomic blocked_stop_calls{0}; - std::atomic peer_cancel_calls{0}; - std::atomic peer_stop_calls{0}; - - auto blocked = parallel_coordinator.registerControl( - [&] { ++blocked_cancel_calls; }, - [&] { - if (blocked_cancel_calls.load() != 1) { - return false; - } - ++blocked_stop_calls; - blocked_stop_entered.set_value(); - release_blocked_stop_future.wait(); - return true; - }, - [] { return true; }, - "blocked motor"); - auto peer = parallel_coordinator.registerControl( - [&] { ++peer_cancel_calls; }, - [&] { - if (peer_cancel_calls.load() != 1) { - return false; - } - ++peer_stop_calls; - peer_stop_entered.set_value(); - return true; - }, - [] { return true; }, - "peer motor"); - CHECK_TRUE(blocked && peer); - - const auto parallel_ticket = - parallel_coordinator.beginStopAll(true); - CHECK_TRUE(parallel_ticket.valid()); - CHECK_TRUE(blocked_cancel_calls.load() == 0); - CHECK_TRUE(peer_cancel_calls.load() == 0); - std::vector operations; - std::string collect_error; - CHECK_TRUE(parallel_coordinator.collectStopOperations( - parallel_ticket, operations, &collect_error)); - CHECK_TRUE(collect_error.empty()); - CHECK_TRUE(operations.size() == 2); - CHECK_TRUE(operations[0].resource_key != operations[1].resource_key); - - auto first = std::async(std::launch::async, operations[0].operation); - auto second = std::async(std::launch::async, operations[1].operation); - CHECK_TRUE(blocked_stop_entered_future.wait_for(100ms) == - std::future_status::ready); - CHECK_TRUE(peer_stop_entered_future.wait_for(100ms) == - std::future_status::ready); - release_blocked_stop.set_value(); - CHECK_TRUE(first.get().success); - CHECK_TRUE(second.get().success); - CHECK_TRUE(operations[0].operation().success); - CHECK_TRUE(operations[1].operation().success); - CHECK_TRUE(blocked_cancel_calls.load() == 1); - CHECK_TRUE(blocked_stop_calls.load() == 1); - CHECK_TRUE(peer_cancel_calls.load() == 1); - CHECK_TRUE(peer_stop_calls.load() == 1); - CHECK_TRUE(parallel_coordinator.finishStopAll( - parallel_ticket, true)); - } - - { - MotorActivityCoordinator exception_coordinator; - std::atomic cancel_calls_for_exception{0}; - std::atomic stop_calls_after_exception{0}; - auto throwing = exception_coordinator.registerControl( - [&] { - ++cancel_calls_for_exception; - throw std::runtime_error("cancel failed"); - }, - [&] { - ++stop_calls_after_exception; - throw std::runtime_error("quick-stop failed"); - return false; - }, - [] { return true; }, - "throwing motor"); - CHECK_TRUE(throwing); - const auto exception_ticket = - exception_coordinator.beginStopAll(true); - std::vector operations; - CHECK_TRUE(exception_coordinator.collectStopOperations( - exception_ticket, operations)); - CHECK_TRUE(operations.size() == 1); - const auto first_result = operations[0].operation(); - const auto cached_result = operations[0].operation(); - CHECK_TRUE(!first_result.success); - CHECK_TRUE(first_result.detail.find("cancel failed") != - std::string::npos); - CHECK_TRUE(first_result.detail.find("quick-stop failed") != - std::string::npos); - CHECK_TRUE(cached_result.detail == first_result.detail); - CHECK_TRUE(cancel_calls_for_exception.load() == 1); - CHECK_TRUE(stop_calls_after_exception.load() == 1); - CHECK_TRUE(!exception_coordinator.finishStopAll( - exception_ticket, false)); - } - - { - cmvr::service::DeferredStopOperation detached_operation; - MotorActivityCoordinator::Registration retained_registration; - std::atomic detached_cancel_calls{0}; - std::atomic detached_stop_calls{0}; - { - MotorActivityCoordinator ephemeral_coordinator; - retained_registration = ephemeral_coordinator.registerControl( - [&] { ++detached_cancel_calls; }, - [&] { - ++detached_stop_calls; - return true; - }, - [] { return true; }, - "detached motor"); - const auto ticket = - ephemeral_coordinator.beginStopAll(true); - std::vector operations; - CHECK_TRUE(ephemeral_coordinator.collectStopOperations( - ticket, operations)); - CHECK_TRUE(operations.size() == 1); - detached_operation = std::move(operations[0]); - } - CHECK_TRUE(detached_operation.operation().success); - CHECK_TRUE(detached_cancel_calls.load() == 1); - CHECK_TRUE(detached_stop_calls.load() == 1); - retained_registration.reset(); - } - - std::atomic stop_succeeds{false}; - auto failing = coordinator.registerControl( - [] {}, - [&] { return stop_succeeds.load(std::memory_order_acquire); }, - [] { return true; }, - "failing motor"); - CHECK_TRUE(failing); - - const auto failed_ticket = coordinator.beginStopAll(); - CHECK_TRUE(failed_ticket.valid()); - std::string error; - CHECK_TRUE(!coordinator.stopAndWait(failed_ticket, 100ms, &error)); - CHECK_TRUE(!error.empty()); - CHECK_TRUE(!coordinator.finishStopAll(failed_ticket, false)); - CHECK_TRUE(!coordinator.lockAdmission().accepting()); - - stop_succeeds.store(true, std::memory_order_release); - const auto recovery_ticket = coordinator.beginStopAll(); - CHECK_TRUE(recovery_ticket.valid()); - CHECK_TRUE(coordinator.stopAndWait(recovery_ticket, 1s, &error)); - CHECK_TRUE(coordinator.finishStopAll(recovery_ticket, true)); - CHECK_TRUE(coordinator.lockAdmission().accepting()); - - { - MotorActivityCoordinator detailed_coordinator; - const auto first = detailed_coordinator.beginStopAll(true); - const auto second = detailed_coordinator.beginStopAll(true); - const auto first_result = - detailed_coordinator.finishStopAllDetailed(first, true); - CHECK_TRUE(first_result.ticket_consumed); - CHECK_TRUE(first_result.participant_stopped); - CHECK_TRUE(!first_result.admission_resumed); - const auto second_result = - detailed_coordinator.finishStopAllDetailed(second, true); - CHECK_TRUE(second_result.ticket_consumed); - CHECK_TRUE(second_result.participant_stopped); - CHECK_TRUE(second_result.admission_resumed); - CHECK_TRUE(detailed_coordinator.lockAdmission().accepting()); - } - - std::cout << "motor_activity_coordinator_test: PASS\n"; - return 0; -} diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp new file mode 100644 index 00000000..1f8d160f --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -0,0 +1,752 @@ +#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/server/tests/grpc_arm_client_test.cpp b/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp similarity index 100% rename from cmvr-es/service/grpc/server/tests/grpc_arm_client_test.cpp rename to cmvr-es/service/grpc/src/grpc_arm_client_test.cpp diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp new file mode 100644 index 00000000..60dc3a26 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -0,0 +1,573 @@ +#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/server/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp similarity index 74% rename from cmvr-es/service/grpc/server/src/grpc_camera_service.cpp rename to cmvr-es/service/grpc/src/grpc_camera_service.cpp index 63633def..08bc739c 100644 --- a/cmvr-es/service/grpc/server/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -1,10 +1,5 @@ #include "common/base/logging/logger.h" -#include "manager/media_source_manager/include/device_media_source_adapter.h" -#include "service/grpc/server/include/camera_operational_activity_registry.h" -#include "service/grpc/server/include/camera_ptz_activity_registry.h" -#include "service/grpc/server/include/grpc_command_transaction.h" -#include "service/grpc/server/include/media_activity_coordinator.h" -#include "service/grpc/server/include/grpc_security.h" +#include "manager/media_source_hub/include/device_media_source_adapter.h" // // Created by xtkuang on 2025/6/1. // @@ -17,7 +12,6 @@ #include #include #include -#include using namespace std; using namespace cmvr::service; @@ -26,6 +20,7 @@ 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()); @@ -74,24 +69,14 @@ bool toPtzCommand(cmvr::api::ControlPtzCommand_Command command, PtzCommand& out) } // The legacy depth/RGBD RPCs acquire the camera's shared producer directly -// instead of going through MediaSourceManager. Keep that lease exception-safe: +// 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: - CameraStreamingLease( - std::shared_ptr camera, - const MediaActivityCoordinator::Session& session, - cmvr::safety::SafetyManager& coordinator) + explicit CameraStreamingLease(std::shared_ptr camera) : camera_(std::move(camera)) { - (void)session.runIfCurrent([this, &coordinator] { - auto dispatch = cmvr::media::beginMediaSourceStartDispatch( - coordinator, camera_ ? camera_->id() : std::string{}); - if (!dispatch.acquired()) { - return; - } - active_ = camera_ && camera_->startStreaming(); - }); + active_ = camera_ && camera_->startStreaming(); } ~CameraStreamingLease() { @@ -117,41 +102,16 @@ private: std::shared_ptr camera_; bool active_{false}; }; - -grpc::Status mediaStoppedStatus() -{ - return grpc::Status( - grpc::StatusCode::CANCELLED, - "Media activity stopped by StopAll"); -} - -template -grpc::Status rejectStreamDuringStopAll(StreamT* stream) -{ - FeedbackT response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message( - "Media activities are temporarily paused by StopAll"); - setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - stream->Write(response); - return grpc::Status::OK; -} } gRPCCameraServiceImpl::gRPCCameraServiceImpl( - CameraStreamLowLatencyConfig stream_config, - std::shared_ptr security_gateway) + CameraStreamLowLatencyConfig stream_config) : dmgr_(DeviceManager::getInstance()), - stream_config_(stream_config), - security_gateway_(security_gateway - ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()) {} + stream_config_(stream_config) {} grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.CameraService/GetStatus"); try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetStatus): id=" << dev_id; @@ -185,107 +145,56 @@ grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.CameraService/StartCamera", request, response, - [this, request, response](GrpcCommandTransaction& command) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } + 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); } - CameraOperationalActivityRegistry::DispatchResult dispatch = - CameraOperationalActivityRegistry::DispatchResult::DeviceFailure; - const bool start_allowed = media_session.runIfCurrent([&] { - dispatch = globalCameraOperationalActivityRegistry().start( - dev_id, dev, nullptr, - [&command] { return command.beginDispatch(); }); - }); - if (!start_allowed) { - return failResponse( - response, "Camera start was canceled by StopAll"); - } - if (dispatch == - CameraOperationalActivityRegistry::DispatchResult:: - RejectedByDispatchFence) { - return command.dispatchStatus(); - } - if (dispatch == - CameraOperationalActivityRegistry::DispatchResult:: - RejectedByStopAll) { - return failResponse( - response, "Camera start was canceled by StopAll"); - } - if (dispatch == - CameraOperationalActivityRegistry::DispatchResult::DeviceFailure) { + 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.CameraService/StopCamera", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - const auto dispatch = - globalCameraOperationalActivityRegistry().stopLifecycle( - dev_id, dev, - [&command] { return command.beginDispatch(); }); - if (dispatch == - CameraOperationalActivityRegistry::DispatchResult:: - RejectedByStopAll) { - return failResponse( - response, "Camera control is temporarily paused by StopAll"); - } - if (dispatch == - CameraOperationalActivityRegistry::DispatchResult:: - RejectedByDispatchFence) { - return command.dispatchStatus(); - } - if (dispatch == - CameraOperationalActivityRegistry::DispatchResult::DeviceFailure) { + if (!dev->stop()) { return failResponse(response, "Failed to stop camera: " + dev_id); } - globalCameraPtzActivityRegistry().markCameraStopped(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) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.CameraService/GetRGBImage"); - auto media_session = globalMediaActivityCoordinator().beginSession( - [context] { - if (context) { - context->TryCancel(); - } - }); - if (!media_session) { - return failResponse( - response, "Camera capture is temporarily paused by StopAll"); - } try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id; @@ -295,11 +204,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, return failResponse(response, "Camera device not found: " + dev_id); } Rs2Intrinsics intrinsics = {0}; - if (!media_session.runIfCurrent( - [&] { dev->getRGBImage(image, intrinsics); })) { - return failResponse( - response, "Camera capture was canceled by StopAll"); - } + dev->getRGBImage(image,intrinsics); if (image.empty()) { return failResponse(response, "Camera returned an empty RGB image: " + dev_id); } @@ -345,18 +250,6 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, const api::GetDepthImageCommand_Request* request, api::GetDepthImageCommand_Feedback* response) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.CameraService/GetDepthImage"); - auto media_session = globalMediaActivityCoordinator().beginSession( - [context] { - if (context) { - context->TryCancel(); - } - }); - if (!media_session) { - return failResponse( - response, "Camera capture is temporarily paused by StopAll"); - } try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImage): id=" << dev_id; @@ -366,11 +259,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, return failResponse(response, "Camera device not found: " + dev_id); } Rs2Intrinsics intrinsics = {0}; - if (!media_session.runIfCurrent( - [&] { dev->getDepthImage(image, intrinsics); })) { - return failResponse( - response, "Camera capture was canceled by StopAll"); - } + dev->getDepthImage(image,intrinsics); if (image.empty()) { return failResponse(response, "Camera returned an empty depth image: " + dev_id); } @@ -420,18 +309,6 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.CameraService/GetRGBDImages"); - auto media_session = globalMediaActivityCoordinator().beginSession( - [context] { - if (context) { - context->TryCancel(); - } - }); - if (!media_session) { - return failResponse( - response, "Camera capture is temporarily paused by StopAll"); - } try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImages): id=" << dev_id; @@ -441,14 +318,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, return failResponse(response, "Camera device not found: " + dev_id); } Rs2Intrinsics intrinsics = {0}; - if (!media_session.runIfCurrent( - [&] { - dev->getRGBDImages( - color_image, depth_image, intrinsics); - })) { - return failResponse( - response, "Camera capture was canceled by StopAll"); - } + dev->getRGBDImages(color_image,depth_image, intrinsics); if (color_image.empty()) { return failResponse(response, "Camera returned an empty RGB image: " + dev_id); } @@ -517,90 +387,53 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.CameraService/StartRecording", request, response, - [this, request, response](GrpcCommandTransaction& command) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } + 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); } - bool dispatch_allowed = false; - if (!media_session.runIfCurrent([&] { - dispatch_allowed = command.beginDispatch(); - if (!dispatch_allowed) { - return; - } - dev->startRecording(request->video_path()); - })) { - return failResponse( - response, "Camera recording start was canceled by StopAll"); - } - if (!dispatch_allowed) { - return command.dispatchStatus(); - } + 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.CameraService/StopRecording", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } 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) { - if (!request) { - return grpc::Status( - grpc::StatusCode::INTERNAL, "ControlPtz request is null"); - } - const auto registered = defaultGrpcMethodPolicyRegistry().find( - "/cmvr.api.CameraService/ControlPtz"); - if (!registered.has_value()) { - return grpc::Status( - grpc::StatusCode::INTERNAL, - "ControlPtz method policy is not registered"); - } - auto effective_policy = *registered; - if (request->action() == api::ControlPtzCommand_Action_STOP) { - effective_policy.access = GrpcAccessClass::Stop; - effective_policy.command_intent = safety::CommandIntent::Stop; - effective_policy.safety_lane = true; - } - - return executeServerDerivedGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.CameraService/ControlPtz", std::move(effective_policy), - request, response, - [this, request, response](GrpcCommandTransaction& command_tx) { + try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (ControlPtz): id=" << dev_id << ", command=" << request->command() @@ -621,22 +454,7 @@ grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context, return failResponse(response, "Invalid PTZ action"); } const bool stop = request->action() == api::ControlPtzCommand_Action_STOP; - const auto dispatch = globalCameraPtzActivityRegistry().control( - dev_id, - dev, - command, - stop, - static_cast(request->speed()), - [&command_tx] { return command_tx.beginDispatch(); }); - if (dispatch == CameraPtzActivityRegistry::DispatchResult::RejectedByStopAll) { - return failResponse( - response, "PTZ control is temporarily paused by StopAll"); - } - if (dispatch == - CameraPtzActivityRegistry::DispatchResult::RejectedByDispatchFence) { - return command_tx.dispatchStatus(); - } - if (dispatch == CameraPtzActivityRegistry::DispatchResult::DeviceFailure) { + if (!dev->controlPtz(command, stop, static_cast(request->speed()))) { CameraState state{}; dev->getState(state); const std::string error_message = @@ -647,28 +465,23 @@ grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context, 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){ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.CameraService/GetDepthImageStream"); - auto media_session = globalMediaActivityCoordinator().beginSession( - [context] { context->TryCancel(); }); - if (!media_session) { - return rejectStreamDuringStopAll< - api::GetDepthImageStreamCommand_Feedback>(stream); - } try { //读取首次传递的数据，获取设备id api::GetDepthImageStreamCommand_Request request; if (!stream->Read(&request)) { - return media_session.cancelled() - ? mediaStoppedStatus() - : grpc::Status::OK; + return grpc::Status::OK; } string dev_id = request.header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start,id=" << dev_id; @@ -681,8 +494,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con stream->Write(response); return grpc::Status::OK; } - CameraStreamingLease stream_lease( - dev, media_session, dmgr_.safetyManager()); + CameraStreamingLease stream_lease(dev); if (!stream_lease) { api::GetDepthImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); @@ -696,7 +508,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con size_t index = 0; while (true) { - if (media_session.cancelled() || context->IsCancelled()) + if (context->IsCancelled()) { CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; break; @@ -705,7 +517,6 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con api::GetDepthImageStreamCommand_Feedback response; cmvr::device::StreamFrameData frame_data; if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) && - !media_session.cancelled() && !frame_data.depthFrame.empty()) { response.mutable_header()->set_success(true); setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); @@ -732,14 +543,9 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con } } CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): end,id=" << dev_id; - return media_session.cancelled() - ? mediaStoppedStatus() - : grpc::Status::OK; + return grpc::Status::OK; } catch (const exception &e) { - if (media_session.cancelled()) { - return mediaStoppedStatus(); - } api::GetDepthImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); response.mutable_header()->set_error_message(e.what()); @@ -750,22 +556,11 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con } grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* context , grpc::ServerReaderWriter* stream){ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.CameraService/GetRGBDImagesStream"); - auto media_session = globalMediaActivityCoordinator().beginSession( - [context] { context->TryCancel(); }); - if (!media_session) { - return rejectStreamDuringStopAll< - api::GetRGBDImagesStreamCommand_Feedback>(stream); - } try { //读取首次传递的数据，获取设备id api::GetRGBDImagesStreamCommand_Request request; if (!stream->Read(&request)) { - return media_session.cancelled() - ? mediaStoppedStatus() - : grpc::Status::OK; + return grpc::Status::OK; } string dev_id = request.header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start,id=" << dev_id; @@ -778,8 +573,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con stream->Write(response); return grpc::Status::OK; } - CameraStreamingLease stream_lease( - dev, media_session, dmgr_.safetyManager()); + CameraStreamingLease stream_lease(dev); if (!stream_lease) { api::GetRGBDImagesStreamCommand_Feedback response; response.mutable_header()->set_success(false); @@ -793,7 +587,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con size_t index = 0; while (true) { - if (media_session.cancelled() || context->IsCancelled()) + if (context->IsCancelled()) { CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBDImagesStream) context is cancelled,id=" << dev_id; break; @@ -802,7 +596,6 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con api::GetRGBDImagesStreamCommand_Feedback response; cmvr::device::StreamFrameData frame_data; if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) && - !media_session.cancelled() && !frame_data.rgbFrame.empty() && !frame_data.depthFrame.empty()) { response.mutable_header()->set_success(true); @@ -836,14 +629,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con } } CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id; - return media_session.cancelled() - ? mediaStoppedStatus() - : grpc::Status::OK; + return grpc::Status::OK; } catch (const exception &e) { - if (media_session.cancelled()) { - return mediaStoppedStatus(); - } api::GetRGBDImagesStreamCommand_Feedback response; response.mutable_header()->set_success(false); response.mutable_header()->set_error_message(e.what()); @@ -853,22 +641,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con } } grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream){ - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.CameraService/GetRGBImageStream"); - auto media_session = globalMediaActivityCoordinator().beginSession( - [context] { context->TryCancel(); }); - if (!media_session) { - return rejectStreamDuringStopAll< - api::GetRGBImageStreamCommand_Feedback>(stream); - } try { //读取首次传递的数据，获取设备id api::GetRGBImageStreamCommand_Request request; if (!stream->Read(&request)) { - return media_session.cancelled() - ? mediaStoppedStatus() - : grpc::Status::OK; + return grpc::Status::OK; } string dev_id = request.header().device_id(); CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id @@ -883,33 +660,20 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte stream->Write(response); return grpc::Status::OK; } - auto& media_hub = cmvr::media::globalMediaSourceManager(); + auto& media_hub = cmvr::media::globalMediaSourceHub(); const std::string track_id = cmvr::media::cameraColorTrackId(dev_id); - bool source_ready = false; - const bool source_setup_allowed = media_session.runIfCurrent([&] { - source_ready = cmvr::media::ensureCameraMediaSource(media_hub, dev); - }); - if (!source_setup_allowed || !source_ready) { + if (!cmvr::media::ensureCameraMediaSource(media_hub, dev)) { api::GetRGBImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message( - media_session.cancelled() - ? "Camera stream start was canceled by StopAll" - : "Failed to register camera media source: " + dev_id); + 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 source_dispatch = cmvr::media::beginMediaSourceStartDispatch( - dmgr_.safetyManager(), dev_id); - auto subscription = source_dispatch.acquired() - ? media_hub.subscribe( + auto subscription = media_hub.subscribe( track_id, - cmvr::media::MediaSourceManager::StartPosition::NEXT_PUBLISHED, - [context, &media_session] { - return context->IsCancelled() || media_session.cancelled(); - }) - : cmvr::media::MediaSourceManager::Subscription{}; + cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED, + [context] { return context->IsCancelled(); }); if (!subscription) { api::GetRGBImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); @@ -1002,7 +766,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte exit_reason = "client_eof"; break; } - if (media_session.cancelled() || context->IsCancelled()) + if (context->IsCancelled()) { exit_reason = "context_cancelled"; CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled" @@ -1014,9 +778,6 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte } const auto read = subscription.waitRead(std::chrono::milliseconds(100)); - if (media_session.cancelled()) { - break; - } if (!read || !read->value || read->value->empty()) { if (!subscription.valid()) { exit_reason = "subscription_invalid"; @@ -1106,9 +867,8 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte static_cast(std::numeric_limits::max())))); const auto write_started = std::chrono::steady_clock::now(); - if (media_session.cancelled() || !stream->Write(response)) { - exit_reason = media_session.cancelled() - ? "media_session_cancelled" : "write_failed"; + if (!stream->Write(response)) { + exit_reason = "write_failed"; CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed" << ", id=" << dev_id << ", peer=" << context->peer() @@ -1148,14 +908,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte << ", client_eof=" << client_eof_requested.load() << ", request_stream_closed=" << request_stream_closed.load() << ", control_requests=" << control_requests_read.load(); - return media_session.cancelled() - ? mediaStoppedStatus() - : grpc::Status::OK; + return grpc::Status::OK; } catch (const exception &e) { - if (media_session.cancelled()) { - return mediaStoppedStatus(); - } api::GetRGBImageStreamCommand_Feedback response; response.mutable_header()->set_success(false); response.mutable_header()->set_error_message(e.what()); diff --git a/cmvr-es/service/grpc/server/src/grpc_dexhand_service.cpp b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp similarity index 60% rename from cmvr-es/service/grpc/server/src/grpc_dexhand_service.cpp rename to cmvr-es/service/grpc/src/grpc_dexhand_service.cpp index 0e69af20..84bf5003 100644 --- a/cmvr-es/service/grpc/server/src/grpc_dexhand_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_dexhand_service.cpp @@ -5,21 +5,13 @@ #include "../include/grpc_dexhand_service.h" -#include #include #include -#include #include #include -#include #include #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h" -#include "manager/control_authority_manager/include/control_authority_manager.h" -#include "service/grpc/server/include/grpc_command_transaction.h" -#include "service/grpc/server/include/media_activity_coordinator.h" -#include "service/grpc/server/include/grpc_security.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" using namespace std; using namespace cmvr::service; @@ -122,134 +114,13 @@ bool applyFreedomValues(const FreedomCollection& freedoms, 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; } -class ScopedDexHandControlLease final { -public: - ScopedDexHandControlLease(const std::string& device_id, - const char* operation) - : manager_(cmvr::control::ControlAuthorityManager::instance()) { - static std::atomic sequence{0U}; - const std::string owner = - std::string("grpc-dexhand-unary:") + operation + ":" + - std::to_string( - sequence.fetch_add(1U, std::memory_order_relaxed) + 1U); - const auto ttl = std::chrono::duration_cast< - cmvr::control::ControlAuthorityManager::Duration>( - std::chrono::hours(24)); - - auto admission = globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting()) { - rejected_by_stop_all_ = true; - detail_ = "System StopAll admission is closed"; - return; - } - admission_generation_ = admission.generation(); - - auto acquired = manager_.tryAcquire(device_id, owner, ttl); - acquired_ = acquired.acquired; - token_ = std::move(acquired.token); - detail_ = std::move(acquired.detail); - } - - ~ScopedDexHandControlLease() { - manager_.release(token_); - } - - bool acquired() const noexcept { return acquired_; } - bool rejectedByStopAll() const noexcept { - return rejected_by_stop_all_; - } - const std::string& detail() const noexcept { return detail_; } - - bool admissionCurrent() const { - auto admission = globalStopAllAdmissionGate().lockAdmission(); - return admission.accepting() && - admission.generation() == admission_generation_; - } - - cmvr::control::ControlDispatchGuard tryBeginDispatch() { - auto admission = globalStopAllAdmissionGate().lockAdmission(); - if (!admission.accepting() || - admission.generation() != admission_generation_) { - return {}; - } - return manager_.tryBeginDispatch(token_); - } - -private: - cmvr::control::ControlAuthorityManager& manager_; - cmvr::control::ControlLeaseToken token_; - std::string detail_; - bool acquired_{false}; - bool rejected_by_stop_all_{false}; - std::uint64_t admission_generation_{0U}; -}; - -template -grpc::Status failControlAdmission( - ResponseT* response, - const std::string& device_id, - const ScopedDexHandControlLease& lease) { - if (lease.rejectedByStopAll()) { - return failResponse( - response, - "DexHand control is temporarily paused by StopAll: " + device_id); - } - return failResponse( - response, - "DexHand control is leased by another active operation: " + - device_id + - (lease.detail().empty() ? "" : " (" + lease.detail() + ")")); -} - -template -grpc::Status failControlDispatch( - ResponseT* response, - const std::string& device_id, - const ScopedDexHandControlLease& lease) { - if (!lease.admissionCurrent()) { - return failResponse( - response, - "DexHand control was preempted by StopAll: " + device_id); - } - return failResponse( - response, - "DexHand control lease was preempted before device dispatch: " + - device_id); -} - -template -bool dispatchDexHandCommand( - ResponseT* response, - const std::string& device_id, - const std::shared_ptr& dev, - ScopedDexHandControlLease& lease, - GrpcCommandTransaction& command, - Operation&& operation) { - auto dispatch = lease.tryBeginDispatch(); - if (!dispatch.acquired()) { - (void)failControlDispatch(response, device_id, lease); - return false; - } - if (!command.beginDispatch()) { - return false; - } - if (!dev->resumeOperationalActivity()) { - (void)failResponse( - response, - "DexHand operational activity could not be resumed: " + - device_id); - return false; - } - std::forward(operation)(); - return true; -} - std::vector readCurrentAngles(const std::shared_ptr& dev) { DexHandState state{}; dev->getState(state); @@ -310,20 +181,10 @@ void maybeConfigureRh56FullTactilePolling(const std::shared_ptr } // namespace -gRPCDexHandServiceImpl::gRPCDexHandServiceImpl() - : gRPCDexHandServiceImpl(makeDefaultGrpcSecurityGateway()) {} - -gRPCDexHandServiceImpl::gRPCDexHandServiceImpl( - std::shared_ptr security_gateway) - : dmgr_(DeviceManager::getInstance()), - security_gateway_(security_gateway - ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()) {} +gRPCDexHandServiceImpl::gRPCDexHandServiceImpl(): dmgr_(DeviceManager::getInstance()) {} grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetDexHandStateCommand_Request* request, api::GetDexHandStateCommand_Feedback* response) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.DexHandService/GetStatus"); try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (GetStatus): id=" << dev_id; @@ -366,20 +227,13 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context, grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context , const cmvr::api::SetDexHandPositionsCommand_Request* request , cmvr::api::SetDexHandPositionsCommand_Feedback* response) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.DexHandService/SetDexHandPos", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - ScopedDexHandControlLease control_lease(dev_id, "SetDexHandPos"); - if (!control_lease.acquired()) { - return failControlAdmission(response, dev_id, control_lease); - } if (respondUnsupportedForRh56(dev, "SetDexHandPos", "Use SetDexHandAngle for RH56 joint commands.", @@ -392,42 +246,31 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context if (!applyFreedomValues(request->values(), DEXHAND_MAX_POSITION, finger_joint_targets, &error_message)) { return failResponse(response, error_message); } - if (!dispatchDexHandCommand( - response, - dev_id, - dev, - control_lease, - command, - [&] { dev->setPositions(finger_joint_targets); })) { - return command.dispatchStatus().ok() - ? grpc::Status::OK - : command.dispatchStatus(); - } + 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.DexHandService/SetDexHandAngle", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - ScopedDexHandControlLease control_lease(dev_id, "SetDexHandAngle"); - if (!control_lease.acquired()) { - return failControlAdmission(response, dev_id, control_lease); - } if (const auto rh56 = std::dynamic_pointer_cast(dev)) { std::vector finger_joint_targets = readCurrentAngles(dev); @@ -435,34 +278,14 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex if (!applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets, &error_message)) { return failResponse(response, error_message); } - if (!dispatchDexHandCommand( - response, - dev_id, - dev, - control_lease, - command, - [&] { rh56->setAngles(finger_joint_targets); })) { - return command.dispatchStatus().ok() - ? grpc::Status::OK - : command.dispatchStatus(); - } + 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); } - if (!dispatchDexHandCommand( - response, - dev_id, - dev, - control_lease, - command, - [&] { dev->setAngles(finger_joint_targets); })) { - return command.dispatchStatus().ok() - ? grpc::Status::OK - : command.dispatchStatus(); - } + dev->setAngles(finger_joint_targets); } response->mutable_header()->set_success(true); @@ -470,26 +293,25 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.DexHandService/SetDexHandForce", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - ScopedDexHandControlLease control_lease(dev_id, "SetDexHandForce"); - if (!control_lease.acquired()) { - return failControlAdmission(response, dev_id, control_lease); - } if (respondUnsupportedForRh56(dev, "SetDexHandForce", "RH56DFTPDexhand currently exposes angle and tactile APIs only.", @@ -502,42 +324,31 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex if (!applyFreedomValues(request->values(), DEXHAND_MAX_FORCE, finger_joint_targets, &error_message)) { return failResponse(response, error_message); } - if (!dispatchDexHandCommand( - response, - dev_id, - dev, - control_lease, - command, - [&] { dev->setForce(finger_joint_targets); })) { - return command.dispatchStatus().ok() - ? grpc::Status::OK - : command.dispatchStatus(); - } + 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.DexHandService/SetDexHandSpeed", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - ScopedDexHandControlLease control_lease(dev_id, "SetDexHandSpeed"); - if (!control_lease.acquired()) { - return failControlAdmission(response, dev_id, control_lease); - } if (respondUnsupportedForRh56(dev, "SetDexHandSpeed", "RH56DFTPDexhand currently exposes angle and tactile APIs only.", @@ -550,43 +361,31 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex if (!applyFreedomValues(request->values(), DEXHAND_MAX_SPEED, finger_joint_targets, &error_message)) { return failResponse(response, error_message); } - if (!dispatchDexHandCommand( - response, - dev_id, - dev, - control_lease, - command, - [&] { dev->setVelocities(finger_joint_targets); })) { - return command.dispatchStatus().ok() - ? grpc::Status::OK - : command.dispatchStatus(); - } + 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.DexHandService/SetDexHandPresetAct", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - ScopedDexHandControlLease control_lease( - dev_id, "SetDexHandPresetAct"); - if (!control_lease.acquired()) { - return failControlAdmission(response, dev_id, control_lease); - } if (respondUnsupportedForRh56(dev, "SetDexHandPresetAct", "RH56DFTPDexhand currently exposes angle and tactile APIs only.", @@ -595,38 +394,24 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co } auto presetActId = request->presetactid(); - if (!dispatchDexHandCommand( - response, - dev_id, - dev, - control_lease, - command, - [&] { dev->setPresetAct(presetActId); })) { - return command.dispatchStatus().ok() - ? grpc::Status::OK - : command.dispatchStatus(); - } + 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) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.DexHandService/GetSensorData"); - - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, - "DexHand sensor activity is temporarily paused by StopAll"); - } try { string dev_id = request->header().device_id(); @@ -635,28 +420,8 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context if (!dev) { return failResponse(response, "DexHand device not found: " + dev_id); } - - bool resumed = false; - std::vector sensor_data; - if (!media_session.runIfCurrent([&] { - resumed = dev->resumeOperationalActivity(); - if (!resumed) { - return; - } - maybeConfigureRh56FullTactilePolling(dev); - sensor_data = dev->getSensorData(); - })) { - return failResponse( - response, - "DexHand sensor activity was preempted by StopAll: " + - dev_id); - } - if (!resumed) { - return failResponse( - response, - "DexHand sensor activity could not be resumed: " + dev_id); - } - appendSensorData(sensor_data, response); + 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 @@ -674,21 +439,6 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* context , grpc::ServerReaderWriter* stream) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, - "/cmvr.api.DexHandService/GetSensorDataStream"); - auto media_session = globalMediaActivityCoordinator().beginSession( - [context] { - if (context) { - context->TryCancel(); - } - }); - if (!media_session) { - return grpc::Status( - grpc::StatusCode::UNAVAILABLE, - "DexHand sensor stream is temporarily paused by StopAll"); - } - try { api::GetSensorDataStreamCommand_Request request; if (!stream->Read(&request)) { @@ -706,46 +456,16 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co stream->Write(response); return grpc::Status::OK; } - - bool resumed = false; - if (!media_session.runIfCurrent([&] { - resumed = dev->resumeOperationalActivity(); - if (resumed) { - maybeConfigureRh56FullTactilePolling(dev); - } - })) { - return grpc::Status( - grpc::StatusCode::CANCELLED, - "DexHand sensor stream was preempted by StopAll"); - } - if (!resumed) { - api::GetSensorDataStreamCommand_Feedback response; - response.mutable_header()->set_success(false); - response.mutable_header()->set_error_message( - "DexHand sensor activity could not be resumed: " + 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 (!media_session.cancelled() && - !(context && context->IsCancelled())) + while (!context->IsCancelled()) { api::GetSensorDataStreamCommand_Feedback response; - std::vector sensor_data; - if (!media_session.runIfCurrent( - [&] { sensor_data = dev->getSensorData(); })) { - break; - } - appendSensorData(sensor_data, &response); + appendSensorData(dev->getSensorData(), &response); response.mutable_header()->set_success(true); setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - if (media_session.cancelled() || - (context && context->IsCancelled())) { - break; - } if (!stream->Write(response)) { CMVR_LOG(INFO) << "[gRPCDexHandServiceImpl] (stream->Write) failed,id=" << dev_id; break; diff --git a/cmvr-es/service/grpc/src/grpc_head_service.cpp b/cmvr-es/service/grpc/src/grpc_head_service.cpp new file mode 100644 index 00000000..6ad3aff8 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_head_service.cpp @@ -0,0 +1,475 @@ +#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/server/tests/grpc_hlc_client_test.cpp b/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp similarity index 100% rename from cmvr-es/service/grpc/server/tests/grpc_hlc_client_test.cpp rename to cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp new file mode 100644 index 00000000..5edc18b0 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp @@ -0,0 +1,87 @@ +// +// 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/server/src/grpc_microphone_service.cpp b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp similarity index 65% rename from cmvr-es/service/grpc/server/src/grpc_microphone_service.cpp rename to cmvr-es/service/grpc/src/grpc_microphone_service.cpp index b18e9572..63f07cb2 100644 --- a/cmvr-es/service/grpc/server/src/grpc_microphone_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_microphone_service.cpp @@ -1,13 +1,9 @@ #include "common/base/logging/logger.h" -#include "manager/media_source_manager/include/device_media_source_adapter.h" -#include "service/grpc/server/include/grpc_command_transaction.h" -#include "service/grpc/server/include/media_activity_coordinator.h" -#include "service/grpc/server/include/grpc_security.h" +#include "manager/media_source_hub/include/device_media_source_adapter.h" #include #include #include #include -#include // // Created by linbo on 2025/6/13. // Created by xtkuang on 2025/6/13. @@ -21,35 +17,19 @@ 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; } -grpc::Status mediaStoppedStatus() -{ - return grpc::Status( - grpc::StatusCode::CANCELLED, - "Media activity stopped by StopAll"); } -} - -gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl() - : gRPCMicroPhoneServiceImpl(makeDefaultGrpcSecurityGateway()) {} - -gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl( - std::shared_ptr security_gateway) - : dmgr_(DeviceManager::getInstance()), - security_gateway_(security_gateway - ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()) {} +gRPCMicroPhoneServiceImpl::gRPCMicroPhoneServiceImpl(): dmgr_(DeviceManager::getInstance()) {} grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetMicStateCommand_Request* request, api::GetMicStateCommand_Feedback* response) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.MicPhoneService/GetStatus"); try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetStatus): id=" << dev_id; @@ -83,150 +63,103 @@ grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context, grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context, const api::StartMicRecordingCommand_Request* request, api::StartMicRecordingCommand_Feedback* response) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MicPhoneService/StartRecord", request, response, - [this, request, response](GrpcCommandTransaction& command) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } + 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); } - bool started = false; - bool dispatch_allowed = false; - const bool start_allowed = media_session.runIfCurrent([&] { - dispatch_allowed = command.beginDispatch(); - if (!dispatch_allowed) { - return; - } - started = dev->start(); - if (started) { - dev->startRecording(request->file_path()); - } - }); - if (!start_allowed) { - return failResponse( - response, "Microphone recording start was canceled by StopAll"); - } - if (!dispatch_allowed) { - return command.dispatchStatus(); - } - if (!started) { + if (!dev->start()) { return failResponse(response, "Failed to start microphone: " + dev_id); } + dev->startRecording(request->file_path()); 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MicPhoneService/StopRecord", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MicPhoneService/PauseRecord", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MicPhoneService/ResumeRecord", request, response, - [this, request, response](GrpcCommandTransaction& command) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } + 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); } - bool dispatch_allowed = false; - if (!media_session.runIfCurrent([&] { - dispatch_allowed = command.beginDispatch(); - if (dispatch_allowed) { - dev->resume(); - } - })) { - return failResponse( - response, "Microphone recording resume was canceled by StopAll"); - } - if (!dispatch_allowed) { - return command.dispatchStatus(); - } + 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) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.MicPhoneService/StreamAudio"); - auto media_session = globalMediaActivityCoordinator().beginSession( - [context] { context->TryCancel(); }); - if (!media_session) { - api::StreamMicAudioCommand_Feedback feedback; - feedback.mutable_header()->set_success(false); - feedback.mutable_header()->set_error_message( - "Media activities are temporarily paused by StopAll"); - setCurrentTimestamp(feedback.mutable_header()->mutable_timestamp()); - writer->Write(feedback); - return grpc::Status::OK; - } try { const string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (StreamAudio): id=" << dev_id; @@ -240,35 +173,21 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context return grpc::Status::OK; } - auto& media_hub = cmvr::media::globalMediaSourceManager(); + auto& media_hub = cmvr::media::globalMediaSourceHub(); const std::string track_id = cmvr::media::microphoneTrackId(dev_id); - bool source_ready = false; - const bool source_setup_allowed = media_session.runIfCurrent([&] { - source_ready = - cmvr::media::ensureMicrophoneMediaSource(media_hub, dev); - }); - if (!source_setup_allowed || !source_ready) { + if (!cmvr::media::ensureMicrophoneMediaSource(media_hub, dev)) { api::StreamMicAudioCommand_Feedback feedback; feedback.mutable_header()->set_success(false); - feedback.mutable_header()->set_error_message( - media_session.cancelled() - ? "Microphone stream start was canceled by StopAll" - : "Failed to register microphone media source: " + dev_id); + 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 source_dispatch = cmvr::media::beginMediaSourceStartDispatch( - dmgr_.safetyManager(), dev_id); - auto subscription = source_dispatch.acquired() - ? media_hub.subscribe( + auto subscription = media_hub.subscribe( track_id, - cmvr::media::MediaSourceManager::StartPosition::NEXT_PUBLISHED, - [context, &media_session] { - return context->IsCancelled() || media_session.cancelled(); - }) - : cmvr::media::MediaSourceManager::Subscription{}; + cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED, + [context] { return context->IsCancelled(); }); if (!subscription) { api::StreamMicAudioCommand_Feedback feedback; feedback.mutable_header()->set_success(false); @@ -278,11 +197,8 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context return grpc::Status::OK; } - while (!media_session.cancelled() && !context->IsCancelled()) { + while (!context->IsCancelled()) { const auto read = subscription.waitRead(std::chrono::milliseconds(100)); - if (media_session.cancelled()) { - break; - } if (!read || !read->value || read->value->empty()) { if (!subscription.valid()) { break; @@ -320,9 +236,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context feedback.mutable_header()->set_error_message( "Unsupported microphone stream codec: " + dev_id); feedback.clear_audio(); - if (!media_session.cancelled()) { - writer->Write(feedback); - } + writer->Write(feedback); break; } audio->set_pts(frame.pts); @@ -333,17 +247,12 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context sample_count, 0, std::numeric_limits::max()))); - if (media_session.cancelled() || !writer->Write(feedback)) { + if (!writer->Write(feedback)) { break; } } - return media_session.cancelled() - ? mediaStoppedStatus() - : grpc::Status::OK; + return grpc::Status::OK; } catch (const std::exception& error) { - if (media_session.cancelled()) { - return mediaStoppedStatus(); - } api::StreamMicAudioCommand_Feedback feedback; feedback.mutable_header()->set_success(false); feedback.mutable_header()->set_error_message(error.what()); @@ -355,32 +264,30 @@ grpc::Status gRPCMicroPhoneServiceImpl::StreamAudio(grpc::ServerContext* context grpc::Status gRPCMicroPhoneServiceImpl::SetVolume(grpc::ServerContext* context, const api::SetMicPhoneVolumeCommand_Request* request, api::SetMicPhoneVolumeCommand_Feedback* response) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.MicPhoneService/SetVolume", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } 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) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.MicPhoneService/GetVolume"); try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCMicroPhoneServiceImpl] (GetVolume): id=" << dev_id; diff --git a/cmvr-es/service/grpc/server/src/grpc_speaker_service.cpp b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp similarity index 51% rename from cmvr-es/service/grpc/server/src/grpc_speaker_service.cpp rename to cmvr-es/service/grpc/src/grpc_speaker_service.cpp index f9704fda..756fab83 100644 --- a/cmvr-es/service/grpc/server/src/grpc_speaker_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_speaker_service.cpp @@ -1,10 +1,5 @@ #include "common/base/logging/logger.h" -#include "service/grpc/server/include/grpc_command_transaction.h" -#include "service/grpc/server/include/media_activity_coordinator.h" -#include "service/grpc/server/include/grpc_security.h" #include -#include -#include // // Created by xtkuang on 2025/6/10. // @@ -18,6 +13,7 @@ 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()); @@ -50,68 +46,12 @@ AudioStreamFrameData fromProtoAudioData(const cmvr::api::AudioData& audio) { frame.nb_samples = audio.nb_samples(); return frame; } - -std::string speakerResourceKey(const std::string& device_id) -{ - return "speaker:" + device_id; } -grpc::Status mediaStoppedStatus() -{ - return grpc::Status( - grpc::StatusCode::CANCELLED, - "Media activity stopped by StopAll"); -} - -class SpeakerStreamingLease final { -public: - explicit SpeakerStreamingLease(std::shared_ptr speaker) - : speaker_(std::move(speaker)) {} - - ~SpeakerStreamingLease() - { - if (!armed_ || !speaker_) { - return; - } - try { - speaker_->stopStreaming(); - } catch (const std::exception& error) { - CMVR_LOG(ERROR) - << "[gRPCSpeakerServiceImpl] failed to release speaker " - "stream session: " - << error.what(); - } catch (...) { - CMVR_LOG(ERROR) - << "[gRPCSpeakerServiceImpl] failed to release speaker " - "stream session"; - } - } - - SpeakerStreamingLease(const SpeakerStreamingLease&) = delete; - SpeakerStreamingLease& operator=(const SpeakerStreamingLease&) = delete; - - void arm() noexcept { armed_ = true; } - -private: - std::shared_ptr speaker_; - bool armed_{false}; -}; -} - -gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl() - : gRPCSpeakerServiceImpl(makeDefaultGrpcSecurityGateway()) {} - -gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl( - std::shared_ptr security_gateway) - : dmgr_(DeviceManager::getInstance()), - security_gateway_(security_gateway - ? std::move(security_gateway) - : makeDefaultGrpcSecurityGateway()) {} +gRPCSpeakerServiceImpl::gRPCSpeakerServiceImpl(): dmgr_(DeviceManager::getInstance()) {} grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context, const api::GetSpeakerStateCommand_Request* request, api::GetSpeakerStateCommand_Feedback* response) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.SpeakerService/GetStatus"); try { string dev_id = request->header().device_id(); //CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetStatus): id=" << dev_id; @@ -146,68 +86,38 @@ grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context, grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request, api::PlayAudioCommand_Feedback* response) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.SpeakerService/PlayAudio", request, response, - [this, request, response](GrpcCommandTransaction& command) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } + 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); } - if (!media_session.claimExclusiveResource(speakerResourceKey(dev_id))) { - return failResponse( - response, "Speaker is already controlled by another media session: " + dev_id); - } - bool dispatch_allowed = false; - if (!media_session.runIfCurrent([&] { - dispatch_allowed = command.beginDispatch(); - if (dispatch_allowed) { - dev->play(request->audio_path()); - } - })) { - return failResponse( - response, "Speaker playback start was canceled by StopAll"); - } - if (!dispatch_allowed) { - return command.dispatchStatus(); - } + //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) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.SpeakerService/StreamAudio"); - auto media_session = globalMediaActivityCoordinator().beginSession( - [context] { context->TryCancel(); }); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } try { api::StreamSpeakerAudioCommand_Request request; std::shared_ptr dev; - std::unique_ptr stream_lease; - std::optional safety_session; std::string dev_id; while (reader->Read(&request)) { - if (media_session.cancelled()) { - return mediaStoppedStatus(); - } if (!dev) { dev_id = request.header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (StreamAudio): id=" << dev_id; @@ -215,88 +125,25 @@ grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, if (!dev) { return failResponse(response, "Speaker device not found: " + dev_id); } - if (!media_session.claimExclusiveResource( - speakerResourceKey(dev_id))) { - return failResponse( - response, - "Speaker is already controlled by another media session: " + dev_id); + if (!dev->start()) { + return failResponse(response, "Failed to start speaker: " + dev_id); } - stream_lease = std::make_unique(dev); - - const auto& header = request.header(); - GrpcStreamingSafetyOpen safety_open; - safety_open.full_method_name = - "/cmvr.api.SpeakerService/StreamAudio"; - safety_open.device_id = dev_id; - safety_open.session_id = header.command_id().empty() - ? cmvr_grpc_call_guard.context().correlation_id - : header.command_id(); - safety_open.expected_service_instance_id = - header.expected_service_instance_id(); - if (header.has_expected_device_generation()) { - safety_open.expected_device_generation = - header.expected_device_generation(); - } - safety_open.deadline = - cmvr_grpc_call_guard.context().deadline; - safety_session.emplace( - dmgr_.safetyManager(), - cmvr_grpc_call_guard.context(), - std::move(safety_open)); - if (!safety_session->admitted()) { - return safety_session->status(); - } - } else if (!request.header().device_id().empty() && - request.header().device_id() != dev_id) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "speaker stream cannot change device_id after its first frame"); } - if (!safety_session || !safety_session->revalidate()) { - return safety_session - ? safety_session->status() - : grpc::Status( - grpc::StatusCode::INTERNAL, - "speaker stream safety session was not initialized"); - } - - const auto frame = fromProtoAudioData(request.audio()); - bool pushed = false; - grpc::Status dispatch_status = grpc::Status::OK; - const bool push_allowed = media_session.runIfCurrent([&] { - auto dispatch = safety_session->beginDispatch(); - if (!dispatch.acquired()) { - dispatch_status = safety_session->status(); - return; - } - if (!frame.data.empty()) { - stream_lease->arm(); - } - pushed = dev->pushAudioFrame(frame); - }); - if (!push_allowed || media_session.cancelled()) { - return mediaStoppedStatus(); - } - if (!dispatch_status.ok()) { - return dispatch_status; - } - if (!pushed) { + if (!dev->pushAudioFrame(fromProtoAudioData(request.audio()))) { + dev->stopStreaming(); return failResponse(response, "Failed to push speaker audio frame: " + dev_id); } } - if (media_session.cancelled()) { - return mediaStoppedStatus(); + 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) { - if (media_session.cancelled()) { - return mediaStoppedStatus(); - } response->mutable_header()->set_success(false); response->mutable_header()->set_error_message(e.what()); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); @@ -306,121 +153,101 @@ grpc::Status gRPCSpeakerServiceImpl::StreamAudio(grpc::ServerContext* context, grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context, const api::StopSpeakerCommand_Request* request, api::StopSpeakerCommand_Feedback* response) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.SpeakerService/StopPlayback", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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 (!command.beginDispatch()) { - return command.dispatchStatus(); - } - if (!dev->stopPlayback()) { + 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.SpeakerService/PausePlayback", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.SpeakerService/ResumePlayback", request, response, - [this, request, response](GrpcCommandTransaction& command) { - auto media_session = globalMediaActivityCoordinator().beginSession(); - if (!media_session) { - return failResponse( - response, "Media activities are temporarily paused by StopAll"); - } + 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); } - if (!media_session.claimExclusiveResource(speakerResourceKey(dev_id))) { - return failResponse( - response, "Speaker is already controlled by another media session: " + dev_id); - } - bool dispatch_allowed = false; - if (!media_session.runIfCurrent([&] { - dispatch_allowed = command.beginDispatch(); - if (dispatch_allowed) { - dev->resume(); - } - })) { - return failResponse( - response, "Speaker playback resume was canceled by StopAll"); - } - if (!dispatch_allowed) { - return command.dispatchStatus(); - } + 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) { - return executeRegisteredGrpcCommand( - security_gateway_, context, dmgr_.safetyManager(), - "/cmvr.api.SpeakerService/SetVolume", request, response, - [this, request, response](GrpcCommandTransaction& command) { + 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); } - if (!command.beginDispatch()) { - return command.dispatchStatus(); - } 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) { - CMVR_GRPC_REQUIRE_REGISTERED_CALL( - security_gateway_, context, "/cmvr.api.SpeakerService/GetVolume"); try { string dev_id = request->header().device_id(); CMVR_LOG(INFO) << "[gRPCSpeakerServiceImpl] (GetVolume): id=" << dev_id; diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp new file mode 100644 index 00000000..a9ac6a31 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -0,0 +1,141 @@ +// +// 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/stop_all/CMakeLists.txt b/cmvr-es/service/grpc/stop_all/CMakeLists.txt deleted file mode 100644 index d6469ad3..00000000 --- a/cmvr-es/service/grpc/stop_all/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -add_library(stop_all_admission_gate STATIC - src/stop_all_admission_gate.cpp -) - -target_compile_features(stop_all_admission_gate PUBLIC cxx_std_17) -target_include_directories(stop_all_admission_gate - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../.. -) - -add_library(cmvr_es::stop_all_admission_gate ALIAS stop_all_admission_gate) - -add_library(camera_operational_activity_registry STATIC - ../server/src/camera_operational_activity_registry.cpp -) - -target_compile_features(camera_operational_activity_registry PUBLIC cxx_std_17) -target_include_directories(camera_operational_activity_registry - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../.. -) -target_link_libraries(camera_operational_activity_registry - PUBLIC - cmvr_es::stop_all_admission_gate -) - -add_library(cmvr_es::camera_operational_activity_registry ALIAS - camera_operational_activity_registry) diff --git a/cmvr-es/service/grpc/stop_all/include/deferred_stop_operation.h b/cmvr-es/service/grpc/stop_all/include/deferred_stop_operation.h deleted file mode 100644 index 0b94fb5c..00000000 --- a/cmvr-es/service/grpc/stop_all/include/deferred_stop_operation.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef CMVR_ES_DEFERRED_STOP_OPERATION_H -#define CMVR_ES_DEFERRED_STOP_OPERATION_H - -#include -#include - -namespace cmvr::service { - -struct DeferredStopResult { - bool success{false}; - std::string detail; -}; - -// A coordinator-owned stop callback which can be submitted to any executor. -// The resource key is stable for the lifetime of the underlying registration -// or session, and the callback owns all state needed after collection returns. -struct DeferredStopOperation { - std::string resource_key; - std::function operation; - - explicit operator bool() const noexcept - { - return !resource_key.empty() && static_cast(operation); - } -}; - -} // namespace cmvr::service - -#endif // CMVR_ES_DEFERRED_STOP_OPERATION_H diff --git a/cmvr-es/service/grpc/stop_all/include/stop_all_admission_gate.h b/cmvr-es/service/grpc/stop_all/include/stop_all_admission_gate.h deleted file mode 100644 index 4ade4fde..00000000 --- a/cmvr-es/service/grpc/stop_all/include/stop_all_admission_gate.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifndef CMVR_ES_STOP_ALL_ADMISSION_GATE_H -#define CMVR_ES_STOP_ALL_ADMISSION_GATE_H - -#include -#include -#include - -namespace cmvr::service { - -// Provides the cross-domain admission boundary for SystemService::StopAll. -// Action and media coordinators may finish independently, but neither domain -// can admit new work until this gate is completed last. -class StopAllAdmissionGate final { -public: - struct FinishResult { - bool ticket_consumed{false}; - bool admission_reopened{false}; - }; - - struct StopAllTicket { - std::uint64_t generation{0}; - std::uint64_t ticket_id{0}; - - bool valid() const noexcept - { - return generation != 0U && ticket_id != 0U; - } - }; - - class AdmissionGuard final { - public: - AdmissionGuard(AdmissionGuard&&) noexcept = default; - AdmissionGuard& operator=(AdmissionGuard&&) noexcept = default; - - AdmissionGuard(const AdmissionGuard&) = delete; - AdmissionGuard& operator=(const AdmissionGuard&) = delete; - - bool accepting() const noexcept { return accepting_; } - std::uint64_t generation() const noexcept { return generation_; } - - private: - friend class StopAllAdmissionGate; - - AdmissionGuard( - std::unique_lock&& lock, - bool accepting, - std::uint64_t generation) noexcept; - - std::unique_lock lock_; - bool accepting_{false}; - std::uint64_t generation_{0}; - }; - - // The returned guard linearizes a bounded admission operation against - // beginStopAll(). Do not retain it while executing device work. - AdmissionGuard lockAdmission(); - - // Concurrent callers join one round. A failed round remains closed until - // a later StopAll round successfully confirms every domain is stopped. - StopAllTicket beginStopAll(); - bool finishStopAll( - const StopAllTicket& ticket, - bool all_domains_stop_confirmed); - - // Consumes one participant ticket and reports whether the complete round - // actually reopened admission. A participant can confirm its own work - // while another participant has failed or is still outstanding. - FinishResult finishStopAllDetailed( - const StopAllTicket& ticket, - bool all_domains_stop_confirmed); - - // Test/process teardown hook. Runtime code must recover a failed gate with - // a new successful StopAll round instead of bypassing fail-closed state. - void clearForTesting() noexcept; - -private: - std::mutex mutex_; - bool accepting_{true}; - bool stop_all_failed_{false}; - std::uint64_t generation_{1U}; - std::uint64_t next_ticket_id_{0U}; - std::unordered_set outstanding_tickets_; -}; - -StopAllAdmissionGate& globalStopAllAdmissionGate(); - -} // namespace cmvr::service - -#endif // CMVR_ES_STOP_ALL_ADMISSION_GATE_H diff --git a/cmvr-es/service/grpc/stop_all/include/stop_operation_dispatcher.h b/cmvr-es/service/grpc/stop_all/include/stop_operation_dispatcher.h deleted file mode 100644 index 60c25804..00000000 --- a/cmvr-es/service/grpc/stop_all/include/stop_operation_dispatcher.h +++ /dev/null @@ -1,85 +0,0 @@ -#ifndef CMVR_ES_STOP_OPERATION_DISPATCHER_H -#define CMVR_ES_STOP_OPERATION_DISPATCHER_H - -#include -#include -#include -#include -#include - -namespace cmvr::service { - -// Runs potentially blocking operational-stop calls without allowing one -// resource to delay stop requests for unrelated resources. The dispatcher -// owns worker handles while it is alive. StopAll RPCs may stop waiting at -// their deadline, but dispatcher destruction joins every outstanding worker -// before device lifecycle teardown is allowed to continue. Native threads -// cannot be canceled safely while they may still be inside a device driver. -class StopOperationDispatcher final { -private: - struct JobState; - struct Impl; - -public: - using Clock = std::chrono::steady_clock; - using Deadline = Clock::time_point; - - struct OperationResult { - bool success{false}; - std::string detail; - }; - - struct WaitResult { - bool completed{false}; - bool result{false}; - std::string detail; - }; - - using Operation = std::function; - using TimeoutDetailProvider = std::function; - - class Handle final { - public: - Handle() = default; - - bool valid() const noexcept; - WaitResult waitUntil(Deadline deadline) const; - - private: - friend class StopOperationDispatcher; - - explicit Handle(std::shared_ptr state) noexcept; - - std::shared_ptr state_; - }; - - StopOperationDispatcher(); - ~StopOperationDispatcher(); - - StopOperationDispatcher(const StopOperationDispatcher&) = delete; - StopOperationDispatcher& operator=(const StopOperationDispatcher&) = delete; - StopOperationDispatcher(StopOperationDispatcher&&) = delete; - StopOperationDispatcher& operator=(StopOperationDispatcher&&) = delete; - - // A running job is shared by every submission for the same resource key. - // Once it has completed, the next submission joins/reaps the old worker - // and starts a new job. A timeout provider belongs to the job that is - // actually created; submissions joining a running job retain that job's - // provider. Multiple handles may call the provider concurrently, so it - // must be thread-safe. Empty keys or operations return an invalid handle. - Handle submit(std::string resource_key, Operation operation); - Handle submit( - std::string resource_key, - Operation operation, - TimeoutDetailProvider timeout_detail_provider); - - // Exposed only to verify dispatcher lifecycle behavior in tests. - std::size_t jobCountForTesting() const; - -private: - std::unique_ptr impl_; -}; - -} // namespace cmvr::service - -#endif // CMVR_ES_STOP_OPERATION_DISPATCHER_H diff --git a/cmvr-es/service/grpc/stop_all/src/stop_all_admission_gate.cpp b/cmvr-es/service/grpc/stop_all/src/stop_all_admission_gate.cpp deleted file mode 100644 index cdee9a90..00000000 --- a/cmvr-es/service/grpc/stop_all/src/stop_all_admission_gate.cpp +++ /dev/null @@ -1,90 +0,0 @@ -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -#include - -namespace cmvr::service { - -StopAllAdmissionGate::AdmissionGuard::AdmissionGuard( - std::unique_lock&& lock, - const bool accepting, - const std::uint64_t generation) noexcept - : lock_(std::move(lock)), - accepting_(accepting), - generation_(generation) -{ -} - -StopAllAdmissionGate::AdmissionGuard -StopAllAdmissionGate::lockAdmission() -{ - std::unique_lock lock(mutex_); - return AdmissionGuard( - std::move(lock), accepting_, generation_); -} - -StopAllAdmissionGate::StopAllTicket -StopAllAdmissionGate::beginStopAll() -{ - std::lock_guard lock(mutex_); - if (accepting_ || outstanding_tickets_.empty()) { - accepting_ = false; - stop_all_failed_ = false; - ++generation_; - } - - StopAllTicket ticket; - ticket.generation = generation_; - ticket.ticket_id = ++next_ticket_id_; - outstanding_tickets_.emplace(ticket.ticket_id); - return ticket; -} - -bool StopAllAdmissionGate::finishStopAll( - const StopAllTicket& ticket, - const bool all_domains_stop_confirmed) -{ - const auto result = finishStopAllDetailed( - ticket, all_domains_stop_confirmed); - return result.ticket_consumed && all_domains_stop_confirmed; -} - -StopAllAdmissionGate::FinishResult -StopAllAdmissionGate::finishStopAllDetailed( - const StopAllTicket& ticket, - const bool all_domains_stop_confirmed) -{ - std::lock_guard lock(mutex_); - if (!ticket.valid() || accepting_ || - ticket.generation != generation_ || - outstanding_tickets_.erase(ticket.ticket_id) == 0U) { - return {}; - } - - if (!all_domains_stop_confirmed) { - stop_all_failed_ = true; - } - if (outstanding_tickets_.empty() && !stop_all_failed_) { - accepting_ = true; - } - return {true, accepting_}; -} - -void StopAllAdmissionGate::clearForTesting() noexcept -{ - try { - std::lock_guard lock(mutex_); - accepting_ = true; - stop_all_failed_ = false; - ++generation_; - outstanding_tickets_.clear(); - } catch (...) { - } -} - -StopAllAdmissionGate& globalStopAllAdmissionGate() -{ - static StopAllAdmissionGate gate; - return gate; -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/stop_all/src/stop_operation_dispatcher.cpp b/cmvr-es/service/grpc/stop_all/src/stop_operation_dispatcher.cpp deleted file mode 100644 index 38bfb0cf..00000000 --- a/cmvr-es/service/grpc/stop_all/src/stop_operation_dispatcher.cpp +++ /dev/null @@ -1,214 +0,0 @@ -#include "service/grpc/stop_all/include/stop_operation_dispatcher.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::service { - -struct StopOperationDispatcher::JobState final { - explicit JobState(TimeoutDetailProvider provider) - : timeout_detail_provider(std::move(provider)) - { - } - - std::mutex mutex; - std::condition_variable condition; - bool completed{false}; - OperationResult outcome; - const TimeoutDetailProvider timeout_detail_provider; -}; - -struct StopOperationDispatcher::Impl final { - struct Job final { - std::shared_ptr state; - std::thread worker; - }; - - std::mutex mutex; - std::unordered_map jobs; -}; - -StopOperationDispatcher::Handle::Handle( - std::shared_ptr state) noexcept - : state_(std::move(state)) -{ -} - -bool StopOperationDispatcher::Handle::valid() const noexcept -{ - return static_cast(state_); -} - -StopOperationDispatcher::WaitResult -StopOperationDispatcher::Handle::waitUntil(const Deadline deadline) const -{ - if (!state_) { - return {false, false, "invalid stop operation handle"}; - } - - std::unique_lock lock(state_->mutex); - if (!state_->condition.wait_until(lock, deadline, [this] { - return state_->completed; - })) { - lock.unlock(); - if (state_->timeout_detail_provider) { - try { - auto detail = state_->timeout_detail_provider(); - if (!detail.empty()) { - return {false, false, std::move(detail)}; - } - } catch (...) { - // Diagnostics must not change timeout behavior. - } - } - return {false, false, - "stop operation did not complete before the deadline"}; - } - - return {true, state_->outcome.success, state_->outcome.detail}; -} - -StopOperationDispatcher::StopOperationDispatcher() - : impl_(std::make_unique()) -{ -} - -StopOperationDispatcher::~StopOperationDispatcher() -{ - std::vector workers; - { - std::lock_guard lock(impl_->mutex); - workers.reserve(impl_->jobs.size()); - for (auto& entry : impl_->jobs) { - if (entry.second.worker.joinable()) { - workers.emplace_back(std::move(entry.second.worker)); - } - } - impl_->jobs.clear(); - } - for (auto& worker : workers) { - worker.join(); - } -} - -StopOperationDispatcher::Handle StopOperationDispatcher::submit( - std::string resource_key, - Operation operation) -{ - return submit(std::move(resource_key), std::move(operation), {}); -} - -StopOperationDispatcher::Handle StopOperationDispatcher::submit( - std::string resource_key, - Operation operation, - TimeoutDetailProvider timeout_detail_provider) -{ - if (resource_key.empty() || !operation) { - return {}; - } - - std::vector completed_workers; - { - std::lock_guard lock(impl_->mutex); - completed_workers.reserve(impl_->jobs.size()); - for (auto job = impl_->jobs.begin(); job != impl_->jobs.end();) { - bool completed = false; - { - std::lock_guard state_lock(job->second.state->mutex); - completed = job->second.state->completed; - } - if (!completed) { - ++job; - continue; - } - - if (job->second.worker.joinable()) { - completed_workers.emplace_back( - std::move(job->second.worker)); - } - job = impl_->jobs.erase(job); - } - } - for (auto& worker : completed_workers) { - worker.join(); - } - - for (;;) { - std::thread completed_worker; - std::unique_lock lock(impl_->mutex); - const auto existing = impl_->jobs.find(resource_key); - if (existing != impl_->jobs.end()) { - bool completed = false; - { - std::lock_guard state_lock(existing->second.state->mutex); - completed = existing->second.state->completed; - } - if (!completed) { - return Handle(existing->second.state); - } - - if (existing->second.worker.joinable()) { - completed_worker = std::move(existing->second.worker); - } - impl_->jobs.erase(existing); - lock.unlock(); - if (completed_worker.joinable()) { - completed_worker.join(); - } - continue; - } - - auto state = std::make_shared( - std::move(timeout_detail_provider)); - const auto inserted = impl_->jobs.emplace( - std::piecewise_construct, - std::forward_as_tuple(std::move(resource_key)), - std::forward_as_tuple()); - auto& job = inserted.first->second; - job.state = state; - try { - job.worker = std::thread( - [state, operation = std::move(operation)]() mutable { - OperationResult outcome; - try { - outcome = operation(); - } catch (const std::exception& error) { - outcome.success = false; - outcome.detail = - std::string("stop operation threw: ") + - error.what(); - } catch (...) { - outcome.success = false; - outcome.detail = - "stop operation threw an unknown exception"; - } - - { - std::lock_guard state_lock(state->mutex); - state->outcome = std::move(outcome); - state->completed = true; - } - state->condition.notify_all(); - }); - } catch (...) { - impl_->jobs.erase(inserted.first); - throw; - } - - return Handle(std::move(state)); - } -} - -std::size_t StopOperationDispatcher::jobCountForTesting() const -{ - std::lock_guard lock(impl_->mutex); - return impl_->jobs.size(); -} - -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/stop_all/tests/stop_all_admission_gate_test.cpp b/cmvr-es/service/grpc/stop_all/tests/stop_all_admission_gate_test.cpp deleted file mode 100644 index 21990915..00000000 --- a/cmvr-es/service/grpc/stop_all/tests/stop_all_admission_gate_test.cpp +++ /dev/null @@ -1,104 +0,0 @@ -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" - -#include - -namespace cmvr::service { -namespace { - -TEST(StopAllAdmissionGateTest, SuccessfulRoundReopensAdmission) -{ - StopAllAdmissionGate gate; - { - const auto before = gate.lockAdmission(); - EXPECT_TRUE(before.accepting()); - } - - const auto ticket = gate.beginStopAll(); - ASSERT_TRUE(ticket.valid()); - { - const auto blocked = gate.lockAdmission(); - EXPECT_FALSE(blocked.accepting()); - EXPECT_EQ(blocked.generation(), ticket.generation); - } - EXPECT_TRUE(gate.finishStopAll(ticket, true)); - EXPECT_TRUE(gate.lockAdmission().accepting()); -} - -TEST(StopAllAdmissionGateTest, FailedRoundStaysClosedAndCanBeRecovered) -{ - StopAllAdmissionGate gate; - const auto failed_ticket = gate.beginStopAll(); - ASSERT_TRUE(failed_ticket.valid()); - EXPECT_FALSE(gate.finishStopAll(failed_ticket, false)); - EXPECT_FALSE(gate.lockAdmission().accepting()); - - const auto recovery_ticket = gate.beginStopAll(); - ASSERT_TRUE(recovery_ticket.valid()); - EXPECT_TRUE(gate.finishStopAll(recovery_ticket, true)); - EXPECT_TRUE(gate.lockAdmission().accepting()); -} - -TEST(StopAllAdmissionGateTest, EveryConcurrentParticipantMustSucceed) -{ - StopAllAdmissionGate gate; - const auto first = gate.beginStopAll(); - const auto second = gate.beginStopAll(); - ASSERT_TRUE(first.valid()); - ASSERT_TRUE(second.valid()); - - EXPECT_TRUE(gate.finishStopAll(first, true)); - EXPECT_FALSE(gate.lockAdmission().accepting()); - EXPECT_FALSE(gate.finishStopAll(second, false)); - EXPECT_FALSE(gate.lockAdmission().accepting()); -} - -TEST(StopAllAdmissionGateTest, - DetailedFinishDoesNotReportRecoveryAfterAnotherParticipantFailed) -{ - StopAllAdmissionGate gate; - const auto system_ticket = gate.beginStopAll(); - const auto failed_participant = gate.beginStopAll(); - - const auto failed_result = - gate.finishStopAllDetailed(failed_participant, false); - EXPECT_TRUE(failed_result.ticket_consumed); - EXPECT_FALSE(failed_result.admission_reopened); - - const auto system_result = - gate.finishStopAllDetailed(system_ticket, true); - EXPECT_TRUE(system_result.ticket_consumed); - EXPECT_FALSE(system_result.admission_reopened); - EXPECT_FALSE(gate.lockAdmission().accepting()); -} - -TEST(StopAllAdmissionGateTest, - DetailedFinishSeparatesConsumptionFromOutstandingParticipants) -{ - StopAllAdmissionGate gate; - const auto first = gate.beginStopAll(); - const auto second = gate.beginStopAll(); - - const auto first_result = gate.finishStopAllDetailed(first, true); - EXPECT_TRUE(first_result.ticket_consumed); - EXPECT_FALSE(first_result.admission_reopened); - - const auto second_result = gate.finishStopAllDetailed(second, true); - EXPECT_TRUE(second_result.ticket_consumed); - EXPECT_TRUE(second_result.admission_reopened); - EXPECT_TRUE(gate.lockAdmission().accepting()); -} - -TEST(StopAllAdmissionGateTest, TestClearInvalidatesOutstandingTickets) -{ - StopAllAdmissionGate gate; - const auto stale = gate.beginStopAll(); - ASSERT_TRUE(stale.valid()); - - gate.clearForTesting(); - - EXPECT_TRUE(gate.lockAdmission().accepting()); - EXPECT_FALSE(gate.finishStopAll(stale, true)); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/stop_all/tests/stop_operation_dispatcher_test.cpp b/cmvr-es/service/grpc/stop_all/tests/stop_operation_dispatcher_test.cpp deleted file mode 100644 index 562cd05b..00000000 --- a/cmvr-es/service/grpc/stop_all/tests/stop_operation_dispatcher_test.cpp +++ /dev/null @@ -1,399 +0,0 @@ -#include "service/grpc/stop_all/include/stop_operation_dispatcher.h" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace cmvr::service { -namespace { - -using namespace std::chrono_literals; - -TEST(StopOperationDispatcherTest, RejectsEmptyKeysAndOperations) -{ - StopOperationDispatcher dispatcher; - StopOperationDispatcher::Operation empty_operation; - - const auto empty_key = dispatcher.submit("", [] { - return StopOperationDispatcher::OperationResult{true, {}}; - }); - const auto empty_callback = dispatcher.submit( - "arm:one", std::move(empty_operation)); - - EXPECT_FALSE(empty_key.valid()); - EXPECT_FALSE(empty_callback.valid()); - const auto invalid_result = empty_key.waitUntil( - StopOperationDispatcher::Clock::now()); - EXPECT_FALSE(invalid_result.completed); - EXPECT_FALSE(invalid_result.result); - EXPECT_FALSE(invalid_result.detail.empty()); -} - -TEST(StopOperationDispatcherTest, ReturnsOperationOutcome) -{ - StopOperationDispatcher dispatcher; - const auto handle = dispatcher.submit("arm:one", [] { - return StopOperationDispatcher::OperationResult{ - false, "driver did not confirm idle"}; - }); - - ASSERT_TRUE(handle.valid()); - const auto result = handle.waitUntil( - StopOperationDispatcher::Clock::now() + 1s); - EXPECT_TRUE(result.completed); - EXPECT_FALSE(result.result); - EXPECT_EQ(result.detail, "driver did not confirm idle"); -} - -TEST(StopOperationDispatcherTest, TimeoutDoesNotCancelTheJob) -{ - StopOperationDispatcher dispatcher; - std::promise release; - auto released = release.get_future().share(); - const auto handle = dispatcher.submit("agv:one", [released] { - released.wait(); - return StopOperationDispatcher::OperationResult{true, "stopped"}; - }); - - const auto timed_out = handle.waitUntil( - StopOperationDispatcher::Clock::now() + 20ms); - EXPECT_FALSE(timed_out.completed); - EXPECT_FALSE(timed_out.result); - - release.set_value(); - const auto completed = handle.waitUntil( - StopOperationDispatcher::Clock::now() + 1s); - EXPECT_TRUE(completed.completed); - EXPECT_TRUE(completed.result); - EXPECT_EQ(completed.detail, "stopped"); -} - -TEST(StopOperationDispatcherTest, - RunningJobKeepsItsOriginalTimeoutDetailProvider) -{ - StopOperationDispatcher dispatcher; - std::promise release; - auto released = release.get_future().share(); - std::atomic first_operation_calls{0}; - std::atomic duplicate_operation_calls{0}; - std::atomic first_provider_calls{0}; - std::atomic duplicate_provider_calls{0}; - - const auto first = dispatcher.submit( - "arm:one", - [&] { - ++first_operation_calls; - released.wait(); - return StopOperationDispatcher::OperationResult{true, {}}; - }, - [&] { - ++first_provider_calls; - return std::string("first job is waiting for its handler"); - }); - const auto duplicate = dispatcher.submit( - "arm:one", - [&] { - ++duplicate_operation_calls; - return StopOperationDispatcher::OperationResult{true, {}}; - }, - [&] { - ++duplicate_provider_calls; - return std::string("duplicate submission detail"); - }); - - const auto first_timeout = first.waitUntil( - StopOperationDispatcher::Clock::now() + 20ms); - const auto duplicate_timeout = duplicate.waitUntil( - StopOperationDispatcher::Clock::now() + 20ms); - release.set_value(); - const auto completed = first.waitUntil( - StopOperationDispatcher::Clock::now() + 1s); - - EXPECT_FALSE(first_timeout.completed); - EXPECT_EQ( - first_timeout.detail, "first job is waiting for its handler"); - EXPECT_FALSE(duplicate_timeout.completed); - EXPECT_EQ( - duplicate_timeout.detail, "first job is waiting for its handler"); - EXPECT_TRUE(completed.completed); - EXPECT_TRUE(completed.result); - EXPECT_EQ(first_operation_calls.load(), 1); - EXPECT_EQ(duplicate_operation_calls.load(), 0); - EXPECT_EQ(first_provider_calls.load(), 2); - EXPECT_EQ(duplicate_provider_calls.load(), 0); -} - -TEST(StopOperationDispatcherTest, - ThrowingTimeoutDetailProviderFallsBackToGenericDetail) -{ - StopOperationDispatcher dispatcher; - std::promise release; - auto released = release.get_future().share(); - const auto handle = dispatcher.submit( - "arm:one", - [released] { - released.wait(); - return StopOperationDispatcher::OperationResult{true, {}}; - }, - []() -> std::string { - throw std::runtime_error("diagnostic provider failed"); - }); - - const auto timed_out = handle.waitUntil( - StopOperationDispatcher::Clock::now() + 20ms); - release.set_value(); - - EXPECT_FALSE(timed_out.completed); - EXPECT_FALSE(timed_out.result); - EXPECT_EQ( - timed_out.detail, - "stop operation did not complete before the deadline"); -} - -TEST(StopOperationDispatcherTest, RunningSubmissionsForAKeyShareOneJob) -{ - StopOperationDispatcher dispatcher; - std::promise started; - std::promise release; - auto released = release.get_future().share(); - std::atomic first_calls{0}; - std::atomic duplicate_calls{0}; - - const auto first = dispatcher.submit("arm:one", [&] { - ++first_calls; - started.set_value(); - released.wait(); - return StopOperationDispatcher::OperationResult{true, "first"}; - }); - ASSERT_EQ(started.get_future().wait_for(1s), std::future_status::ready); - - const auto duplicate = dispatcher.submit("arm:one", [&] { - ++duplicate_calls; - return StopOperationDispatcher::OperationResult{false, "duplicate"}; - }); - release.set_value(); - - const auto first_result = first.waitUntil( - StopOperationDispatcher::Clock::now() + 1s); - const auto duplicate_result = duplicate.waitUntil( - StopOperationDispatcher::Clock::now() + 1s); - EXPECT_TRUE(first_result.completed); - EXPECT_TRUE(first_result.result); - EXPECT_EQ(first_result.detail, "first"); - EXPECT_TRUE(duplicate_result.completed); - EXPECT_TRUE(duplicate_result.result); - EXPECT_EQ(duplicate_result.detail, "first"); - EXPECT_EQ(first_calls.load(), 1); - EXPECT_EQ(duplicate_calls.load(), 0); -} - -TEST(StopOperationDispatcherTest, ConcurrentSubmissionsForAKeyShareOneJob) -{ - StopOperationDispatcher dispatcher; - constexpr int submitter_count = 12; - std::promise release; - auto released = release.get_future().share(); - std::atomic operation_calls{0}; - std::mutex handles_mutex; - std::vector handles; - std::vector submitters; - handles.reserve(submitter_count); - submitters.reserve(submitter_count); - - for (int index = 0; index < submitter_count; ++index) { - submitters.emplace_back([&] { - auto handle = dispatcher.submit("dexhand:one", [&] { - ++operation_calls; - released.wait(); - return StopOperationDispatcher::OperationResult{true, {}}; - }); - std::lock_guard lock(handles_mutex); - handles.emplace_back(std::move(handle)); - }); - } - for (auto& submitter : submitters) { - submitter.join(); - } - release.set_value(); - - ASSERT_EQ(handles.size(), static_cast(submitter_count)); - const auto deadline = StopOperationDispatcher::Clock::now() + 1s; - for (const auto& handle : handles) { - const auto result = handle.waitUntil(deadline); - EXPECT_TRUE(result.completed); - EXPECT_TRUE(result.result); - } - EXPECT_EQ(operation_calls.load(), 1); -} - -TEST(StopOperationDispatcherTest, CompletedJobIsReapedBeforeNextSubmission) -{ - StopOperationDispatcher dispatcher; - std::atomic calls{0}; - - const auto first = dispatcher.submit("arm:one", [&] { - ++calls; - return StopOperationDispatcher::OperationResult{true, "round one"}; - }); - const auto first_result = first.waitUntil( - StopOperationDispatcher::Clock::now() + 1s); - ASSERT_TRUE(first_result.completed); - ASSERT_TRUE(first_result.result); - - const auto second = dispatcher.submit("arm:one", [&] { - ++calls; - return StopOperationDispatcher::OperationResult{true, "round two"}; - }); - const auto second_result = second.waitUntil( - StopOperationDispatcher::Clock::now() + 1s); - - EXPECT_TRUE(second_result.completed); - EXPECT_TRUE(second_result.result); - EXPECT_EQ(second_result.detail, "round two"); - EXPECT_EQ(calls.load(), 2); - EXPECT_EQ(first.waitUntil( - StopOperationDispatcher::Clock::now()).detail, "round one"); -} - -TEST(StopOperationDispatcherTest, SubmissionReapsAllCompletedJobs) -{ - StopOperationDispatcher dispatcher; - constexpr int completed_job_count = 24; - std::promise release; - auto released = release.get_future().share(); - std::vector handles; - handles.reserve(completed_job_count); - - for (int index = 0; index < completed_job_count; ++index) { - handles.emplace_back(dispatcher.submit( - "arm:" + std::to_string(index), [released] { - released.wait(); - return StopOperationDispatcher::OperationResult{true, {}}; - })); - } - ASSERT_EQ(dispatcher.jobCountForTesting(), - static_cast(completed_job_count)); - - release.set_value(); - const auto deadline = StopOperationDispatcher::Clock::now() + 1s; - for (const auto& handle : handles) { - ASSERT_TRUE(handle.waitUntil(deadline).completed); - } - ASSERT_EQ(dispatcher.jobCountForTesting(), - static_cast(completed_job_count)); - - std::promise trigger_started; - std::promise release_trigger; - auto trigger_released = release_trigger.get_future().share(); - const auto trigger = dispatcher.submit( - "agv:cleanup-trigger", [&] { - trigger_started.set_value(); - trigger_released.wait(); - return StopOperationDispatcher::OperationResult{true, {}}; - }); - - ASSERT_EQ(trigger_started.get_future().wait_for(1s), - std::future_status::ready); - EXPECT_EQ(dispatcher.jobCountForTesting(), 1U); - - release_trigger.set_value(); - EXPECT_TRUE(trigger.waitUntil( - StopOperationDispatcher::Clock::now() + 1s).result); -} - -TEST(StopOperationDispatcherTest, DifferentKeysRunIndependently) -{ - StopOperationDispatcher dispatcher; - std::promise release_first; - auto first_released = release_first.get_future().share(); - std::promise second_started; - - const auto first = dispatcher.submit("camera:one", [first_released] { - first_released.wait(); - return StopOperationDispatcher::OperationResult{true, {}}; - }); - const auto second = dispatcher.submit("speaker:one", [&] { - second_started.set_value(); - return StopOperationDispatcher::OperationResult{true, {}}; - }); - - EXPECT_EQ( - second_started.get_future().wait_for(1s), - std::future_status::ready); - const auto second_result = second.waitUntil( - StopOperationDispatcher::Clock::now() + 1s); - EXPECT_TRUE(second_result.completed); - EXPECT_TRUE(second_result.result); - - release_first.set_value(); - EXPECT_TRUE(first.waitUntil( - StopOperationDispatcher::Clock::now() + 1s).result); -} - -TEST(StopOperationDispatcherTest, ConvertsOperationExceptionsToFailures) -{ - StopOperationDispatcher dispatcher; - const auto standard = dispatcher.submit("arm:one", []() - -> StopOperationDispatcher::OperationResult { - throw std::runtime_error("transport failed"); - }); - const auto unknown = dispatcher.submit("agv:one", []() - -> StopOperationDispatcher::OperationResult { - throw 42; - }); - - const auto deadline = StopOperationDispatcher::Clock::now() + 1s; - const auto standard_result = standard.waitUntil(deadline); - const auto unknown_result = unknown.waitUntil(deadline); - EXPECT_TRUE(standard_result.completed); - EXPECT_FALSE(standard_result.result); - EXPECT_NE(standard_result.detail.find("transport failed"), - std::string::npos); - EXPECT_TRUE(unknown_result.completed); - EXPECT_FALSE(unknown_result.result); - EXPECT_NE(unknown_result.detail.find("unknown exception"), - std::string::npos); -} - -TEST(StopOperationDispatcherTest, - DestructorWaitsForOutstandingWorkers) -{ - auto dispatcher = std::make_unique(); - std::promise started; - std::promise release; - auto released = release.get_future().share(); - const auto handle = dispatcher->submit("arm:one", [&] { - started.set_value(); - released.wait(); - return StopOperationDispatcher::OperationResult{ - true, "completed after dispatcher destruction"}; - }); - ASSERT_EQ(started.get_future().wait_for(1s), std::future_status::ready); - - auto destroy = std::async(std::launch::async, [&] { - dispatcher.reset(); - }); - EXPECT_EQ(destroy.wait_for(20ms), std::future_status::timeout); - - release.set_value(); - EXPECT_EQ(destroy.wait_for(1s), std::future_status::ready); - destroy.get(); - const auto result = handle.waitUntil( - StopOperationDispatcher::Clock::now() + 1s); - EXPECT_TRUE(result.completed); - EXPECT_TRUE(result.result); - EXPECT_EQ(result.detail, "completed after dispatcher destruction"); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/server/tests/grpc_camera_stream_policy_test.cpp b/cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp similarity index 96% rename from cmvr-es/service/grpc/server/tests/grpc_camera_stream_policy_test.cpp rename to cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp index ad450c4c..1d84b942 100644 --- a/cmvr-es/service/grpc/server/tests/grpc_camera_stream_policy_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp @@ -1,4 +1,4 @@ -#include "service/grpc/server/include/grpc_camera_stream_policy.h" +#include "service/grpc/include/grpc_camera_stream_policy.h" #include #include diff --git a/cmvr-es/task/CMakeLists.txt b/cmvr-es/task/CMakeLists.txt index 05d0896d..e16bbb5f 100644 --- a/cmvr-es/task/CMakeLists.txt +++ b/cmvr-es/task/CMakeLists.txt @@ -12,44 +12,13 @@ target_link_libraries(task cmvr_es::ik_solver cmvr_es::base_motion cmvr_es::self_collision_checker - cmvr_es::control_authority_manager PRIVATE cmvr_es::device_manager - cmvr_es::stop_all_admission_gate - cmvr_es::camera_operational_activity_registry ) add_library(cmvr_es::task ALIAS task) install(TARGETS task LIBRARY DESTINATION lib) -if(BUILD_TESTING) - add_executable(touch_screen_admission_test - touch_screen_task/src/touch_screen_admission_test.cpp - ) - target_link_libraries(touch_screen_admission_test PRIVATE - cmvr_es::task - cmvr_es::device_manager - cmvr_es::stop_all_admission_gate - cmvr_es::camera_operational_activity_registry - gtest - gtest_main - pthread - ) - add_test( - NAME touch_screen_admission_test - COMMAND touch_screen_admission_test - ) - set(_touch_screen_admission_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _touch_screen_admission_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(touch_screen_admission_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_touch_screen_admission_test_environment}") -endif() - #add_executable(touch_screen_task_test # touch_screen_task/src/touch_screen_task_test.cpp #) 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 0a275eed..055e2eed 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,12 +11,6 @@ #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 { @@ -26,10 +20,6 @@ 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; @@ -65,12 +55,6 @@ 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 c7799fc2..60d8c697 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,6 +1,5 @@ #include "task/grpc_server_task/include/grpc_server_task.h" -#include #include #include @@ -9,36 +8,21 @@ #include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "common/base/logging/logger.h" #include "common/config/config_files.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 "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 "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()) { @@ -114,45 +98,18 @@ bool GrpcServerTask::start() camera_service_ = std::make_unique( service::makeCameraStreamLowLatencyConfig( cfg_.camera_stream_max_pending_frames(), - 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_); + 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(); 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()); @@ -160,8 +117,6 @@ 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()); @@ -176,15 +131,7 @@ bool GrpcServerTask::start() address_ = local_address; state_ = TaskState::RUNNING; - 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; + CMVR_LOG(INFO) << "[GrpcServerTask] gRPC server started, address=" << address_; wait_thread_ = std::thread(&GrpcServerTask::waitLoop, this); return true; } @@ -202,101 +149,6 @@ 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; @@ -312,11 +164,6 @@ void GrpcServerTask::stop() { { std::lock_guard lock(mutex_); - if (auto* system_service = - dynamic_cast( - system_service_.get())) { - system_service->prepareForShutdown(); - } if (server_) { server_->Shutdown(); } @@ -405,19 +252,14 @@ 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 571fad9b..a6786f4e 100644 --- a/cmvr-es/task/task.h +++ b/cmvr-es/task/task.h @@ -19,30 +19,17 @@ 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 e94cabf1..6e284d7e 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,9 +4,7 @@ #define CMVR_ES_TOUCH_SCREEN_TASK_H #include -#include #include -#include #include #include #include @@ -19,16 +17,12 @@ #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 { @@ -61,22 +55,11 @@ 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; @@ -87,23 +70,11 @@ 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); @@ -122,36 +93,18 @@ 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 resetActivityUnlocked(); + void stopUnlocked(); bool applyConfig(); bool validateControlJointNames() const; bool stepAligning(double dt); @@ -179,11 +132,8 @@ 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}; @@ -198,11 +148,6 @@ 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_admission_test.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_admission_test.cpp deleted file mode 100644 index f02cd579..00000000 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_admission_test.cpp +++ /dev/null @@ -1,559 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#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 "task/touch_screen_task/include/touch_screen_task.h" - -namespace cmvr::task { - -class TouchScreenTaskAdmissionTestPeer final { -public: - static void prepare( - TouchScreenTask& task, - const std::shared_ptr& arm, - const std::shared_ptr& camera) - { - std::lock_guard lock(task.mutex_); - task.arm_ = arm; - { - std::lock_guard arm_lock(task.activity_arm_mutex_); - task.activity_arm_ = arm; - } - task.camera_ = camera; - task.initialized_ = true; - task.phase_ = TouchScreenTask::Phase::IDLE; - task.last_status_ = TouchScreenTask::Status::IDLE; - } - - static void prepareInit( - TouchScreenTask& task, - const std::string& arm_id, - const std::string& dexhand_id, - const std::string& camera_id) - { - std::lock_guard lock(task.mutex_); - task.config_valid_ = true; - auto* devices = task.config_.mutable_devices(); - devices->set_arm_id(arm_id); - devices->set_dexhand_id(dexhand_id); - devices->set_camera_id(camera_id); - task.initialized_ = false; - task.phase_ = TouchScreenTask::Phase::IDLE; - task.last_status_ = TouchScreenTask::Status::NOT_INITIALIZED; - } - - static bool sendJointVelocity(TouchScreenTask& task) - { - std::lock_guard lock(task.mutex_); - return task.sendJointVelocity({}); - } -}; - -} // namespace cmvr::task - -namespace { - -class AdmissionRobotArm final : public cmvr::device::RobotArm { -public: - AdmissionRobotArm() { id_ = "touch-admission-arm"; } - - std::string typeName() const override { return "AdmissionRobotArm"; } - 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 - { - ++speed_j_calls_; - 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_; - 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 speedJCalls() const noexcept { return speed_j_calls_.load(); } - int stopMotionCalls() const noexcept - { - return stop_motion_calls_.load(); - } - -private: - static cmvr::device::Result success() - { - return cmvr::device::Result::success(); - } - - std::atomic speed_j_calls_{0}; - std::atomic stop_motion_calls_{0}; -}; - -class AdmissionCamera final : public cmvr::device::AbstractCamera { -public: - explicit AdmissionCamera(std::string id) - { - id_ = std::move(id); - state_.is_initialized = true; - } - - std::string typeName() const override { return "AdmissionCamera"; } - - void getState(cmvr::device::CameraState& state) override - { - std::lock_guard lock(mutex_); - state = state_; - } - - bool start() override - { - ++lifecycle_start_calls_; - return true; - } - - bool stop() override - { - ++lifecycle_stop_calls_; - std::lock_guard lock(mutex_); - operational_active_ = false; - state_.is_opened = false; - return true; - } - - bool startOperationalActivity() override - { - ++operational_start_calls_; - std::unique_lock lock(mutex_); - start_entered_ = true; - condition_.notify_all(); - condition_.wait(lock, [this] { return !block_start_; }); - operational_active_ = true; - state_.is_opened = true; - return true; - } - - bool stopOperationalActivity() override - { - ++operational_stop_calls_; - std::lock_guard lock(mutex_); - operational_active_ = false; - state_.is_opened = false; - return true; - } - - void blockStart() - { - std::lock_guard lock(mutex_); - block_start_ = true; - start_entered_ = false; - } - - void waitForStartEntered() - { - std::unique_lock lock(mutex_); - condition_.wait(lock, [this] { return start_entered_; }); - } - - void releaseStart() - { - { - std::lock_guard lock(mutex_); - block_start_ = false; - } - condition_.notify_all(); - } - - bool operationalActive() const - { - std::lock_guard lock(mutex_); - return operational_active_; - } - - int lifecycleStartCalls() const { return lifecycle_start_calls_; } - int lifecycleStopCalls() const { return lifecycle_stop_calls_; } - int operationalStartCalls() const { return operational_start_calls_; } - int operationalStopCalls() const { return operational_stop_calls_; } - -private: - mutable std::mutex mutex_; - std::condition_variable condition_; - bool block_start_{false}; - bool start_entered_{false}; - bool operational_active_{false}; - std::atomic lifecycle_start_calls_{0}; - std::atomic lifecycle_stop_calls_{0}; - std::atomic operational_start_calls_{0}; - std::atomic operational_stop_calls_{0}; -}; - -class TouchScreenAdmissionTest : public ::testing::Test { -protected: - void SetUp() override - { - cmvr::service::globalCameraOperationalActivityRegistry() - .clearForTesting(); - cmvr::device::DeviceManager::destroyInstance(); - cmvr::service::globalStopAllAdmissionGate().clearForTesting(); - cmvr::control::ControlAuthorityManager::instance().clear(); - } - - void TearDown() override - { - cmvr::service::globalCameraOperationalActivityRegistry() - .clearForTesting(); - cmvr::device::DeviceManager::destroyInstance(); - cmvr::service::globalStopAllAdmissionGate().clearForTesting(); - cmvr::control::ControlAuthorityManager::instance().clear(); - } -}; - -TEST_F(TouchScreenAdmissionTest, - DirectTouchAndStartFromPixelHonorFailClosedAdmissionAndRecovery) -{ - auto arm = std::make_shared(); - auto camera = std::make_shared( - "touch-admission-camera"); - cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); - cmvr::task::TouchScreenTaskAdmissionTestPeer::prepare( - task, arm, camera); - auto& gate = cmvr::service::globalStopAllAdmissionGate(); - - auto ticket = gate.beginStopAll(); - EXPECT_FALSE(task.touch(10, 20)); - EXPECT_FALSE(task.startFromPixel(10, 20)); - EXPECT_FALSE(task.isBusy()); - - EXPECT_TRUE(gate.finishStopAll(ticket, true)); - ASSERT_TRUE(task.touch(10, 20)); - EXPECT_TRUE(task.isBusy()); - EXPECT_TRUE(task.stopActivity()); - - ticket = gate.beginStopAll(); - EXPECT_FALSE(gate.finishStopAll(ticket, false)); - EXPECT_FALSE(task.touch(10, 20)); - EXPECT_FALSE(task.startFromPixel(10, 20)); - - ticket = gate.beginStopAll(); - EXPECT_TRUE(gate.finishStopAll(ticket, true)); - EXPECT_TRUE(task.startFromPixel(30, 40)); - EXPECT_TRUE(task.stopActivity()); -} - -TEST_F(TouchScreenAdmissionTest, - SafetyHooksFenceEveryArmActuation) -{ - auto arm = std::make_shared(); - auto camera = std::make_shared( - "touch-safety-hooks-camera"); - cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); - cmvr::task::TouchScreenTaskAdmissionTestPeer::prepare( - task, arm, camera); - - bool admitted = true; - int revalidations = 0; - int dispatches = 0; - cmvr::task::TouchScreenTask::SafetyHooks hooks; - hooks.revalidate = [&] { - ++revalidations; - return admitted; - }; - hooks.dispatch_actuation = [&](const auto& operation) { - ++dispatches; - return operation(); - }; - hooks.dispatch_stop = [](const auto& operation) { - return operation(); - }; - - ASSERT_TRUE(task.touchIfCurrent( - 10, 20, [] { return true; }, std::move(hooks))); - EXPECT_TRUE( - cmvr::task::TouchScreenTaskAdmissionTestPeer::sendJointVelocity(task)); - EXPECT_EQ(arm->speedJCalls(), 1); - EXPECT_EQ(dispatches, 1); - EXPECT_GE(revalidations, 2); - - admitted = false; - EXPECT_FALSE( - cmvr::task::TouchScreenTaskAdmissionTestPeer::sendJointVelocity(task)); - EXPECT_EQ(arm->speedJCalls(), 1); - EXPECT_EQ(dispatches, 1); - EXPECT_TRUE(task.stopActivity()); -} - -TEST_F(TouchScreenAdmissionTest, - RevokedSafetySessionStopsActivityThroughStopLane) -{ - auto arm = std::make_shared(); - auto camera = std::make_shared( - "touch-revoked-safety-camera"); - cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); - cmvr::task::TouchScreenTaskAdmissionTestPeer::prepare( - task, arm, camera); - - bool admitted = true; - int stop_dispatches = 0; - cmvr::task::TouchScreenTask::SafetyHooks hooks; - hooks.revalidate = [&] { return admitted; }; - hooks.dispatch_actuation = [](const auto& operation) { - return operation(); - }; - hooks.dispatch_stop = [&](const auto& operation) { - ++stop_dispatches; - return operation(); - }; - - ASSERT_TRUE(task.touchIfCurrent( - 10, 20, [] { return true; }, std::move(hooks))); - admitted = false; - EXPECT_FALSE(task.step(0.01)); - EXPECT_EQ(task.phase(), cmvr::task::TouchScreenTask::Phase::FAILED); - EXPECT_EQ( - task.lastStatus(), - cmvr::task::TouchScreenTask::Status::SAFETY_ADMISSION_REVOKED); - EXPECT_EQ(stop_dispatches, 1); - EXPECT_EQ(arm->stopMotionCalls(), 1); -} - -TEST_F(TouchScreenAdmissionTest, - StopAllStopsCameraActivityAndNextTouchRestartsIt) -{ - auto arm = std::make_shared(); - auto camera = std::make_shared( - "touch-restart-camera"); - cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); - cmvr::task::TouchScreenTaskAdmissionTestPeer::prepare( - task, arm, camera); - auto& gate = cmvr::service::globalStopAllAdmissionGate(); - auto& registry = - cmvr::service::globalCameraOperationalActivityRegistry(); - - ASSERT_TRUE(task.touch(10, 20)); - EXPECT_TRUE(camera->operationalActive()); - EXPECT_EQ(camera->operationalStartCalls(), 1); - - const auto ticket = gate.beginStopAll(); - ASSERT_TRUE(ticket.valid()); - EXPECT_TRUE(task.stopActivity()); - EXPECT_TRUE(registry.stopAllActivities()); - EXPECT_FALSE(camera->operationalActive()); - EXPECT_EQ(camera->operationalStopCalls(), 1); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - ASSERT_TRUE(gate.finishStopAll(ticket, true)); - - ASSERT_TRUE(task.touch(30, 40)); - EXPECT_TRUE(camera->operationalActive()); - EXPECT_EQ(camera->operationalStartCalls(), 2); - EXPECT_EQ(camera->lifecycleStartCalls(), 0); - EXPECT_TRUE(task.stopActivity()); -} - -TEST_F(TouchScreenAdmissionTest, - TouchRollsBackCameraStartThatCrossesStopAllGeneration) -{ - auto arm = std::make_shared(); - auto camera = std::make_shared( - "touch-racing-camera"); - cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); - cmvr::task::TouchScreenTaskAdmissionTestPeer::prepare( - task, arm, camera); - camera->blockStart(); - - bool touch_result = true; - std::thread touch_thread([&] { - touch_result = task.touch(10, 20); - }); - camera->waitForStartEntered(); - - auto& gate = cmvr::service::globalStopAllAdmissionGate(); - const auto ticket = gate.beginStopAll(); - EXPECT_TRUE(ticket.valid()); - camera->releaseStart(); - touch_thread.join(); - - EXPECT_FALSE(touch_result); - EXPECT_FALSE(task.isBusy()); - EXPECT_FALSE(camera->operationalActive()); - EXPECT_EQ(camera->operationalStartCalls(), 1); - EXPECT_EQ(camera->operationalStopCalls(), 1); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - EXPECT_TRUE(gate.finishStopAll(ticket, true)); -} - -TEST_F(TouchScreenAdmissionTest, - InitRollsBackCameraStartThatCrossesStopAllGeneration) -{ - cmvr::config::DeviceManagerConfig manager_config; - auto& manager = - cmvr::device::DeviceManager::getInstance(manager_config); - auto camera = std::make_shared( - "touch-init-racing-camera"); - manager.registerDevice(camera); - - cmvr::task::TouchScreenTask task(cmvr::config::TouchScreenTaskConfig{}); - cmvr::task::TouchScreenTaskAdmissionTestPeer::prepareInit( - task, - "missing-arm", - "missing-dexhand", - camera->id()); - camera->blockStart(); - - bool init_result = true; - std::thread init_thread([&] { - init_result = task.init(); - }); - camera->waitForStartEntered(); - - auto& gate = cmvr::service::globalStopAllAdmissionGate(); - const auto ticket = gate.beginStopAll(); - EXPECT_TRUE(ticket.valid()); - camera->releaseStart(); - init_thread.join(); - - EXPECT_FALSE(init_result); - EXPECT_EQ(task.state(), cmvr::task::TaskState::UNINITIALIZED); - EXPECT_FALSE(camera->operationalActive()); - EXPECT_EQ(camera->operationalStartCalls(), 1); - EXPECT_EQ(camera->operationalStopCalls(), 1); - EXPECT_EQ(camera->lifecycleStopCalls(), 0); - EXPECT_TRUE(gate.finishStopAll(ticket, true)); -} - -} // namespace 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 85572300..7640cc33 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,8 +12,6 @@ #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 { @@ -279,17 +277,6 @@ 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; @@ -307,73 +294,12 @@ 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) { + if (!camera || !camera->start()) { CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: " << devices.camera_id(); 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( - 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; + return init(arm, dexhand, camera); } bool TouchScreenTask::init(const std::shared_ptr& arm, @@ -381,10 +307,6 @@ 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; @@ -461,283 +383,16 @@ bool TouchScreenTask::init(const std::shared_ptr& arm, } bool TouchScreenTask::touch(const int u, const int v) { - 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; - } - 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(); + return startFromPixelUnlocked(u, v); } bool TouchScreenTask::startFromPixel(const int u, const int v) { - return touchIfCurrent(u, v, [] { return true; }); + std::lock_guard lock(mutex_); + return startFromPixelUnlocked(u, v); } bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { @@ -750,6 +405,7 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { return false; } + stopUnlocked(); if (!moveToInitPositionBeforeStartIfEnabled()) { return false; } @@ -786,32 +442,12 @@ 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) { - enterFailed(Status::INVALID_CONFIG); + last_status_ = Status::INVALID_CONFIG; return false; } @@ -858,102 +494,26 @@ bool TouchScreenTask::step(const double dt) { case Phase::FAILED: return false; } - enterFailed(Status::INVALID_CONFIG); + last_status_ = Status::INVALID_CONFIG; return false; } void TouchScreenTask::stop() { - (void)stopActivity(); + std::lock_guard lock(mutex_); + stopUnlocked(); } -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()) { +void TouchScreenTask::stopUnlocked() { + if (arm_) { try { - 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(); + arm_->stopL(); } catch (...) { } } - 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() { + sendZeroJointVelocity(); ibvs_.resetTwistCommandState(); + holdCurrentControlledPosition(); phase_ = Phase::IDLE; phase_after_retract_ = Phase::DONE; @@ -1057,14 +617,6 @@ 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()); } @@ -1108,7 +660,6 @@ 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"; @@ -1777,22 +1328,21 @@ bool TouchScreenTask::stepRetracting() { << ", final_tcp_delta_base=unavailable"; } - if (!runArmStopIfCurrent([this] { - return arm_ && arm_->stopL().ok(); - })) { + try { + arm_->stopL(); + } catch (...) { enterFailed(Status::ROBOT_COMMAND_FAILED); return false; } holdCurrentControlledPosition(); if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) && !moveToInitPositionIfEnabled()) { - finishActivityUnlocked( - Phase::FAILED, Status::ROBOT_COMMAND_FAILED); + phase_ = Phase::FAILED; + last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } - const auto completed_phase = phase_after_retract_; - const auto completed_status = final_status_after_retract_; - finishActivityUnlocked(completed_phase, completed_status); + phase_ = phase_after_retract_; + last_status_ = final_status_after_retract_; return phase_ != Phase::FAILED; } @@ -1829,9 +1379,11 @@ bool TouchScreenTask::sendJointVelocity(const std::vector& qdot) const { device::JointVelocityCommand cmd; cmd.velocity = qdot; - return runArmActuationIfCurrent([this, &cmd] { - return arm_ && arm_->speedJ(cmd, 0.0, 0.0).ok(); - }); + const auto result = arm_->speedJ(cmd, 0.0, 0.0); + if (!result.ok()) { + return false; + } + return true; } bool TouchScreenTask::sendZeroJointVelocity() const { @@ -1918,9 +1470,11 @@ bool TouchScreenTask::holdCurrentControlledPosition() const { joints.position.push_back(it->second); } - return runArmActuationIfCurrent([this, &joints] { - return arm_ && arm_->servoJ(joints).ok(); - }); + const auto result = arm_->servoJ(joints); + if (!result.ok()) { + return false; + } + return true; } bool TouchScreenTask::buildInitJointPositions(std::vector& positions_out) const { @@ -1946,10 +1500,8 @@ bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() { device::MotionOptions options; options.velocity = config_.initialization().velocity(); options.acceleration = config_.initialization().acceleration(); - options.cancellation_requested = activityCancellationRequested(); - if (!runArmActuationIfCurrent([this, &init_cmd, &options] { - return arm_ && arm_->moveJ(init_cmd, options).ok(); - })) { + const auto result = arm_->moveJ(init_cmd, options); + if (!result.ok()) { last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } @@ -1969,17 +1521,17 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const { device::MotionOptions options; options.velocity = config_.initialization().velocity(); options.acceleration = config_.initialization().acceleration(); - options.cancellation_requested = activityCancellationRequested(); - return runArmActuationIfCurrent([this, &init_cmd, &options] { - return arm_ && arm_->moveJ(init_cmd, options).ok(); - }); + const auto result = arm_->moveJ(init_cmd, options); + if (!result.ok()) { + return false; + } + return true; } bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) { if (stop_forward_motion) { - if (!runArmStopIfCurrent([this] { - return arm_ && arm_->stopL().ok(); - })) { + const auto result = arm_->stopL(); + if (!result.ok()) { return false; } } @@ -2013,15 +1565,12 @@ bool TouchScreenTask::startTouchPhase() { last_status_ = Status::ROBOT_STATE_FAILED; return false; } - 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(); - })) { + 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()) { last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } @@ -2047,11 +1596,8 @@ 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()); - options.cancellation_requested = activityCancellationRequested(); - if (!runArmActuationIfCurrent([this, &pose_cmd, &options] { - return arm_ && arm_->moveL( - pose_cmd, options, device::FrameType::Tool).ok(); - })) { + const auto result = arm_->moveL(pose_cmd, options, device::FrameType::Tool); + if (!result.ok()) { last_status_ = Status::ROBOT_COMMAND_FAILED; return false; } @@ -2061,9 +1607,12 @@ bool TouchScreenTask::startTouchPhase() { return false; } - finishActivityUnlocked(Phase::DONE, Status::DONE); + phase_ = Phase::DONE; + touch_command_started_ = false; + retract_command_started_ = false; retract_start_position_valid_ = false; retract_start_position_base_.setZero(); + last_status_ = Status::DONE; return true; } @@ -2095,14 +1644,13 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, << ", start_tcp_base=unavailable"; } - 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"; + 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; return false; } @@ -2117,15 +1665,18 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, } void TouchScreenTask::enterFailed(const Status status) { - (void)runArmStopIfCurrent([this] { - return arm_ && arm_->stopL().ok(); - }); + try { + if (arm_) { + arm_->stopL(); + } + } catch (...) { + } hardStopIbvsMotion(); holdCurrentControlledPosition(); - const auto final_status = moveToInitPositionIfEnabled() - ? status - : Status::ROBOT_COMMAND_FAILED; - finishActivityUnlocked(Phase::FAILED, final_status); + phase_ = Phase::FAILED; + touch_command_started_ = false; + retract_command_started_ = false; + last_status_ = moveToInitPositionIfEnabled() ? status : Status::ROBOT_COMMAND_FAILED; } 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 4ca05918..55a45b27 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,7 +11,6 @@ #include #include #include -#include #include #include #include @@ -34,33 +33,6 @@ #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; @@ -72,168 +44,6 @@ 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"; @@ -612,60 +422,6 @@ 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/cmvr-es/task/ume_teleop_task/CMakeLists.txt b/cmvr-es/task/ume_teleop_task/CMakeLists.txt deleted file mode 100644 index 1b3b6362..00000000 --- a/cmvr-es/task/ume_teleop_task/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -find_package(Threads REQUIRED) - -add_library(ume_teleop_task STATIC - src/ume_teleop_task.cpp -) -target_compile_features(ume_teleop_task PUBLIC cxx_std_17) -target_include_directories(ume_teleop_task PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-es) -target_link_libraries(ume_teleop_task - PUBLIC - cmvr_es::task - cmvr_es::arm_teleop_client - cmvr_es::proto - PRIVATE - cmvr_es::logging - cmvr_es::stop_all_admission_gate - Threads::Threads -) - -add_library(cmvr_es::ume_teleop_task ALIAS ume_teleop_task) -install(TARGETS ume_teleop_task ARCHIVE DESTINATION lib) - -if(BUILD_TESTING) - add_executable(ume_teleop_task_test - tests/ume_teleop_task_test.cpp - ) - target_compile_features(ume_teleop_task_test PRIVATE cxx_std_17) - target_link_libraries(ume_teleop_task_test - PRIVATE - cmvr_es::ume_teleop_task - cmvr_es::stop_all_admission_gate - Threads::Threads - ) - add_test(NAME ume_teleop_task_test COMMAND ume_teleop_task_test) - set(_ume_teleop_task_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _ume_teleop_task_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(ume_teleop_task_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_ume_teleop_task_test_environment}") -endif() diff --git a/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h b/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h deleted file mode 100644 index 14353b8d..00000000 --- a/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h +++ /dev/null @@ -1,101 +0,0 @@ -#ifndef CMVR_ES_UME_TELEOP_TASK_H -#define CMVR_ES_UME_TELEOP_TASK_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cmvr/config/ume_teleop_config/ume_teleop_config.pb.h" -#include "service/grpc/client/include/grpc_arm_teleop_client.h" -#include "task/task.h" - -namespace cmvr::task { - -class UmeTeleopTask final : public Task { -public: - explicit UmeTeleopTask( - const config::UmeTeleopConfig& config, - std::shared_ptr client = {}); - ~UmeTeleopTask() override; - - const std::string& id() const override { return id_; } - TaskRunMode runMode() const override { return TaskRunMode::BLOCKING_SERVICE; } - bool init() override; - bool start() override; - bool step(double dt) override; - void stop() override; - bool stopActivity() override; - - TaskState state() const override; - bool isBusy() const override; - bool isFinished() const override; - bool isFailed() const override; - std::string stateString() const override; - std::string detailStatusString() const override; - - // Thread-safe, capacity-one command mailbox. The caller provides only - // already-computed joint values; sequence is assigned by the sender loop. - // A newer command replaces an unsent older command. Submission is rejected - // unless the current session is ready, and pending values are discarded - // across disconnect/reconnect so motion cannot resume from stale intent. - bool submitSetpoint( - const api::armteleop::v1::JointSetpoint& setpoint); - -private: - using Clock = std::chrono::steady_clock; - - struct PendingSetpoint { - api::armteleop::v1::JointSetpoint value; - Clock::time_point submitted; - }; - - bool validateConfig(std::string& error) const; - void run(); - void runSender(); - void handleServerFrame( - const api::armteleop::v1::ServerFrame& frame); - - config::UmeTeleopConfig config_; - std::string id_; - std::shared_ptr client_; - - mutable std::mutex lifecycle_mutex_; - mutable std::mutex mutex_; - std::condition_variable stop_condition_; - std::thread worker_; - std::thread sender_; - std::atomic stop_requested_{false}; - TaskState state_{TaskState::UNINITIALIZED}; - std::string last_error_; - std::string session_id_; - std::string server_detail_; - api::armteleop::v1::SessionPhase server_phase_{ - api::armteleop::v1::SESSION_PHASE_UNSPECIFIED}; - std::uint64_t connection_attempts_{0}; - - bool receiver_session_active_{false}; - bool session_ready_{false}; - std::uint64_t client_session_generation_{0}; - std::uint32_t negotiated_watchdog_ms_{0}; - - std::optional pending_setpoint_; - std::uint64_t mailbox_replacements_{0}; - std::uint64_t stale_setpoints_dropped_{0}; - std::uint64_t heartbeats_sent_{0}; - std::uint64_t setpoints_sent_{0}; - - bool stop_write_attempted_{false}; - bool stop_write_succeeded_{false}; -}; - -void registerUmeTeleopTaskFactory(); - -} // namespace cmvr::task - -#endif // CMVR_ES_UME_TELEOP_TASK_H diff --git a/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp b/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp deleted file mode 100644 index c793f074..00000000 --- a/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp +++ /dev/null @@ -1,705 +0,0 @@ -#include "task/ume_teleop_task/include/ume_teleop_task.h" - -#include -#include -#include -#include -#include - -#include -#include - -#include "cmvr/config/task_manager_config/task_manager_config.pb.h" -#include "common/base/logging/logger.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" -#include "common/config/config_files.h" -#include "task/task_factory.h" - -namespace cmvr::task { -namespace { - -constexpr auto kStopWriteGrace = std::chrono::milliseconds(50); -constexpr auto kStopAckGrace = std::chrono::milliseconds(20); - -std::shared_ptr createUmeTeleopTask( - const config::TaskConfigEntry& entry) -{ - if (entry.id().empty() || entry.config_file().empty()) { - CMVR_LOG(ERROR) << "[UmeTeleopTask] Task id or config_file is empty"; - return nullptr; - } - - config::UmeTeleopRootConfig root; - if (!ConfigHelper::loadConfigFile(entry.config_file(), root)) { - CMVR_LOG(ERROR) << "[UmeTeleopTask] Failed to load config: " - << entry.config_file(); - return nullptr; - } - const auto& config = root.ume_teleop(); - if (config.id().empty() || config.id() != entry.id()) { - CMVR_LOG(ERROR) << "[UmeTeleopTask] Task ID mismatch: manager=" - << entry.id() << ", config=" << config.id(); - return nullptr; - } - return std::make_shared(config); -} - -std::string grpcStatusDetail(const grpc::Status& status) -{ - std::ostringstream output; - output << "gRPC code=" << static_cast(status.error_code()); - if (!status.error_message().empty()) { - output << " message=" << status.error_message(); - } - return output.str(); -} - -} // namespace - -UmeTeleopTask::UmeTeleopTask( - const config::UmeTeleopConfig& config, - std::shared_ptr client) - : config_(config), id_(config.id()), client_(std::move(client)) -{ -} - -UmeTeleopTask::~UmeTeleopTask() -{ - stop(); -} - -bool UmeTeleopTask::init() -{ - std::lock_guard lifecycle_lock(lifecycle_mutex_); - std::lock_guard lock(mutex_); - if (state_ == TaskState::IDLE) { - return true; - } - if (worker_.joinable() || sender_.joinable()) { - last_error_ = "cannot initialize while worker is running"; - state_ = TaskState::FAILED; - return false; - } - - std::string error; - if (!validateConfig(error)) { - last_error_ = std::move(error); - state_ = TaskState::FAILED; - return false; - } - - if (!client_) { - auto channel = grpc::CreateChannel( - config_.server_address(), - grpc::InsecureChannelCredentials()); - if (!channel) { - last_error_ = "failed to create gRPC channel"; - state_ = TaskState::FAILED; - return false; - } - client_ = std::make_shared( - std::move(channel)); - } - - stop_requested_ = false; - last_error_.clear(); - session_id_.clear(); - server_detail_.clear(); - server_phase_ = api::armteleop::v1::SESSION_PHASE_UNSPECIFIED; - connection_attempts_ = 0; - receiver_session_active_ = false; - session_ready_ = false; - client_session_generation_ = 0; - negotiated_watchdog_ms_ = 0; - pending_setpoint_.reset(); - mailbox_replacements_ = 0; - stale_setpoints_dropped_ = 0; - heartbeats_sent_ = 0; - setpoints_sent_ = 0; - stop_write_attempted_ = false; - stop_write_succeeded_ = false; - state_ = TaskState::IDLE; - return true; -} - -bool UmeTeleopTask::start() -{ - 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(); - } - - std::unique_lock lifecycle_lock(lifecycle_mutex_); - // A start request may have waited behind stopActivity(). Recheck the exact - // generation before creating workers, but do not retain the global guard - // while starting or cleaning up task threads. - { - auto admission = admission_gate.lockAdmission(); - if (!admission.accepting() || - admission.generation() != admission_generation) { - return false; - } - } - std::unique_lock lock(mutex_); - if (state_ == TaskState::RUNNING) { - lock.unlock(); - auto current = admission_gate.lockAdmission(); - return current.accepting() && - current.generation() == admission_generation; - } - if ((state_ != TaskState::IDLE && state_ != TaskState::STOPPED) || - !client_ || worker_.joinable() || sender_.joinable()) { - last_error_ = "UME teleop task is not initialized"; - state_ = TaskState::FAILED; - return false; - } - - stop_requested_ = false; - session_ready_ = false; - client_session_generation_ = 0; - negotiated_watchdog_ms_ = 0; - pending_setpoint_.reset(); - stop_write_attempted_ = false; - stop_write_succeeded_ = false; - try { - worker_ = std::thread(&UmeTeleopTask::run, this); - sender_ = std::thread(&UmeTeleopTask::runSender, this); - } catch (const std::exception& error) { - last_error_ = std::string("failed to start worker: ") + error.what(); - stop_requested_ = true; - stop_condition_.notify_all(); - auto client = client_; - std::thread worker; - std::thread sender; - if (worker_.joinable()) { - worker = std::move(worker_); - } - if (sender_.joinable()) { - sender = std::move(sender_); - } - lock.unlock(); - client->tryCancel(); - if (sender.joinable()) { - sender.join(); - } - if (worker.joinable()) { - worker.join(); - } - lock.lock(); - state_ = TaskState::FAILED; - return false; - } catch (...) { - last_error_ = "failed to start worker"; - stop_requested_ = true; - stop_condition_.notify_all(); - auto client = client_; - std::thread worker; - std::thread sender; - if (worker_.joinable()) { - worker = std::move(worker_); - } - if (sender_.joinable()) { - sender = std::move(sender_); - } - lock.unlock(); - client->tryCancel(); - if (sender.joinable()) { - sender.join(); - } - if (worker.joinable()) { - worker.join(); - } - lock.lock(); - state_ = TaskState::FAILED; - return false; - } - - state_ = TaskState::RUNNING; - lock.unlock(); - - bool admission_current = false; - { - auto admission = admission_gate.lockAdmission(); - admission_current = admission.accepting() && - admission.generation() == admission_generation; - } - if (admission_current) { - return true; - } - - // Let stopActivity take the lifecycle lock and roll back every worker - // created by this stale start request. - lifecycle_lock.unlock(); - (void)stopActivity(); - return false; -} - -bool UmeTeleopTask::step(const double dt) -{ - (void)dt; - return !isFailed(); -} - -void UmeTeleopTask::stop() -{ - (void)stopActivity(); -} - -bool UmeTeleopTask::stopActivity() -{ - std::lock_guard lifecycle_lock(lifecycle_mutex_); - std::shared_ptr client; - std::thread worker; - std::thread sender; - { - std::unique_lock lock(mutex_); - stop_requested_ = true; - stop_condition_.notify_all(); - client = client_; - - // The sender is the only normal writer. Give it a bounded opportunity - // to put StopSession on the stream before cancellation interrupts a - // blocked Write or Read. - if (sender_.joinable()) { - stop_condition_.wait_for( - lock, - kStopWriteGrace, - [this] { return stop_write_attempted_; }); - if (stop_write_succeeded_ && receiver_session_active_) { - stop_condition_.wait_for( - lock, - kStopAckGrace, - [this] { return !receiver_session_active_; }); - } - sender = std::move(sender_); - } - if (worker_.joinable()) { - worker = std::move(worker_); - } - } - - // Do not hold the task mutex while cancelling or joining: the receive - // callback and the worker exit path both update task status under it. - if (client) { - client->tryCancel(); - } - if (sender.joinable()) { - sender.join(); - } - if (worker.joinable()) { - worker.join(); - } - - bool stopped = !client || !client->isSessionActive(); - { - std::lock_guard lock(mutex_); - stopped = stopped && !worker_.joinable() && !sender_.joinable() && - !receiver_session_active_; - if (state_ != TaskState::FAILED) { - state_ = TaskState::STOPPED; - } - } - return stopped; -} - -TaskState UmeTeleopTask::state() const -{ - std::lock_guard lock(mutex_); - return state_; -} - -bool UmeTeleopTask::isBusy() const -{ - return state() == TaskState::RUNNING; -} - -bool UmeTeleopTask::isFinished() const -{ - return state() == TaskState::STOPPED; -} - -bool UmeTeleopTask::isFailed() const -{ - return state() == TaskState::FAILED; -} - -std::string UmeTeleopTask::stateString() const -{ - return taskStateToString(state()); -} - -std::string UmeTeleopTask::detailStatusString() const -{ - std::lock_guard lock(mutex_); - std::ostringstream output; - output << taskStateToString(state_) - << " target=" << config_.server_address() - << " attempts=" << connection_attempts_ - << " phase=" - << api::armteleop::v1::SessionPhase_Name(server_phase_) - << " watchdog_ms=" << negotiated_watchdog_ms_ - << " heartbeats=" << heartbeats_sent_ - << " setpoints=" << setpoints_sent_ - << " mailbox_replacements=" << mailbox_replacements_ - << " stale_dropped=" << stale_setpoints_dropped_; - if (!session_id_.empty()) { - output << " session=" << session_id_; - } - if (!server_detail_.empty()) { - output << " server_detail=" << server_detail_; - } - if (!last_error_.empty()) { - output << " error=" << last_error_; - } - return output.str(); -} - -bool UmeTeleopTask::submitSetpoint( - const api::armteleop::v1::JointSetpoint& setpoint) -{ - if (setpoint.valid_for_us() == 0U) { - return false; - } - - std::lock_guard lock(mutex_); - if (state_ != TaskState::RUNNING || - stop_requested_.load(std::memory_order_acquire) || - !session_ready_ || - client_session_generation_ == 0U) { - return false; - } - if (pending_setpoint_.has_value()) { - ++mailbox_replacements_; - } - - PendingSetpoint pending; - pending.value = setpoint; - // The Task owns the only session sequence generator. - pending.value.set_sequence(0); - pending.submitted = Clock::now(); - pending_setpoint_ = std::move(pending); - stop_condition_.notify_all(); - return true; -} - -bool UmeTeleopTask::validateConfig(std::string& error) const -{ - if (id_.empty()) { - error = "UME teleop task id is empty"; - return false; - } - if (config_.server_address().empty()) { - error = "UME teleop server_address is empty"; - return false; - } - if (!config_.allow_insecure()) { - error = - "M6 requires explicit allow_insecure=true; TLS is not configured"; - return false; - } - - const auto& open = config_.open_session(); - if (open.protocol_major() == 0U || - open.client_instance_id().empty() || - open.expected_robot().robot_id().empty() || - open.expected_robot().joint_names().empty() || - open.requested_command_rate_hz() == 0U || - open.requested_state_rate_hz() == 0U || - open.watchdog_timeout_ms() == 0U || - open.requested_lease_ms() == 0U) { - error = "UME teleop OpenSession configuration is incomplete"; - return false; - } - - const auto& reconnect = config_.reconnect(); - if (reconnect.initial_delay_ms() == 0U || - reconnect.maximum_delay_ms() < reconnect.initial_delay_ms() || - !std::isfinite(reconnect.multiplier()) || - reconnect.multiplier() < 1.0) { - error = "UME teleop reconnect configuration is invalid"; - return false; - } - return true; -} - -void UmeTeleopTask::run() -{ - auto backoff = - std::chrono::milliseconds(config_.reconnect().initial_delay_ms()); - const auto maximum_backoff = - std::chrono::milliseconds(config_.reconnect().maximum_delay_ms()); - - while (true) { - { - std::lock_guard lock(mutex_); - if (stop_requested_) { - break; - } - ++connection_attempts_; - receiver_session_active_ = true; - session_ready_ = false; - client_session_generation_ = 0; - negotiated_watchdog_ms_ = 0; - // A command produced for an old or disconnected session must - // never become the first motion command after reconnect. - pending_setpoint_.reset(); - stop_condition_.notify_all(); - } - - const grpc::Status status = client_->runSession( - config_.open_session(), - [this](const api::armteleop::v1::ServerFrame& frame) { - handleServerFrame(frame); - }, - [this] { - return stop_requested_.load(std::memory_order_acquire); - }); - - std::unique_lock lock(mutex_); - receiver_session_active_ = false; - session_ready_ = false; - client_session_generation_ = 0; - negotiated_watchdog_ms_ = 0; - pending_setpoint_.reset(); - stop_condition_.notify_all(); - if (stop_requested_) { - break; - } - - last_error_ = status.ok() - ? "arm teleop peer closed the session" - : grpcStatusDetail(status); - if (stop_condition_.wait_for( - lock, - backoff, - [this] { - return stop_requested_.load(std::memory_order_acquire); - })) { - break; - } - - const double multiplied = - static_cast(backoff.count()) * - config_.reconnect().multiplier(); - const auto next_count = static_cast( - std::min(multiplied, static_cast(maximum_backoff.count()))); - backoff = std::chrono::milliseconds(std::max(1, next_count)); - } -} - -void UmeTeleopTask::runSender() -{ - const auto command_period = std::chrono::microseconds( - std::max( - 1U, - 1000000ULL / - static_cast( - config_.open_session().requested_command_rate_hz()))); - - std::uint64_t observed_generation = 0; - std::uint64_t next_sequence = 0; - auto next_command_time = Clock::now(); - auto next_heartbeat_time = Clock::time_point::max(); - - for (;;) { - std::optional command; - bool heartbeat = false; - bool stop = false; - std::uint64_t generation = 0; - std::uint64_t sequence = 0; - std::chrono::microseconds heartbeat_period{0}; - - { - std::unique_lock lock(mutex_); - for (;;) { - if (stop_requested_.load(std::memory_order_acquire)) { - stop = true; - generation = client_session_generation_; - break; - } - - if (!session_ready_ || - client_session_generation_ == 0 || - negotiated_watchdog_ms_ == 0U) { - stop_condition_.wait(lock, [this] { - return stop_requested_.load( - std::memory_order_acquire) || - (session_ready_ && - client_session_generation_ != 0 && - negotiated_watchdog_ms_ != 0U); - }); - continue; - } - - if (observed_generation != client_session_generation_) { - observed_generation = client_session_generation_; - next_sequence = 0; - next_command_time = Clock::now(); - heartbeat_period = std::chrono::microseconds( - std::max( - 1000U, - static_cast( - negotiated_watchdog_ms_) * - 1000ULL / 3ULL)); - next_heartbeat_time = Clock::now() + heartbeat_period; - } else { - heartbeat_period = std::chrono::microseconds( - std::max( - 1000U, - static_cast( - negotiated_watchdog_ms_) * - 1000ULL / 3ULL)); - } - - const auto now = Clock::now(); - if (pending_setpoint_.has_value()) { - const auto queued_age = - std::chrono::duration_cast( - now - pending_setpoint_->submitted); - if (queued_age.count() >= - pending_setpoint_->value.valid_for_us()) { - pending_setpoint_.reset(); - ++stale_setpoints_dropped_; - } - } - - if (pending_setpoint_.has_value() && - now >= next_command_time) { - command = std::move(pending_setpoint_); - pending_setpoint_.reset(); - generation = client_session_generation_; - sequence = ++next_sequence; - break; - } - if (now >= next_heartbeat_time) { - heartbeat = true; - generation = client_session_generation_; - sequence = ++next_sequence; - break; - } - - auto wake_time = next_heartbeat_time; - if (pending_setpoint_.has_value()) { - wake_time = std::min(wake_time, next_command_time); - } - stop_condition_.wait_until(lock, wake_time); - } - } - - if (stop) { - api::armteleop::v1::StopSession stop_frame; - stop_frame.set_reason( - api::armteleop::v1::STOP_REASON_CLIENT_SHUTDOWN); - stop_frame.set_detail("UME teleoperation task stopped"); - const bool sent = client_->sendStop(stop_frame, generation); - { - std::lock_guard lock(mutex_); - stop_write_attempted_ = true; - stop_write_succeeded_ = sent; - } - stop_condition_.notify_all(); - return; - } - - bool sent = false; - if (command.has_value()) { - const auto queued_age = - std::chrono::duration_cast( - Clock::now() - command->submitted); - const auto original_validity = - static_cast( - command->value.valid_for_us()); - if (queued_age.count() >= 0 && - static_cast(queued_age.count()) < - original_validity) { - command->value.set_sequence(sequence); - command->value.set_valid_for_us( - static_cast( - original_validity - - static_cast(queued_age.count()))); - sent = client_->sendSetpoint( - command->value, generation); - } else { - std::lock_guard lock(mutex_); - ++stale_setpoints_dropped_; - continue; - } - } else if (heartbeat) { - api::armteleop::v1::ClientHeartbeat heartbeat_frame; - heartbeat_frame.set_sequence(sequence); - sent = client_->sendHeartbeat( - heartbeat_frame, generation); - } - - const auto sent_at = Clock::now(); - bool cancel_session = false; - { - std::lock_guard lock(mutex_); - if (generation == client_session_generation_) { - if (sent) { - next_heartbeat_time = - sent_at + heartbeat_period; - if (command.has_value()) { - ++setpoints_sent_; - next_command_time = - sent_at + command_period; - } else if (heartbeat) { - ++heartbeats_sent_; - } - } else { - session_ready_ = false; - cancel_session = true; - } - } - } - if (cancel_session) { - stop_condition_.notify_all(); - client_->tryCancel(); - } - } -} - -void UmeTeleopTask::handleServerFrame( - const api::armteleop::v1::ServerFrame& frame) -{ - if (!frame.has_status()) { - return; - } - - const std::uint64_t generation = - client_->activeSessionGeneration(); - std::lock_guard lock(mutex_); - server_phase_ = frame.status().phase(); - session_id_ = frame.status().session_id(); - server_detail_ = frame.status().detail(); - if (server_phase_ == api::armteleop::v1::SESSION_PHASE_OPENED || - server_phase_ == api::armteleop::v1::SESSION_PHASE_READY || - server_phase_ == api::armteleop::v1::SESSION_PHASE_ACTIVE) { - last_error_.clear(); - if (generation != 0 && - frame.status().negotiated_watchdog_ms() != 0U) { - client_session_generation_ = generation; - negotiated_watchdog_ms_ = - frame.status().negotiated_watchdog_ms(); - session_ready_ = true; - } - } else { - session_ready_ = false; - pending_setpoint_.reset(); - } - stop_condition_.notify_all(); -} - -void registerUmeTeleopTaskFactory() -{ - TaskFactory::registerCreator( - config::TaskConfigEntry::TASK_TYPE_UME_TELEOP, - createUmeTeleopTask); -} - -} // namespace cmvr::task diff --git a/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp b/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp deleted file mode 100644 index 67766831..00000000 --- a/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp +++ /dev/null @@ -1,530 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "cmvr/api/arm_teleop_v1.grpc.pb.h" -#include "service/grpc/client/include/grpc_arm_teleop_client.h" -#include "service/grpc/stop_all/include/stop_all_admission_gate.h" -#include "task/ume_teleop_task/include/ume_teleop_task.h" - -namespace { - -using namespace std::chrono_literals; -namespace api = cmvr::api::armteleop::v1; - -class WatchdogArmTeleopService final : public api::ArmTeleopService::Service { -public: - explicit WatchdogArmTeleopService( - const std::chrono::milliseconds watchdog) - : watchdog_(watchdog) - { - } - - grpc::Status Teleoperate( - grpc::ServerContext* context, - grpc::ServerReaderWriter* stream) override - { - api::ClientFrame frame; - if (!stream->Read(&frame) || !frame.has_open()) { - return grpc::Status( - grpc::StatusCode::INVALID_ARGUMENT, - "OpenSession must be first"); - } - { - std::lock_guard lock(mutex_); - open_received_ = true; - ++open_count_; - reader_finished_ = false; - last_sequence_ = 0; - last_activity_ = std::chrono::steady_clock::now(); - } - condition_.notify_all(); - - api::ServerFrame opened; - opened.mutable_status()->set_session_id("task-test-session"); - opened.mutable_status()->set_phase(api::SESSION_PHASE_OPENED); - opened.mutable_status()->set_negotiated_watchdog_ms( - static_cast(watchdog_.count())); - if (!stream->Write(opened)) { - return grpc::Status::OK; - } - - api::ServerFrame ready; - ready.mutable_status()->set_session_id("task-test-session"); - ready.mutable_status()->set_phase(api::SESSION_PHASE_READY); - ready.mutable_status()->set_negotiated_watchdog_ms( - static_cast(watchdog_.count())); - if (!stream->Write(ready)) { - return grpc::Status::OK; - } - - std::thread reader([&] { - api::ClientFrame incoming; - while (stream->Read(&incoming)) { - const auto arrived = std::chrono::steady_clock::now(); - bool terminal = false; - { - std::lock_guard lock(mutex_); - if (incoming.has_heartbeat() || - incoming.has_setpoint()) { - const std::uint64_t sequence = - incoming.has_heartbeat() - ? incoming.heartbeat().sequence() - : incoming.setpoint().sequence(); - if (sequence == 0 || sequence <= last_sequence_) { - sequence_valid_ = false; - } - last_sequence_ = sequence; - last_activity_ = arrived; - ++activity_version_; - if (incoming.has_heartbeat()) { - ++heartbeat_count_; - } else { - setpoints_.push_back(incoming.setpoint()); - } - } else if (incoming.has_stop()) { - stop_received_ = true; - ++stop_count_; - terminal = true; - } - if (terminal) { - reader_finished_ = true; - } - } - condition_.notify_all(); - incoming.Clear(); - if (terminal) { - break; - } - } - { - std::lock_guard lock(mutex_); - reader_finished_ = true; - } - condition_.notify_all(); - }); - - bool expired = false; - { - std::unique_lock lock(mutex_); - std::uint64_t observed_activity = activity_version_; - while (!reader_finished_) { - const auto deadline = last_activity_ + watchdog_; - if (!condition_.wait_until( - lock, - deadline, - [&] { - return reader_finished_ || - activity_version_ != observed_activity; - })) { - watchdog_expired_ = true; - expired = true; - break; - } - observed_activity = activity_version_; - } - } - if (expired) { - context->TryCancel(); - } - if (reader.joinable()) { - reader.join(); - } - { - std::lock_guard lock(mutex_); - handler_finished_ = true; - ++handler_finish_count_; - } - condition_.notify_all(); - return expired - ? grpc::Status( - grpc::StatusCode::DEADLINE_EXCEEDED, - "test watchdog expired") - : grpc::Status::OK; - } - - bool waitForOpen(const std::chrono::milliseconds timeout) - { - return waitForOpenCount(1, timeout); - } - - bool waitForOpenCount( - const std::size_t count, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [&] { return open_count_ >= count; }); - } - - bool waitForHandlerFinish(const std::chrono::milliseconds timeout) - { - return waitForHandlerFinishCount(1, timeout); - } - - bool waitForHandlerFinishCount( - const std::size_t count, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [&] { return handler_finish_count_ >= count; }); - } - - bool waitForHeartbeatCount( - const std::size_t count, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [&] { return heartbeat_count_ >= count; }); - } - - bool waitForSetpointCount( - const std::size_t count, - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(mutex_); - return condition_.wait_for( - lock, timeout, [&] { return setpoints_.size() >= count; }); - } - - std::size_t setpointCount() const - { - std::lock_guard lock(mutex_); - return setpoints_.size(); - } - - std::size_t heartbeatCount() const - { - std::lock_guard lock(mutex_); - return heartbeat_count_; - } - - std::size_t stopCount() const - { - std::lock_guard lock(mutex_); - return stop_count_; - } - - api::JointSetpoint lastSetpoint() const - { - std::lock_guard lock(mutex_); - return setpoints_.empty() - ? api::JointSetpoint{} - : setpoints_.back(); - } - - bool watchdogExpired() const - { - std::lock_guard lock(mutex_); - return watchdog_expired_; - } - - bool sequenceValid() const - { - std::lock_guard lock(mutex_); - return sequence_valid_; - } - - bool stopReceived() const - { - std::lock_guard lock(mutex_); - return stop_received_; - } - -private: - const std::chrono::milliseconds watchdog_; - mutable std::mutex mutex_; - std::condition_variable condition_; - bool open_received_{false}; - bool reader_finished_{false}; - bool handler_finished_{false}; - bool watchdog_expired_{false}; - bool sequence_valid_{true}; - bool stop_received_{false}; - std::size_t open_count_{0}; - std::size_t handler_finish_count_{0}; - std::size_t stop_count_{0}; - std::uint64_t activity_version_{0}; - std::uint64_t last_sequence_{0}; - std::size_t heartbeat_count_{0}; - std::vector setpoints_; - std::chrono::steady_clock::time_point last_activity_{}; -}; - -cmvr::config::UmeTeleopConfig validConfig(const std::string& endpoint) -{ - cmvr::config::UmeTeleopConfig config; - config.set_id("ume_teleop_test"); - config.set_server_address(endpoint); - config.set_allow_insecure(true); - - auto* open = config.mutable_open_session(); - open->set_protocol_major(1); - open->set_protocol_minor(0); - open->set_client_instance_id("ume-task-test"); - open->set_requested_command_rate_hz(20); - open->set_requested_state_rate_hz(250); - open->set_watchdog_timeout_ms(120); - open->set_requested_lease_ms(500); - open->mutable_expected_robot()->set_robot_id("test-arm"); - open->mutable_expected_robot()->add_joint_names("joint1"); - - config.mutable_reconnect()->set_initial_delay_ms(10); - config.mutable_reconnect()->set_maximum_delay_ms(50); - config.mutable_reconnect()->set_multiplier(2.0); - return config; -} - -int fail(const std::string& detail) -{ - std::cerr << "ume_teleop_task_test: " << detail << '\n'; - return 1; -} - -} // namespace - -int main() -{ - auto& admission = cmvr::service::globalStopAllAdmissionGate(); - admission.clearForTesting(); - - { - cmvr::config::UmeTeleopConfig invalid; - invalid.set_id("invalid"); - cmvr::task::UmeTeleopTask task(invalid); - if (task.init() || task.state() != cmvr::task::TaskState::FAILED) { - return fail("invalid transport configuration was not rejected"); - } - task.stop(); - if (task.state() != cmvr::task::TaskState::FAILED) { - return fail("stop did not preserve an initialization failure"); - } - } - - { - // Exercise the start/stop race before a synchronous stream necessarily - // publishes its ClientContext. The Task's cancellation predicate closes - // this gap, while tryCancel() interrupts it once the context is visible. - const std::string unavailable_endpoint = - "unix:/tmp/cmvr_ume_teleop_unavailable_" + - std::to_string(static_cast(::getpid())) + ".sock"; - auto channel = grpc::CreateChannel( - unavailable_endpoint, - grpc::InsecureChannelCredentials()); - auto client = - std::make_shared(channel); - cmvr::task::UmeTeleopTask task( - validConfig(unavailable_endpoint), client); - if (!task.init() || !task.start()) { - return fail("immediate-stop task did not initialize and start"); - } - api::JointSetpoint disconnected_setpoint; - disconnected_setpoint.add_position_rad(1.0); - disconnected_setpoint.set_valid_for_us(100000); - if (task.submitSetpoint(disconnected_setpoint)) { - task.stop(); - return fail( - "task accepted a motion command without a ready session"); - } - const auto stop_begin = std::chrono::steady_clock::now(); - task.stop(); - if (std::chrono::steady_clock::now() - stop_begin > 2s || - task.state() != cmvr::task::TaskState::STOPPED) { - return fail("immediate Task stop did not cancel and join promptly"); - } - } - - WatchdogArmTeleopService service(120ms); - grpc::ServerBuilder builder; - int selected_port = 0; - builder.AddListeningPort( - "127.0.0.1:0", - grpc::InsecureServerCredentials(), - &selected_port); - builder.RegisterService(&service); - std::unique_ptr server = builder.BuildAndStart(); - if (!server || selected_port == 0) { - return fail("failed to start in-process gRPC server"); - } - const std::string endpoint = - "127.0.0.1:" + std::to_string(selected_port); - - auto channel = grpc::CreateChannel( - endpoint, - grpc::InsecureChannelCredentials()); - auto client = - std::make_shared(channel); - cmvr::task::UmeTeleopTask task(validConfig(endpoint), client); - - if (task.runMode() != cmvr::task::TaskRunMode::BLOCKING_SERVICE) { - server->Shutdown(); - return fail("task is not a BLOCKING_SERVICE"); - } - if (!task.init()) { - server->Shutdown(); - return fail("valid task did not initialize"); - } - - auto admission_ticket = admission.beginStopAll(); - if (task.start() || task.isBusy()) { - server->Shutdown(); - return fail("public start bypassed closed StopAll admission"); - } - if (!admission.finishStopAll(admission_ticket, true) || !task.start()) { - server->Shutdown(); - return fail("explicit start was not restored after successful StopAll"); - } - if (!service.waitForOpen(2s)) { - task.stop(); - server->Shutdown(); - return fail("task worker did not open the teleoperation stream"); - } - - // No application commands are submitted here. Heartbeats alone must keep - // the session alive for longer than the negotiated watchdog. - if (!service.waitForHeartbeatCount(4, 2s) || - service.watchdogExpired() || !task.isBusy()) { - task.stop(); - server->Shutdown(); - return fail("silent command stream did not survive via heartbeats"); - } - - api::JointSetpoint first; - first.set_sequence(999); // Task must replace caller-owned sequence values. - first.add_position_rad(1.0); - first.add_velocity_rad_s(0.0); - first.set_valid_for_us(100000); - if (!task.submitSetpoint(first) || - !service.waitForSetpointCount(1, 2s)) { - task.stop(); - server->Shutdown(); - return fail("first mailbox setpoint was not sent"); - } - - // The sender rate is 20 Hz. These arrive inside one command period, so the - // capacity-one mailbox must publish only the newest value. - for (int value = 2; value <= 4; ++value) { - api::JointSetpoint setpoint; - setpoint.set_sequence(1000 + value); - setpoint.add_position_rad(static_cast(value)); - setpoint.add_velocity_rad_s(0.0); - setpoint.set_valid_for_us(100000); - if (!task.submitSetpoint(setpoint)) { - task.stop(); - server->Shutdown(); - return fail("latest-only mailbox rejected a valid setpoint"); - } - } - if (!service.waitForSetpointCount(2, 2s)) { - task.stop(); - server->Shutdown(); - return fail("latest mailbox setpoint was not sent"); - } - std::this_thread::sleep_for(70ms); - const api::JointSetpoint latest = service.lastSetpoint(); - if (service.setpointCount() != 2 || - latest.position_rad_size() != 1 || - latest.position_rad(0) != 4.0 || - latest.sequence() == 0 || - latest.sequence() >= 1000) { - task.stop(); - server->Shutdown(); - return fail("mailbox did not collapse queued commands to the latest value"); - } - - const auto stop_begin = std::chrono::steady_clock::now(); - const bool activity_stopped = task.stopActivity(); - const auto stop_elapsed = std::chrono::steady_clock::now() - stop_begin; - if (!activity_stopped || stop_elapsed > 2s) { - server->Shutdown(); - return fail("stopActivity did not stop and join promptly"); - } - if (task.state() != cmvr::task::TaskState::STOPPED || - task.isBusy() || !task.isFinished()) { - server->Shutdown(); - return fail("task did not reach STOPPED after stopping its activity"); - } - if (!service.waitForHandlerFinish(2s)) { - server->Shutdown(); - return fail("server handler did not observe activity cancellation"); - } - if (!service.stopReceived()) { - server->Shutdown(); - return fail("stopActivity did not send StopSession before TryCancel"); - } - api::JointSetpoint stopped_setpoint; - stopped_setpoint.add_position_rad(5.0); - stopped_setpoint.set_valid_for_us(100000); - if (task.submitSetpoint(stopped_setpoint)) { - server->Shutdown(); - return fail("stopped activity still accepted a setpoint"); - } - const auto heartbeats_after_stop = service.heartbeatCount(); - const auto setpoints_after_stop = service.setpointCount(); - std::this_thread::sleep_for(200ms); - if (service.heartbeatCount() != heartbeats_after_stop || - service.setpointCount() != setpoints_after_stop) { - server->Shutdown(); - return fail("stopped activity continued sending heartbeat or setpoint frames"); - } - - if (!task.start() || - !service.waitForOpenCount(2, 2s) || - !service.waitForHeartbeatCount(heartbeats_after_stop + 1, 2s) || - !task.isBusy()) { - task.stop(); - server->Shutdown(); - return fail("explicit start did not create a new teleoperation activity"); - } - api::JointSetpoint restarted_setpoint; - restarted_setpoint.add_position_rad(6.0); - restarted_setpoint.set_valid_for_us(100000); - if (!task.submitSetpoint(restarted_setpoint) || - !service.waitForSetpointCount(setpoints_after_stop + 1, 2s)) { - task.stop(); - server->Shutdown(); - return fail("restarted activity did not send a setpoint"); - } - if (!task.stopActivity() || - !service.waitForHandlerFinishCount(2, 2s) || - service.stopCount() != 2 || - task.state() != cmvr::task::TaskState::STOPPED) { - server->Shutdown(); - return fail("restarted activity did not stop cleanly"); - } - - admission_ticket = admission.beginStopAll(); - if (admission.finishStopAll(admission_ticket, false) || task.start()) { - server->Shutdown(); - return fail("failed StopAll did not keep public start fail-closed"); - } - admission_ticket = admission.beginStopAll(); - if (!admission.finishStopAll(admission_ticket, true) || - !task.start() || !service.waitForOpenCount(3, 2s) || - !task.stopActivity() || - !service.waitForHandlerFinishCount(3, 2s)) { - server->Shutdown(); - return fail("successful StopAll did not restore a new explicit start"); - } - if (service.watchdogExpired() || !service.sequenceValid()) { - server->Shutdown(); - return fail("heartbeat/setpoint sequence or watchdog contract failed"); - } - - server->Shutdown(); - admission.clearForTesting(); - std::cout << "ume_teleop_task_test: PASS\n"; - return 0; -} diff --git a/docs/device_safety_control_plane_architecture.md b/docs/device_safety_control_plane_architecture.md deleted file mode 100644 index d6675cd9..00000000 --- a/docs/device_safety_control_plane_architecture.md +++ /dev/null @@ -1,1823 +0,0 @@ -# CMVR-ES 统一设备安全控制面架构设计 - -状态：Implemented in software；默认 Shadow rollout；真实硬件与发布验收待完成 - -修订：2026-08-14，具体认证实现延后，保留统一安全扩展边界并收紧匿名 Recover 暴露 - -范围：设备命令准入、控制权、StopAll、软件锁止恢复、gRPC 请求上下文与安全扩展边界、命令幂等、设备安全状态上报 - -实施方式：阶段 0 到阶段 5，共六个阶段 - -## 当前实施状态（2026-08-14） - -当前分支已经完成六阶段所需的软件骨架和主要入口迁移，但没有把“代码已具备能力”误写成 -“生产安全验收已完成”。源码默认配置仍使用 `SHADOW`、`INSECURE + DISABLED` 和 -`RECOVERY_DISABLED`；切换 `ENFORCE_SELECTED/ENFORCE_ALL` 或开放本机恢复前，必须先完成 -对应设备和部署环境的验收。 - -| 阶段 | 当前状态 | 已落地内容 | 尚未完成 | -| --- | --- | --- | --- | -| 0 | 软件完成 | 统一 RequestContext、完整 method policy/call guard、anonymous principal、显式 Insecure/Disabled 配置、RecoveryExposure 和持久审计边界 | TLS、Token、JWT、mTLS provider 按本轮决策不实现 | -| 1 | 软件完成 | safety types、SnapshotStore、设备 adapter、公共 reason/execution state、service instance、CommandLedger | 厂商结果语义仍需逐台真机校准 | -| 2 | 软件完成 | DeviceManager 持有 SafetyManager、legacy participant、GetSafetyState、启动 coverage 校验、Shadow/Enforce 配置 | 真实运行 Shadow 日志评审 | -| 3 | 软件完成 | Arm、ArmTeleop、AGV、Motor、DexHand、PTZ、ActionQueue 统一准入；dispatch 前 permit/final check；PTZ START/STOP 服务端派生 lane | 每类设备真实停车确认和 ACK/断网故障注入 | -| 4 | 软件完成 | 通用 participant StopAll、RecoveryLedger、RecoverSafetyState、LocalOnly/Authorized policy、审计失败 fail-closed、shutdown quiesce | LocalOnly 现场入口和审计文件运维验收 | -| 5 | 部分完成 | Camera/Microphone/Speaker/BioHead/HLC、TouchScreenTask、媒体 Hub/QUIC Sensor start 已迁移；Enforce 启动覆盖校验已实现 | 默认切换 EnforceAll、删除兼容 gate、capability manifest、安装产物 smoke、TSAN 和真机台架 | - -认证和证书不是本次整改的运行前提。当前只实现无证书的兼容 Profile；配置选择尚未实现的 -认证或 TLS 模式会启动失败，不会静默回退。匿名部署的恢复默认关闭，只有显式配置 -`RECOVERY_LOCAL_ONLY`、服务端确认实际 peer 为 loopback/Unix socket 且持久审计可写时才可开放。 - -AUBO 另有一条设备内硬件语义：真实硬件急停曾有效、随后输入消失且控制器重新报告 -`Normal/ReducedMode` 时，驱动会自动上电到 `Idle`、清理旧队列、执行 `startup()`，并在 -`Running` 下再次确认 quiescent 后解除该硬件锁存，使新的 gRPC 指令可以重新准入； -软件 `emergencyStop()` 使用独立 `SoftwareEmergencyStop` 锁存，即使它与硬件急停重叠也绝不被 -硬件输入释放自动清除。自动流程不 resume、不重放旧目标；显式 Stop/PowerOff 会取消本轮自动上电。 - -## 1. 决策摘要 - -本设计采用以下核心决策： - -1. `DeviceManager` 继续负责设备注册和生命周期，并持有一个独立、可测试的 - `SafetyManager`。状态机代码不直接堆入 `DeviceManager`。 -2. Service 不再自行组合 StopAll gate、设备状态和控制权判断。所有会改变设备或 - 活动状态的命令必须声明 `CommandIntent`，通过统一准入获得短生命周期的 - `AdmissionPermit`。 -3. 设备层不把厂商硬件规则上移。设备或设备适配器持续发布硬件事实，并在真正下发 - 命令前执行最后一次硬件安全检查。 -4. 生命周期、健康状态和安全状态保持正交。`Running`、`Healthy` 都不等于当前可以 - 接受执行器命令。 -5. 策略分为 `SensorPolicy` 和 `ControlPolicy` 两个默认族，但最终按命令意图判定。 - DexHand、Camera PTZ 等混合设备不能只按整个设备归类。 -6. Stop、状态查询和安全恢复使用独立安全通道，不受普通命令 gate 阻塞。 -7. 对外接口命名为 `RecoverSafetyState`。它只在重新验证硬件事实后清除软件准入锁止， - 不能忽略急停、保护停、未知状态或仍未确认的运动。 -8. 普通 unary 控制命令使用统一幂等账本；流式控制使用 session epoch 和严格递增的 - sequence。任何 `OUTCOME_UNKNOWN` 都不得自动重发。 -9. 当前小范围部署不把 TLS、Token、mTLS 等具体认证实现作为设备安全架构的前置条件， - 但阶段 0 必须先建立统一 `RequestContext`、认证提供者、授权策略和审计扩展边界。 -10. 认证关闭时由服务端注入固定的 `anonymous` principal，不能接受客户端自报身份或角色； - `RecoverSafetyState` 默认禁用，只有本机受限模式可以显式开放。 -11. 传输加密、身份认证和方法授权是三个正交层。后续增加静态 Token、JWT 或 mTLS 时， - 只能替换安全网关组件，不修改设备、`SafetyManager` 或业务 Proto。 -12. 该软件控制面不替代独立物理急停，也不声明 SIL、PL 或其他功能安全等级。 - -## 2. 改造前基础与需要保留的行为 - -项目已有一些经过并发测试的能力，应当迁移和复用，而不是重新实现： - -| 当前能力 | 目标用法 | -| --- | --- | -| `ControlAuthorityManager` 的 lease、generation、dispatch fence、quarantine | 作为 `SafetyManager` 内部控制权组件 | -| `StopAllAdmissionGate` 的并发 round 和失败后 fail-closed | 作为阶段 2、3 的兼容参与者，最终由统一状态机接管 | -| `ActionQueueExecutor` 的 `action_id`、service instance 和精确 retired ID 账本 | 作为普通命令幂等账本的语义模板 | -| Motor、Media、Camera activity coordinator | 先包装为 `SafetyParticipant`，最后逐步合并 | -| `StopOperationDispatcher` 的有界异步停止和生命周期管理 | 作为统一 StopAll 执行器的基础 | -| `DeviceManager::inventorySnapshot()` | StopAll 枚举设备时继续使用，禁止持锁进入驱动 | -| `Runtime` 的启动失败回滚和退出时 Task/Device 停止 | 保留，并在退出前增加全局安全准入关闭 | - -整改开始时的主要缺口是： - -- gRPC 默认全网卡明文监听，没有认证和授权；当前阶段可以接受该部署风险，但必须通过显式配置、 - 网络隔离和恢复接口限制控制暴露范围； -- 普通控制 RPC 缺少 `command_id` 和统一重试语义； -- StopAll 在 `grpc_system_service.cpp` 内按设备类型硬编码； -- Arm、AGV、Motor、Media、ActionQueue 分别维护 gate 和 generation； -- 只有少数设备实现 `healthSnapshot()`，且健康状态不能表示硬件安全事实； -- `DeviceManager::snapshot()` 仍会在调用线程逐个调用设备方法； -- 普通 service 直接取得设备并调用驱动，无法保证所有入口都经过同一准入； -- Aubo 等驱动存在“硬件可能已接受，但软件未观察到执行 ID”的不确定结果窗口； -- 错误类型没有统一映射，客户端容易对不应重试的命令进行重试。 - -当前实现锚点： - -- gRPC 监听和 credentials：`cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp`； -- SystemService StopAll：`cmvr-es/service/grpc/server/src/grpc_system_service.cpp`； -- 全局 gate：`cmvr-es/service/grpc/stop_all/include/stop_all_admission_gate.h`； -- 控制权：`cmvr-es/manager/control_authority_manager/include/control_authority_manager.h`； -- 设备生命周期和快照：`cmvr-es/manager/device_manager/`； -- 通用请求头：`protos/cmvr/api/common.proto`； -- System API：`protos/cmvr/api/system_service.proto` 和 `system_command.proto`。 - -## 3. 目标、非目标与安全不变量 - -### 3.1 目标 - -- 所有 gRPC、ActionQueue、Teleoperation 和未来 QUIC 控制入口共享同一安全准入。 -- 新增设备时，安全层只要求注册能力和策略，不修改 StopAll 或 SystemService 主流程。 -- 单个驱动、网络或状态刷新阻塞不能拖住其他设备的 Stop、Status 或 Recover。 -- StopAll、设备级不确定结果和进程重启都能通过 generation 使旧命令永久失效。 -- 锁止原因、状态新鲜度、阻塞设备和恢复结果能够被机器读取和审计。 -- 迁移期间保持现有 API wire compatibility，并允许按设备逐步启用 enforce。 -- 具体认证实现延后时，所有请求仍经过稳定的安全上下文边界；未来启用认证不需要改动 - Service 业务分支、状态机、驱动接口或业务 Proto。 - -### 3.2 非目标 - -- 不把厂商协议、关节限制、工作空间或设备专用故障码搬入 `DeviceManager`。 -- 不允许远程接口绕过真实硬件急停或保护停。 -- 不保证所有设备在一个阶段内同时完成迁移。 -- 不用软件 StopAll 替代机器人、PLC、驱动器或安全控制器的独立安全回路。 -- 不在本设计中实现运行期卸载设备插件；只消除安全协调器中的设备类型硬编码。 -- 首轮六阶段实施不要求交付证书、静态 Token、JWT 或 mTLS；这些能力作为独立的后续安全加固项。 - -### 3.3 必须始终成立的不变量 - -1. `Stop`、`Status`、`GetSafetyState` 和 `RecoverSafetyState` 不依赖普通准入为 OPEN。 -2. 一次 StopAll 开始后，旧 epoch 的命令不能再进入设备下发点。 -3. StopAll 与正常命令竞态时，命令只能是“未下发”或“已被登记并纳入停止”，不能存在 - 未登记的第三种状态。 -4. 状态过期、设备失联、安全能力缺失对控制命令一律 fail-closed。 -5. 同一 effective principal 的同一 `command_id` 最多触发一次硬件提交；认证关闭时所有调用者 - 共享服务端生成的 `anonymous` principal，因此 command ID 必须在整个匿名部署内唯一。 -6. 相同 ID、不同语义 payload 必须返回冲突，不能覆盖旧记录。 -7. 硬件结果不确定时保存 `OUTCOME_UNKNOWN`，重试只能查询该结果，不能再次下发。 -8. 通用 `RecoverSafetyState` 成功只表示软件准入可重新评估，不表示设备被上电、使能、 - 解除急停或自动运动。AUBO 物理急停释放后的自动上电是独立、显式配置的设备内策略。 -9. `SafetyManager` 持有内部锁时不得调用设备、网络或可能阻塞的 participant 方法。 -10. 驱动最终安全检查失败时，即使已经获得 permit，也不能下发设备命令。 -11. 进程重启后，控制设备在新鲜状态确认完成前不能自动恢复到可控制状态。 -12. 所有安全状态转换都增加 epoch、产生事件，并记录明确 reason code。 -13. 匿名网络调用不能执行 `RecoverSafetyState`；认证关闭时只能显式禁用恢复，或仅允许 - 服务端判定为 loopback/Unix Domain Socket 的本机调用。该限制不能由请求字段覆盖。 - -## 4. 统一概念模型 - -### 4.1 三个互不替代的状态维度 - -| 维度 | 说明 | 示例 | -| --- | --- | --- | -| Lifecycle | 进程对设备对象的生命周期管理 | Initializing、Running、Stopped、Error | -| Health | 设备是否正常工作和是否存在故障 | Healthy、Degraded、Fault、Unknown | -| Safety | 当前事实是否足以接受某一类命令 | Nominal、Restricted、Unsafe、Unknown | - -`Lifecycle=Running` 只说明设备 worker 已启动；`Health=Healthy` 只说明没有已知故障； -两者都不能替代 Safety 中的状态新鲜度、急停、运动、使能和代际信息。 - -### 4.2 命令意图 - -```cpp -enum class CommandIntent { - Observe, // 只读状态、图像、传感数据 - StartActivity, // 启动采集、录制、播放等非运动活动 - Configure, // 修改参数、地图、非安全 IO 等 - Actuate, // 运动、速度、力、位置、使能、扭矩输出 - Stop, // 停车、取消、torque-off、停止活动 - ResetFault, // 设备级、不会自动产生运动的故障复位 - RecoverAdmission, // 系统级软件锁止恢复 -}; -``` - -每个 handler 必须在代码中声明固定意图。客户端不能通过请求字段自行选择意图。 - -### 4.3 两个默认策略族 - -```cpp -enum class SafetyPolicyFamily { - Sensor, - Control, -}; -``` - -- `Sensor` 表示非运动型采集或外围活动，不要求它在字面上一定是传感器。Camera、 - Microphone、Battery、普通 Speaker activity 可使用该策略。 -- `Control` 表示能够导致位置、速度、力、扭矩或机械结构变化的能力。Arm、AGV、Motor、 - DexHand 动作、Camera PTZ、BioHead 表情和仿真执行器使用该策略。 -- 混合设备按命令选择策略。例如 Camera 图像流是 Sensor，PTZ START 是 Control， - PTZ STOP 是 Stop。 - -### 4.4 硬件事实使用三值逻辑 - -安全事实不能把“未实现”解释为 false： - -```cpp -enum class TriState { Unknown, False, True }; - -enum class SafetyCondition { - Nominal, // 已知正常，可继续按具体命令规则判断 - Restricted, // 已知静止或受硬件抑制，但不满足普通执行条件 - Unsafe, // 已观察到不允许继续控制的状态 - Unknown, // 缺失、过期、失联或无法解释 -}; -``` - -推荐的进程内快照： - -```cpp -struct DeviceSafetySnapshot { - std::string device_id; - SafetyCondition condition{SafetyCondition::Unknown}; - std::uint64_t device_generation{0}; - std::uint64_t sample_sequence{0}; - std::chrono::steady_clock::time_point observed_at; - std::uint64_t observed_at_unix_ms{0}; - - TriState connected{TriState::Unknown}; - TriState operational_ready{TriState::Unknown}; - TriState quiescent{TriState::Unknown}; - TriState motion_active{TriState::Unknown}; - TriState actuator_enabled{TriState::Unknown}; - TriState emergency_stop_active{TriState::Unknown}; - TriState protective_stop_active{TriState::Unknown}; - TriState fault_active{TriState::Unknown}; - - std::vector blockers; -}; -``` - -`SafetyBlocker` 必须包含稳定 reason、来源和作用域： - -```cpp -enum class BlockerScope { Device, System }; -enum class RecoveryRequirement { - RefreshOnly, - ClearSoftwareLatch, - HardwareReleaseRequired, - ManualInspectionRequired, -}; - -struct SafetyBlocker { - SafetyReason reason; - BlockerScope scope; - RecoveryRequirement recovery_requirement; - std::string source_id; - std::string operation_id; -}; -``` - -设备本地急停可以只阻塞该设备；连接到整机安全链的急停必须发布 `System` scope，并阻止 -全局准入恢复。作用域由经过评审的设备/安全链 adapter 固定，不能由客户端或普通配置修改。 - -安全判断使用本机 monotonic age。UTC 只用于日志和跨系统诊断，不能参与准入时序。 - -`device_generation` 在以下情况增加： - -- 设备后端重新初始化； -- SDK、总线或网络 session 重连； -- 驱动状态机执行需要使旧命令失效的 reset； -- 设备被重新注册。 - -## 5. 目标架构与所有权 - -```mermaid -flowchart LR - Client["gRPC / QUIC / Local Task"] --> Gateway["Request Context Gateway
限制 可选认证 授权策略 审计"] - Gateway --> Adapter["Typed Service Adapter
声明 CommandIntent"] - Adapter --> Ledger["CommandLedger
幂等和结果"] - Adapter --> Coordinator["DeviceManager::SafetyManager"] - Coordinator --> Policy["SensorPolicy / ControlPolicy"] - Coordinator --> Authority["ControlAuthorityManager"] - Coordinator --> Participants["SafetyParticipant Registry"] - Coordinator --> Cache["SafetySnapshotStore"] - Coordinator --> Permit["AdmissionPermit"] - Permit --> Driver["DeviceSafetyEndpoint + Driver"] - Driver --> Cache - StopRecover["StopAll / Recover 安全通道"] --> Coordinator -``` - -### 5.1 建议目录 - -```text -cmvr-es/manager/safety_manager/ - include/safety_types.h - include/safety_reason.h - include/device_safety_endpoint.h - include/safety_participant.h - include/safety_snapshot_store.h - include/command_admission_controller.h - include/command_ledger.h - include/safety_operation_orchestrator.h - include/safety_manager.h - src/... - tests/... - -cmvr-es/service/grpc/security/ - include/grpc_request_context.h - include/grpc_authentication_provider.h - include/grpc_authorization_policy.h - include/grpc_method_policy_registry.h - include/grpc_security_interceptor.h - src/... - tests/... -``` - -新增 CMake target：`cmvr_es::safety_manager`。它可以依赖通用类型和 -`ControlAuthorityManager`，但不能依赖 gRPC、具体设备后端或厂商 SDK。 - -### 5.2 `DeviceManager` 的职责变化 - -`DeviceManager` 增加： - -```cpp -SafetyManager& safetyManager() noexcept; -const SafetyManager& safetyManager() const noexcept; -``` - -它负责： - -- 在设备创建后注册设备描述、Safety endpoint 和快照槽位； -- 在设备销毁前注销 participant； -- 在 lifecycle start/stop/restart 时推进 device generation； -- 在 Runtime shutdown 开始时先关闭全局准入； -- 对外提供纯内存的 Manager + Safety 联合快照。 - -它不负责： - -- 解析 Aubo、Huayan、SEER、Modbus 等厂商状态； -- 根据 `DeviceKind` 写具体停止逻辑； -- 在 Manager mutex 下执行硬件 I/O； -- 判断具体运动目标是否超限。 - -### 5.3 依赖注入 - -最终状态下，gRPC service 构造函数显式接收： - -```cpp -DeviceManager& -SafetyManager& -CommandLedger& -GrpcSecurityGateway& -``` - -每个 handler 通过一个统一 call guard 取得不可变 `RequestContext`。认证 metadata 的解析、 -peer 归一化、角色生成和恢复接口暴露检查只存在于 `GrpcSecurityGateway`；业务 Service 不直接 -解析 metadata，也不从 request message 读取 principal 或 role。具体实现可以由 interceptor -完成前置检查，再由 call guard 取得同一次调用的上下文，但不能依赖 thread-local 传递上下文。 - -阶段 2 可以保留无参构造函数作为兼容入口，但它只能转发到上述依赖。阶段 5 删除业务 -代码对进程级 safety singleton 的直接访问。测试使用独立 coordinator，不再依赖 -`clearForTesting()` 清理全局状态。 - -### 5.4 管理面与设备运行面分离 - -“Stop/Status/Recover 始终可达”不仅要求它们绕过普通 gate，也要求单个设备初始化失败时 -gRPC 管理面仍能启动。目标 Runtime 启动顺序为： - -```text -load config and logging - -> construct DeviceManager core and SafetyManager - -> validate selected gRPC security profile and start management-plane services - -> initialize/start devices - -> reconcile required control snapshots - -> start operational tasks - -> Open with device-level Blocked, or global Latched -``` - -- 配置损坏、所选安全 Profile 的身份材料无效或 SafetyManager 核心构造失败仍使进程启动失败； -- `AuthenticationMode::Disabled` 是显式兼容模式，不伪装成“已认证”；如果使用非 loopback - 明文监听，启动日志、GetSystemInfo 和指标必须持续暴露该风险； -- 单个设备 create/init/start 失败记录在 inventory，并使相关资源 Blocked； -- Device scope 的动态 Unknown/Unsafe 使相关资源 Blocked，System scope blocker 使全局 Latched， - 两者都不停止 SystemService； -- 普通设备 service 可以继续注册，访问失败设备时返回结构化 DEVICE_UNAVAILABLE； -- TaskManager 需要区分 ManagementPlane 和 Operational task，后者失败不能回滚前者； -- 物理急停和本机停机手段始终独立于该网络管理面。 - -在完成该拆分前，“始终可达”只保证 gRPC server 已成功运行期间的行为，不能覆盖当前 -Runtime 在任一 required device 启动失败时直接退出的情况。 - -## 6. 快照发布与线程模型 - -### 6.1 发布模型 - -正常准入和状态查询只能读取 `SafetySnapshotStore`，不能同步查询驱动。 - -```text -driver callback / polling worker - | - v -DeviceSafetyEndpoint normalizes vendor facts - | - v -SafetySnapshotPublisher::publish(value) - | - v -SafetySnapshotStore atomic value slot -``` - -现有设备可以先通过独立 adapter worker 拉取状态；新设备应优先在自己的 SDK callback 或 -状态轮询线程中发布。Manager heartbeat、gRPC Status 和准入只复制值。 - -现有 `healthSnapshot()` 在迁移期只能由 DeviceMonitor 后台 worker 调用，并使用每设备隔离和 -超时；不能继续由 `DeviceManager::snapshot()` 的调用线程执行。目标状态是设备同时发布缓存的 -Health 和 Safety，两者仍保持独立字段。 - -### 6.2 Freshness - -每个 `DeviceSafetyDescriptor` 声明默认最大状态年龄。配置只允许将阈值收紧，不能在生产模式下 -放宽超过代码定义的硬上限。 - -- Sensor Observe 可在业务允许时返回带 `stale=true` 的历史值； -- Sensor StartActivity 在 Missing/Stale 时拒绝； -- Control Actuate、Configure 和 ResetFault 在 Missing/Stale 时拒绝； -- Stop 不因状态过期而拒绝，仍直接尝试安全停止； -- Recover 必须获得一个晚于本次 refresh request 的新样本。 - -### 6.3 主动刷新 - -`RecoverSafetyState` 需要主动复核，但仍不在 gRPC 线程直接做设备 I/O： - -1. 记录当前 `sample_sequence`； -2. 调用 endpoint 的非阻塞 `requestSafetyRefresh()`； -3. 在专用 recovery executor 等待 sequence 增加； -4. 超时则产生 `SAFETY_STATE_STALE` blocker，保持锁止。 - -### 6.4 锁和执行器约束 - -- Coordinator 锁只保护状态转换、epoch、slot 和 participant registry； -- Snapshot slot 使用原子 shared snapshot 或短持有独立 mutex； -- `beginDispatch()` 只建立 fence 和 in-flight 计数，不等待物理运动结束； -- 设备调用、停止和刷新均在锁外执行； -- Stop/Recover 使用有界 worker pool，不为每次调用创建 detached thread； -- 每个 legacy health probe 最多允许一个 in-flight worker，卡住后只标记 stale，不重复创建线程； -- participant 注销前必须等待自己的 callback 和 stop handle 退出。 - -## 7. 状态机 - -### 7.1 系统准入状态 - -```mermaid -stateDiagram-v2 - [*] --> Starting - Starting --> Open: required snapshots reconciled - Starting --> Latched: unknown active control or startup failure - Open --> Stopping: StopAll - Stopping --> Open: all participants quiescent - Stopping --> Latched: timeout or unconfirmed stop - Latched --> Recovering: authorized recovery - Recovering --> Open: every required latch cleared - Recovering --> Latched: blocker remains - Starting --> ShuttingDown: shutdown - Open --> ShuttingDown: shutdown - Stopping --> ShuttingDown: shutdown - Latched --> ShuttingDown: shutdown - Recovering --> ShuttingDown: shutdown -``` - -每次进入 `Stopping`、`Latched`、`Recovering` 或 `ShuttingDown` 都增加 `safety_epoch`。 - -### 7.2 设备准入状态 - -```cpp -enum class DeviceAdmissionState { - Observing, // 已注册，等待第一份有效快照 - Open, - Blocked, // 确定的硬件或生命周期条件不满足 - Quarantined, // 命令或停止结果不确定，需要人工恢复流程 - Recovering, - Removed, -}; -``` - -- `Blocked` 可以在新鲜硬件事实改善后自动回到 Open，例如设备故障被合法清除； -- `Quarantined` 不能仅靠一份健康快照自动清除，必须经过 recovery transaction； -- 一个普通设备命令的结果不确定时，默认只 quarantine 对应 control resource； -- StopAll 无法确认全部参与者时，全局状态进入 Latched。 - -### 7.3 命令生命周期 - -```text -RECEIVED - -> RESERVED - -> REJECTED_BEFORE_DISPATCH - -> ADMITTED - -> DISPATCHING - -> ACCEPTED_BY_HARDWARE - -> COMPLETED | FAILED | CANCELED | OUTCOME_UNKNOWN -``` - -`ACCEPTED_BY_HARDWARE` 的含义必须由每个 adapter 明确定义。SDK 返回码、queue ID、任务 ID、 -ACK 或写入总线分别可能具有不同强度，不能统一解释成“运动完成”。 - -## 8. 命令准入算法 - -### 8.1 普通 unary 控制命令 - -1. Request Context Gateway 完成大小限制、correlation ID、上下文构造和授权策略检查；只有 - 当前 Profile 启用 AuthN 时才执行身份认证。 -2. 解析 `command_id`、service instance、device generation 和本地有效期。 -3. CommandLedger 按 effective principal 预留 ID，并计算语义 payload hash；Disabled 模式使用 - 服务端固定的 `anonymous` namespace。 -4. 若存在同 ID 记录：相同 hash 返回/等待原结果；不同 hash 返回冲突。 -5. Service 使用固定 `CommandDescriptor` 调用 `SafetyManager::admit()`。 -6. Coordinator 在短锁内读取 global state、safety epoch、device slot 和 cached snapshot。 -7. Policy 判断意图、freshness、硬件事实、设备状态和调用角色。 -8. Control intent 获取或校验 `ControlAuthorityManager` lease。 -9. 返回 move-only `AdmissionPermit`，其中固定所有 generation 和 deadline。 -10. Service 调用 `beginDispatch(permit)`。这里是 StopAll 与命令提交的线性化点。 -11. 驱动在自己的串行化上下文中执行最终硬件检查。 -12. 通过检查后调用 SDK，并把结果写入 ledger；失败或异常也必须终结 ledger 记录。 - -`AdmissionPermit` 至少包含： - -```cpp -struct AdmissionPermit { - std::string command_id; - std::string device_id; - CommandIntent intent; - std::uint64_t safety_epoch; - std::uint64_t device_generation; - std::uint64_t authority_generation; - std::chrono::steady_clock::time_point deadline; -}; -``` - -### 8.2 Stop 与 ResetFault - -- Stop 走安全通道，不需要普通 permit，也不受全局 Latched 阻塞； -- Stop 仍需经过 RequestContext、当前 Profile 的访问策略、参数校验、超时和审计；认证关闭时 - 不额外拒绝 Stop，因为停止能力必须保持可达； -- Stop 不因 expected service instance、device generation 或 safety epoch 过期而拒绝；这些字段 - 对 Stop 仅用于诊断，因为安全停止必须优先于防重放限制； -- 重复 Stop 应加入当前 stop operation 或执行幂等停止，不能因为缺少 command ID 而拒绝； -- ResetFault 可在 Latched 下执行，但必须声明为不会上电、使能或恢复运动； -- 如果某厂商的 clear-fault 同时会 resume、enable 或移动，必须拆成两个 typed 操作， - 不能把它登记成 ResetFault； -- ResetFault 成功只触发安全快照刷新，不直接清除 central quarantine。 - -### 8.3 流式控制 - -Teleoperation、Motor cyclic stream 和未来连续控制不为每个 setpoint 写 unary ledger： - -1. OPEN frame 生成 `control_session_id`，绑定 service instance、device generation、 - safety epoch 和 control lease； -2. setpoint sequence 必须严格递增，且带本地可解释的 `valid_for`； -3. 每个 setpoint 下发前轻量校验 session epoch、permit epoch、watchdog 和快照 freshness； -4. StopAll 或 Recover 增加 epoch 后，旧 stream 立即进入 HOLD/STOP，不可自动续接； -5. reconnect 必须创建新 session，不能恢复旧 ACTIVE 状态； -6. stream 结束、write/read 失败和 watchdog 均执行可验证的安全停止。 - -### 8.4 ActionQueue - -- 保留现有 ActionQueue `action_id` 和 service instance； -- 预校验阶段确认所有 step 已分类，但不提前获取可长期持有的 permit； -- 每个 step 真正开始前重新准入并取得当前 epoch 的 permit； -- step 的内部 command ID 由 `action_id + step_id` 确定性派生，不能要求嵌套请求再提供一个 - 可与 action 身份冲突的独立 ID； -- StopAll 仍能抢占队列和活动 step； -- 迁移完成后 ActionQueue 不再拥有独立的普通准入 generation，只保留队列执行 generation； -- 队列结果和单步 CommandLedger 记录使用关联 ID，但不得造成一次 step 两套独立重试语义。 - -## 9. 策略矩阵 - -### 9.1 SensorPolicy - -| Intent | Global Latched | Snapshot 要求 | 默认结果 | -| --- | --- | --- | --- | -| Observe | 允许 | 可返回 stale 标记；不能伪造为 fresh | Allow | -| StartActivity | 拒绝或按设备 Blocked | connected、fresh、无已知 fault | Conditional | -| Configure | 拒绝 | connected、fresh、配置操作已分类 | Conditional | -| Stop | 允许 | 不要求 fresh | Allow safety lane | -| ResetFault | 仅明确支持的设备 | fresh，且操作不会启动活动 | Conditional | -| RecoverAdmission | SafetyAdmin only | fresh sample required | Safety lane | - -### 9.2 ControlPolicy - -| Intent | 必要条件 | -| --- | --- | -| Observe | 始终允许读取缓存；返回 freshness 和 blocker | -| Configure | 全局 Open、设备非 Quarantined、fresh、命令不会隐式 Actuate | -| Actuate | 全局 Open、设备 Open、condition=Nominal、fresh、generation 匹配、lease 有效、deadline 有效、最终硬件检查通过 | -| Stop | 无条件进入安全通道；状态未知仍尝试 stop | -| ResetFault | 允许在 Blocked/Latched 下执行，但只允许非使能、非运动的 typed reset | -| RecoverAdmission | SafetyAdmin、expected epoch 匹配、主动刷新完成、quiescent=true、没有未知 stop worker | - -`emergency_stop_active=true` 或 `protective_stop_active=true` 可以表示设备被硬件抑制且物理上 -静止，但不能被 Recover 清除。Device scope blocker 继续阻止该设备 Actuate；System scope -blocker 继续保持全局 Latched。只有硬件被合法处理并发布新的事实后，恢复事务才可清理对应 -软件 latch。 - -## 10. 设备安全能力接口 - -### 10.1 描述与注册 - -```cpp -struct DeviceSafetyDescriptor { - std::string device_id; - DeviceKind kind; - SafetyPolicyFamily default_policy; - std::chrono::milliseconds maximum_snapshot_age; - bool requires_safe_stop; - bool supports_active_refresh; - bool supports_non_enabling_fault_reset; -}; - -struct DeviceSafetyRegistration { - DeviceSafetyDescriptor descriptor; - std::shared_ptr endpoint; - std::shared_ptr participant; -}; -``` - -控制设备在 enforce 模式下缺少 endpoint、fresh snapshot 或 safe-stop participant 时： - -- 启动阶段标记设备 safety capability invalid； -- 设备可以继续出现在 inventory 和诊断接口中； -- 所有 Actuate 命令拒绝； -- 配置要求严格启动时，可以直接使 Runtime 初始化失败。 - -新增全新 DeviceKind 仍可能需要修改现有 DeviceFactory、配置 Proto 和对外业务 API；本设计 -保证的是不再修改 SafetyManager、StopAll 和 Recover 的设备类型分支。 - -### 10.2 Endpoint - -```cpp -class DeviceSafetyEndpoint { -public: - virtual ~DeviceSafetyEndpoint() = default; - - virtual DeviceSafetyDescriptor descriptor() const = 0; - virtual void bindPublisher(SafetySnapshotPublisher publisher) = 0; - virtual void requestSafetyRefresh() noexcept = 0; - - // 在驱动自己的串行化上下文内执行。不得更改设备状态。 - virtual HardwareCheckResult validateBeforeDispatch( - const AdmissionPermit&) = 0; - - // 只协调驱动内部软件状态，不得上电、使能或启动运动。 - virtual RecoveryCheckResult reconcileAdmissionState( - const RecoveryContext&) = 0; -}; -``` - -如果最终检查需要厂商 I/O，它必须有设备级 deadline，并在驱动 executor 中执行；Manager -不持锁等待。超时返回 Unknown，不允许继续下发。 - -### 10.3 Safe-stop participant - -```cpp -class SafetyParticipant { -public: - virtual ParticipantDescriptor descriptor() const = 0; - - // 纯内存、快速关闭本 participant 的新工作准入。 - virtual BarrierToken beginBarrier(const SafetyOperationContext&) = 0; - - // 非阻塞提交停止，返回可等待 handle。 - virtual StopHandle requestQuiesce( - const BarrierToken&, const SafetyOperationContext&) = 0; - - // 验证“旧 generation 不会继续、当前输出已静止”。 - virtual QuiescenceResult verifyQuiescent( - const BarrierToken&, const SafetyOperationContext&) = 0; - - // 只清理软件 gate、retired holder 或内部 session。 - virtual RecoveryCheckResult recoverAdmission( - const BarrierToken&, const RecoveryContext&) = 0; - - virtual void releaseBarrier(const BarrierToken&) noexcept = 0; -}; -``` - -participant 可以代表设备，也可以代表 ActionQueue、MediaSourceManager、Motor session registry 等 -跨设备活动域。StopAll 不再知道具体 C++ 设备类型。 - -## 11. CommandLedger 与结果语义 - -### 11.1 Key 和 payload hash - -推荐 key：`(effective_principal_id, command_id)`。 - -`effective_principal_id` 只能由服务端安全网关产生：认证启用时取认证结果中的稳定 principal ID； -认证关闭时固定为 `anonymous`。不能使用 request、普通 metadata 中自报的 client ID，也不使用 -易变化的 TCP source port。匿名模式因此要求 command ID 在整个部署内全局唯一。 - -payload hash 包含： - -- 完整 gRPC method 名； -- device/resource ID； -- deterministic protobuf semantic payload； -- expected service instance 和 device generation； -- 不包含诊断 timestamp、认证 metadata 和 command ID 本身。 - -同一 effective principal 内 command ID 必须全局唯一。不同 method 复用同一 ID 会因 hash 不同而冲突。 - -### 11.2 记录内容 - -```cpp -struct CommandRecord { - CommandKey key; - PayloadHash payload_hash; - CommandLifecycle lifecycle; - CommandOutcome outcome; - std::uint64_t safety_epoch; - std::uint64_t device_generation; - bool hardware_submission_possible; - steady_clock::time_point accepted_at; - steady_clock::time_point terminal_at; -}; -``` - -- In-flight 和近期 terminal 结果保留完整响应； -- 淘汰完整结果后保留精确 retired-ID tombstone； -- 达到硬容量后拒绝新 ID，不淘汰仍可能被重放的 tombstone； -- 账本耗尽使用 `RESOURCE_EXHAUSTED/LEDGER_EXHAUSTED`，不能降级成无幂等执行； -- 进程内账本不要求落盘，但客户端必须携带 expected service instance； -- 进程重启后旧 instance 请求拒绝，控制设备先完成 startup reconciliation 才可 Open。 - -### 11.3 RPC 取消 - -- 在 RESERVED/ADMITTED 且未 dispatch 时取消：终结为 `CANCELED_BEFORE_DISPATCH`； -- 硬件已接受后客户端断开：不能假设命令取消，继续记录实际结果； -- 需要“断线即停”的命令必须显式使用 session/watchdog 协议； -- retry 相同 ID 只能加入原执行或读取原结果。 - -## 12. StopAll 统一事务 - -### 12.1 Participant 分组 - -建议固定阶段而非依赖注册顺序： - -1. `Ingress`：关闭 gRPC/QUIC/local task 普通准入； -2. `Scheduler`：取消 ActionQueue 和待执行作业； -3. `ControlSession`：撤销 teleop、motor cyclic、velocity 等连续控制 session； -4. `Actuator`：Arm、AGV、Motor、DexHand、PTZ、BioHead 请求安全停止； -5. `PeripheralActivity`：Camera、Microphone、Speaker、recording 和媒体 producer； -6. `Verification`：等待每个 required participant 的 quiescence 证明。 - -组内可以并行，组间顺序固定。每个 participant 有独立 timeout，外层还有总 deadline。 - -### 12.2 算法 - -1. 使用 process-wide operation mutex 创建或加入当前 StopAll round； -2. 状态设为 Stopping，增加 safety epoch，关闭全局准入； -3. 对当前 participant registry 建立不可变快照； -4. 对所有 participant 调用 `beginBarrier()`； -5. 撤销普通 control lease，并等待已进入 dispatch fence 的短提交退出； -6. 分阶段提交 `requestQuiesce()`； -7. 等待 handle，并调用 `verifyQuiescent()`； -8. 任一 required participant 超时、异常或 Unknown：保留其 barrier，记录 blocker，进入 Latched； -9. 全部成功：清理旧 retired holders，释放本轮 barrier，状态回到 Open； -10. 返回结构化 per-participant 结果和新 epoch。 - -多个 StopAll 调用加入同一 round，并各自得到同一最终结果。RPC waiter 取消不能取消已经开始的 -系统停止事务。 - -成功 StopAll 是一个“已经在本轮确认全部静止”的同步屏障，不是持续维护锁。状态回到 Open 后， -其他通过当前访问策略的客户端可以提交新命令，甚至可能在 StopAll 调用方收到响应前完成准入。需要长期禁止 -控制时应设计独立的 maintenance lock，不复用 StopAll 或 Recover 语义。 - -### 12.3 迟到结果 - -- 旧 round 的 worker 完成后只允许更新该 round 的诊断记录； -- worker 不能凭旧 token 重新打开准入； -- participant 的 release 必须验证 operation ID 和 epoch； -- worker 仍运行时 participant 保持 blocker，Recover 不得跳过它。 - -## 13. RecoverSafetyState 事务 - -### 13.1 语义边界 - -`RecoverSafetyState` 的准确含义是： - -> 在独立安全通道中重新确认设备和活动域已经静止，并清除由软件 StopAll、超时、取消、 -> 旧 session 或不确定结果留下的准入锁止。 - -它不执行： - -- 解除物理急停； -- 自动解除保护停； -- torque-on、power-on、brake-release； -- 自动继续旧轨迹、导航、ActionQueue 或 teleop session； -- 把 Unknown 解释为 Safe； -- 调用测试接口 `clearForTesting()`。 - -### 13.2 恢复算法 - -1. Request Context Gateway 先执行恢复暴露策略：认证模式要求 `SafetyAdmin`；认证关闭时默认 - 禁用，只有 `LOCAL_ONLY` 且服务端确认 peer 为 loopback/Unix Domain Socket 才允许继续； - 随后校验 `recovery_id`、scope、reason 和 deadline； -2. RecoveryLedger 对 `recovery_id` 做幂等处理； -3. 与 StopAll 串行化；expected safety epoch 不匹配立即拒绝； -4. 状态转为 Recovering 并增加 epoch，使所有旧 permit/session 失效； -5. 选取 scope 内处于 Blocked/Quarantined 的 device 和 subsystem participant； -6. 确认没有旧 stop worker、dispatch fence 或 active command 仍未退出； -7. 对设备发起 active refresh，等待晚于本次请求的新 snapshot； -8. 控制设备要求 `quiescent=true`，且 condition 不能是 Unsafe/Unknown；任何 System scope - hardware blocker 都保持全局锁止； -9. 调用 endpoint/participant 的非使能 `reconcileAdmissionState()` 和 `recoverAdmission()`； -10. 清理已经验证的 quarantine、retired safety holder 和软件 gate； -11. scope 外 blocker 或失败 participant 继续保留；只有全部 required latch 清除才回到 Open； -12. 返回 previous/new epoch、每个对象的 before/after 状态和 blocker。 - -### 13.3 部分恢复 - -- 请求可以只恢复指定 device，但不能偷偷排除 subsystem blocker； -- 设备级 quarantine 可以单独清除； -- 如果 global gate 仍被其他 participant 持有，系统状态仍为 Latched； -- response 必须区分 `RECOVERED`、`VERIFIED_BUT_STILL_BLOCKED`、`BLOCKER_REMAINS`、 - `EPOCH_MISMATCH` 和 `NOTHING_TO_RECOVER`。 - -## 14. gRPC API 设计 - -### 14.1 扩展公共命令头 - -保持字段 1、2 不变，使用新 tag 增量扩展： - -```protobuf -message CommandHeader { - message Request { - string device_id = 1; - google.protobuf.Timestamp timestamp = 2; // 仅诊断 - string command_id = 3; - string expected_service_instance_id = 4; - optional uint64 expected_device_generation = 5; - uint32 valid_for_ms = 6; - } - - message Feedback { - bool success = 1; - string error_message = 2; - google.protobuf.Timestamp timestamp = 3; - CommandReasonCode reason_code = 4; - string command_id = 5; - string service_instance_id = 6; - uint64 safety_epoch = 7; - uint64 device_generation = 8; - CommandExecutionState execution_state = 9; - } -} -``` - -兼容阶段中旧客户端字段为空： - -- Legacy/Shadow 模式允许，但记录 `missing_command_identity`； -- EnforceSelected 只对已迁移设备要求； -- EnforceAll 下所有普通 mutating unary 命令必须提供； -- Observe 不要求 command ID；安全 Stop 为保持可达性不把 ID/epoch 作为前置条件，但有 ID 时 - 用于合并结果和审计；Recover 必须提供 recovery ID 和 expected safety epoch。 - -`valid_for_ms` 从服务端收到请求的 monotonic time 开始计算，不能信任跨机器 timestamp。 -Safety Stop 忽略已经过期的普通命令有效期，但仍受服务端 stop operation 总 deadline 约束。 - -- `command_id` 建议使用 UUID，服务端至少限制字符集和最大长度； -- `valid_for_ms=0` 在 Legacy/Shadow 下使用有界服务端默认值，不能表示无限； -- EnforceSelected/EnforceAll 可以要求 Actuate 显式提供非零有效期； -- 服务端对所有客户端有效期施加硬上限，retry 不能延长原 ledger 记录的 deadline； -- expected service instance 为空只在兼容模式接受。 - -### 14.2 新增安全 API - -建议新增 `protos/cmvr/api/safety_command.proto`，并在现有 `SystemService` 增加： - -```protobuf -rpc GetSafetyState(GetSafetyStateCommand.Request) - returns (GetSafetyStateCommand.Feedback); - -rpc RecoverSafetyState(RecoverSafetyStateCommand.Request) - returns (RecoverSafetyStateCommand.Feedback); -``` - -建议核心消息： - -```protobuf -message DeviceIdList { - repeated string device_ids = 1; -} - -message SafetyScope { - oneof target { - bool all_devices = 1; // 必须显式为 true - DeviceIdList devices = 2; // 必须非空且无重复 - } -} - -message RecoverSafetyStateCommand { - enum Mode { - MODE_UNSPECIFIED = 0; - VERIFY_ONLY = 1; - CLEAR_SOFTWARE_LATCH = 2; - } - - message Request { - string recovery_id = 1; - SafetyScope scope = 2; - uint64 expected_safety_epoch = 3; - Mode mode = 4; - string reason = 5; - uint32 timeout_ms = 6; - } - - message Feedback { - CommandHeader.Feedback header = 1; - RecoveryResult result = 2; - uint64 previous_safety_epoch = 3; - uint64 current_safety_epoch = 4; - SystemAdmissionState system_state = 5; - repeated RecoveryTargetResult targets = 6; - } -} -``` - -不要增加 `force=true` 或 `ignore_hardware_state=true`。 - -### 14.3 GetSafetyState - -至少返回： - -- global admission state、safety epoch、control service instance ID； -- device lifecycle、health、device admission state； -- SafetyCondition、snapshot fresh、sample age、device generation； -- active control owner 的脱敏标识； -- blocker reason code、首次发生时间、最后更新时间、来源 operation/command ID； -- 当前 StopAll/Recover operation 的阶段和 deadline； -- subsystem participant 状态。 - -该接口只读内存，即使设备失联或 driver worker 卡住也必须及时返回。 - -### 14.4 gRPC status 与业务结果 - -- 没有产生有效业务结果时使用非 OK status：认证失败、权限不足、格式错误、服务关闭； -- command ID 已预留后，执行结果使用 `grpc::Status::OK + CommandOutcome`，便于相同 ID - 重放完整结果； -- Recover 的部分失败也返回 OK 和 per-target result； -- `INTERNAL` 只表示代码异常或不变量破坏； -- 兼容旧服务时继续填充 `success/error_message`，客户端应迁移到 reason code。 - -## 15. 稳定错误模型 - -建议公共 reason code 至少包括： - -| Reason | gRPC 映射 | 是否可用同 ID 重试 | -| --- | --- | --- | -| INVALID_ARGUMENT | INVALID_ARGUMENT | 否，修正后使用新 ID | -| UNAUTHENTICATED | UNAUTHENTICATED | 认证后重新请求 | -| PERMISSION_DENIED | PERMISSION_DENIED | 否 | -| RECOVERY_RPC_DISABLED | FAILED_PRECONDITION | 是；未进入 RecoveryLedger，本机改配置并重启后可重试 | -| DEVICE_NOT_FOUND | NOT_FOUND | 否 | -| UNSUPPORTED_COMMAND | UNIMPLEMENTED | 否 | -| SYSTEM_STOPPING | ABORTED | 原 ID 查询，不重新下发 | -| SAFETY_LATCHED | FAILED_PRECONDITION | 恢复后新 ID | -| SAFETY_STATE_STALE | UNAVAILABLE | 状态刷新后新 ID | -| HARDWARE_UNSAFE | FAILED_PRECONDITION | 处理硬件后新 ID | -| EMERGENCY_STOP_ACTIVE | FAILED_PRECONDITION | 物理处理后新 ID | -| PROTECTIVE_STOP_ACTIVE | FAILED_PRECONDITION | 合法复位后新 ID | -| DEVICE_DISCONNECTED | UNAVAILABLE | 重连并校验 generation 后新 ID | -| CONTROL_BUSY | RESOURCE_EXHAUSTED | 释放控制权后新 ID | -| GENERATION_MISMATCH | ABORTED | 刷新状态后新 ID | -| COMMAND_ID_CONFLICT | ALREADY_EXISTS | 否 | -| LEDGER_EXHAUSTED | RESOURCE_EXHAUSTED | 稍后提交新 ID | -| BACKPRESSURE | RESOURCE_EXHAUSTED | 仅确认未接收后使用新 ID | -| DEADLINE_EXCEEDED_BEFORE_DISPATCH | DEADLINE_EXCEEDED | 新 ID | -| OUTCOME_UNKNOWN | ABORTED | 不得自动重下发 | -| INTERNAL_ERROR | INTERNAL | 不得自动重下发运动 | - -response 可以附带 retry directive，但 `retryable=true` 不能用于 `OUTCOME_UNKNOWN`。 - -## 16. gRPC 请求上下文与安全扩展设计 - -### 16.1 延后认证的边界 - -本轮设备安全改造允许不实现 TLS、静态 Token、JWT 和 mTLS，但不能把“暂不认证”等同于 -“不设计认证边界”。现在必须固定以下三层接口，后续安全加固只能替换实现： - -| 层 | 当前小范围部署 | 后续可选实现 | 业务层是否感知 | -| --- | --- | --- | --- | -| Transport security | Insecure，可受限到 loopback/隔离网 | Server TLS、mTLS、Unix Domain Socket | 否 | -| Authentication | Disabled，服务端生成 `anonymous` | Static Token、JWT/OIDC、TLS client certificate、Unix peer credential | 否 | -| Authorization | Compatibility policy + Recover exposure policy | 基于 role/capability 的 policy | 只接收允许/拒绝结果 | - -这种调整只表示认证交付可以延后，不表示明文匿名网络具备安全性。使用非 loopback 明文监听时， -安全依赖部署网络、主机防火墙和物理访问控制；该风险必须在配置、启动日志、SystemInfo 和指标中 -保持可见。 - -认证信息继续使用 gRPC metadata 或 transport auth context，不进入业务 request Proto。这样不会 -污染设备 API，也避免以后为了增加 Token 给所有命令消息增加字段。 - -### 16.2 `RequestContext` 与扩展接口 - -建议定义与设备层无关的不可变上下文： - -```cpp -enum class AuthenticationMethod { - Disabled, - StaticToken, - Jwt, - TlsClientCertificate, - UnixPeerCredential, -}; - -struct Principal { - std::string id; // 由服务端生成 - AuthenticationMethod method; - bool authenticated; - std::vector roles; // Disabled 模式只能是 Anonymous -}; - -struct RequestContext { - std::string correlation_id; - std::string full_method_name; - std::string peer; - Principal principal; - bool transport_encrypted; - bool local_peer; - std::chrono::steady_clock::time_point received_at; - std::chrono::steady_clock::time_point deadline; -}; -``` - -核心扩展接口： - -```cpp -class GrpcAuthenticationProvider { -public: - virtual ~GrpcAuthenticationProvider() = default; - virtual AuthenticationResult authenticate(const GrpcCallFacts&) = 0; -}; - -class GrpcAuthorizationPolicy { -public: - virtual ~GrpcAuthorizationPolicy() = default; - virtual AuthorizationDecision authorize( - const RequestContext&, const GrpcMethodPolicy&) const = 0; -}; - -class GrpcSecurityGateway { -public: - virtual GrpcCallGuard beginCall( - grpc::ServerContext&, const GrpcMethodPolicy&) = 0; -}; -``` - -当前提供 `DisabledAuthenticationProvider`：它不读取客户端自报身份，始终产生 -`{id="anonymous", authenticated=false, roles=[Anonymous]}`。同时提供可注入的 fake provider， -用于证明未来切换认证实现时不需要修改 handler。 - -每个 gRPC handler 在入口取得 `GrpcCallGuard`，之后只使用其中的 `RequestContext`。call guard -负责上下文生命周期、统一拒绝状态和审计结束事件。可以用 server interceptor 完成全局前置检查， -但不能依赖 thread-local 在 interceptor 和 handler 之间传递身份；同步、异步和 callback RPC 都必须 -具有明确的 per-call 所有权。 - -`SafetyManager` 不依赖 gRPC 类型。Service 只把从 `RequestContext` 派生的稳定 -`CommandActor`/capability 传给准入和 ledger；驱动层完全不可见认证方式。 - -### 16.3 部署 Profile - -建议提供以下验证 Profile。Profile 是一组配置约束，不是散落在 Service 中的条件分支： - -| Profile | 监听与传输 | AuthN | Recover | 适用范围 | -| --- | --- | --- | --- | --- | -| `LOCAL_COMPATIBILITY` | loopback 或 Unix Domain Socket，可明文 | Disabled | 默认 Disabled，可显式 LocalOnly | 单机开发和维护 | -| `TRUSTED_NETWORK_COMPATIBILITY` | 显式受控网卡，可明文 | Disabled | Disabled | 当前隔离的小范围运行 | -| `LIGHTWEIGHT_AUTHENTICATED` | Server TLS | Static Token | SafetyAdmin | 客户端无需证书的轻量方案 | -| `PRODUCTION_AUTHENTICATED` | Server TLS 或 mTLS | JWT/Token/client certificate | SafetyAdmin | 后续正式部署 | - -本轮只要求实现前两个 Compatibility Profile 和后两个 Profile 的配置校验占位；选择尚未编译的 -认证 provider 必须启动失败，不能静默退回 Disabled。 - -约束如下： - -- Disabled 必须是显式模式，不能因为证书或 Token 文件加载失败而自动进入； -- 非 loopback 的 Insecure + Disabled 必须额外配置 `allow_insecure_non_loopback=true`，并产生 - 高可见度持续告警； -- Profile 只能通过本机配置和进程重启改变，不提供远程降级接口； -- QUIC heartbeat 和 GetSystemInfo 发布实际 transport/authentication/recovery exposure，不能 - 根据配置意图伪报； -- 后续 Static Token 客户端通过统一 client interceptor 添加 `authorization: Bearer ...`， - 业务调用点不变化； -- 启用了 TLS 时才校验证书、私钥、CA、有效期和文件权限。 - -### 16.4 授权与 Recover 暴露策略 - -方法权限仍预先分类，作为未来认证启用后的稳定契约： - -| Role | 权限 | -| --- | --- | -| Anonymous | Compatibility Profile 中除 Recover 外的现有兼容行为 | -| Observer | GetSystemInfo、GetDeviceList、GetSafetyState、只读状态和传感流 | -| Operator | Observer + 普通控制 + 设备 Stop + StopAll | -| SafetyAdmin | Operator + RecoverSafetyState + 安全配置诊断 | - -`GrpcMethodPolicyRegistry` 至少保存完整 method 名、read/mutate/stop/recover 分类、 -`CommandIntent` 和最低 role。未知方法在 Authenticated Profile 中 fail-closed；Compatibility -Profile 可以只为已有 RPC 保留当前行为，但仍必须产生 `unclassified_method` 告警并在阶段 2 前清零。 - -`RecoverSafetyState` 额外使用独立的 `RecoveryExposure`： - -| RecoveryExposure | 行为 | -| --- | --- | -| `DISABLED` | 返回稳定的 `RECOVERY_RPC_DISABLED`，不进入 RecoveryLedger | -| `LOCAL_ONLY` | 仅接受服务端从实际 peer 判定的 loopback/Unix Domain Socket 调用 | -| `AUTHORIZED` | 要求 authenticated principal 且具有 SafetyAdmin | - -校验规则： - -- Authentication Disabled + Recovery Authorized 是非法配置； -- `TRUSTED_NETWORK_COMPATIBILITY` 不能配置 LocalOnly 后再信任代理转发的 IP/header；只有 gRPC - 连接的实际 peer 可以用于本机判定； -- LocalOnly 是部署范围限制，不宣称调用者身份已经认证；优先使用 Unix Domain Socket， - 平台支持时再校验 UID/GID； -- TCP loopback 的 LocalOnly 等价于信任主机上的所有进程。多用户主机、共享容器宿主机或存在 - 不可信本地进程时必须保持 Disabled，或改用具有文件权限/peer credential 的 Unix Domain Socket； -- request 中的 role、principal、`force=true` 或类似字段一律不能改变该策略； -- Stop/StopAll 始终走独立安全通道，不受普通 safety gate 或审计 sink 故障阻塞；Authenticated - Profile 仍执行其访问策略，独立物理急停不能依赖网络认证服务。 - -### 16.5 调用链与状态码 - -```text -request limits -> correlation ID -> selected authentication provider - -> immutable RequestContext -> method/recovery authorization policy - -> audit begin -> service handler -> audit outcome -``` - -- Disabled provider 成功结果仍标记 `authenticated=false`，不能伪造为 Observer/Operator； -- 认证信息无效返回 `UNAUTHENTICATED`，身份有效但权限不足返回 `PERMISSION_DENIED`； -- Recover 被部署配置关闭返回业务 reason `RECOVERY_RPC_DISABLED`； -- LocalOnly 收到非本机 peer 返回 `PERMISSION_DENIED`； -- reflection 独立配置。Authenticated Profile 默认关闭或仅向 SafetyAdmin 开放；Compatibility - Profile 保持显式开关，不能依靠 reflection 状态表示访问安全。 - -### 16.6 审计字段与失败策略 - -- effective principal ID、authentication method、authenticated、roles、peer； -- transport encrypted、security profile、recovery exposure； -- gRPC method、device/resource、intent； -- correlation ID、command/recovery/stop operation ID； -- payload hash，不记录 Token、Authorization metadata、原始音视频和敏感大 payload； -- 准入结果、reason code、safety/device/authority generation； -- 硬件提交状态、终态、耗时； -- Recover reason、before/after blocker； -- 审计写入失败的处理策略。 - -阶段 0 可以先把统一审计事件接入现有日志 sink，但事件 schema 必须稳定。阶段 4 开放任何形式的 -Recover 前必须具备持久本地审计；Recover 审计无法写入时 fail-closed。Stop/StopAll 不能因审计 -sink 不可用而被拒绝，实现应保留本地应急日志并继续停止。普通控制是否因审计失败而拒绝由 -Profile 决定。 - -### 16.7 后续启用认证的变更面 - -阶段 0 边界完成后，从 Disabled 升级到 Static Token/JWT/mTLS 只允许修改或新增： - -- `GrpcServerTask` 的 credential/provider builder； -- `GrpcAuthenticationProvider` 实现、secret/identity 配置加载和角色映射； -- 客户端统一 metadata interceptor 或 channel credential； -- 对应 Profile 的集成测试、密钥轮换和部署 Runbook。 - -以下内容不应因认证升级而修改： - -- 设备命令 request/feedback Proto； -- `DeviceManager`、`SafetyManager`、Sensor/Control policy； -- `DeviceSafetyEndpoint`、`SafetyParticipant` 和厂商驱动； -- handler 内的命令准入、StopAll 或 Recover 业务分支。 - -安全 Profile 只能在重启时切换。重启会生成新的 service instance ID，进程内 anonymous ledger -自然失效；不做运行期 `anonymous -> authenticated principal` 账本迁移，也不允许认证加载失败时 -保留旧监听并降级运行。 - -## 17. 配置设计 - -### 17.1 SafetyManagerConfig - -建议在 `DeviceManagerConfig` 中增加： - -```protobuf -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; -} -``` - -阶段 1 到阶段 4 允许旧配置缺失并进入 Legacy/Shadow，同时产生高可见度告警。阶段 5 的 -Production 配置若仍为 UNSPECIFIED 或 LEGACY，启动失败。 - -### 17.2 设备级覆盖 - -设备 entry 可增加： - -- 是否纳入当前 enforce rollout； -- 更短的 snapshot freshness； -- 更短的 stop/recovery timeout； -- 是否为启动所必需。 - -配置不能： - -- 把 Control endpoint 改成 Sensor； -- 把 required safe-stop 改成 optional； -- 允许 Unknown 通过； -- 关闭驱动最终硬件检查； -- 远程修改 enforce 为 legacy。 - -策略族和 capability 由编译后的 adapter 注册，配置只能收紧。 - -该启动失败开关只针对结构性缺陷，例如控制设备没有 endpoint 或 safe-stop capability。运行时 -设备断线、急停或动态 Unknown 应进入 Blocked/Latched，并保持管理面在线。 - -### 17.3 GRPCSecurityConfig - -建议在现有 `GRPCServerConfig` 中增加独立安全配置。Transport 和 Authentication 不合并成一个 -布尔值，避免以后只能通过客户端证书获得加密连接： - -```protobuf -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; - bool enable_reflection = 5; - - string server_certificate_file = 6; - string server_private_key_file = 7; - string client_ca_file = 8; - string static_token_file = 9; - string jwt_issuer = 10; - string jwt_audience = 11; - string audit_file = 12; -} -``` - -阶段 0 只实现 `INSECURE + DISABLED`，以及 `RECOVERY_DISABLED/RECOVERY_LOCAL_ONLY` 的策略； -其他枚举值先形成稳定配置契约，选择未编译能力时返回明确启动错误。旧配置缺失该 message 时可有 -一个发布周期映射到当前行为，但必须告警；迁移窗口结束后 UNSPECIFIED 一律启动失败。 - -后续 provider 实现后的组合约束： - -| Transport | Authentication | 是否允许 | 说明 | -| --- | --- | --- | --- | -| Insecure/Unix socket | Disabled | 是 | Compatibility；按监听范围和 RecoveryExposure 限制 | -| Insecure TCP | StaticToken/JWT | 否 | 凭据可被监听和重放 | -| Server TLS | Disabled | 是 | 只加密，不识别客户端，仍属于 Compatibility | -| Server TLS | StaticToken/JWT | 是 | 推荐的轻量远程认证 | -| Mutual TLS | TLSClientCertificate | 是 | 后续强身份 Profile | -| Mutual TLS | Disabled | 否 | 要求客户端证书却丢弃身份没有明确语义 | - -首轮不实现多因素组合；如果未来需要 mTLS + Token，必须定义唯一 principal、角色合并和审计规则， -不能简单拼接两个 provider 的结果。 - -配置只保存凭据文件路径，不保存 Token 明文。后续实现静态 Token 时使用权限受限的独立文件， -日志和审计只能记录 token ID，不能记录 secret。所有组合在启动时集中校验，并把实际生效值写入 -capability manifest/SystemInfo。 - -## 18. 当前设备迁移映射 - -| 设备/入口 | 策略 | 关键安全事实 | Stop/恢复要点 | -| --- | --- | --- | --- | -| Aubo Arm | Control | connected、robot mode、exec/queue、power、硬件/软件 EStop、protective stop、fault | 硬件 EStop 释放后在 Normal/Reduced 下自动 poweron/startup，Running 且队列清空、quiescent 后才重新准入；显式 Stop/PowerOff 优先；软件 EStop 独立锁存；无法确认 exec 时 OutcomeUnknown | -| Huayan Arm | Control | lifecycle generation、motion state、fault、stop confirmation | 保留已强化的 fail-closed 生命周期，映射为统一 endpoint | -| MotorRobotArm | Control | group atomicity、joint freshness、bus generation | 不具备原子 group servo 时继续拒绝 teleop capability | -| UME RobotArm | Control | CAN session、watchdog、torque enable、feedback freshness | reconnect 不恢复 torque；本地 haptic loop 不做网络调用 | -| ArmTeleopService | Control（stream） | session ID、sequence、watchdog、lease、safety epoch | OPEN 时准入；每帧校验 epoch；StopAll 后必须新 session | -| SEER AGV | Control | controller session、navigation terminal、velocity、EStop、fault | cancel ACK 不等于停稳；连续零速度采样后才 quiescent | -| MotorManager/Motor | Control | bus session epoch、CiA402 state、enabled、quick-stop、actual velocity | 每个 motor resource 注册；Quick Stop 未确认则 quarantine | -| DexHand control | Control | hand lifecycle、command generation、actuator idle | tactile stream 与控制命令分开分类；stopOperationalActivity 必须可证明 | -| DexHand tactile | Sensor | polling worker、sample freshness | StopAll 可停 stream，但不能把 stream 状态当作手部运动状态 | -| Camera capture | Sensor | opened、streaming、worker generation | 复用 MediaSourceManager；gRPC/QUIC 和直接 startStreaming 在启动设备 producer 前取得 Sensor/StartActivity dispatch guard | -| Camera PTZ | Control | PTZ activity generation、stop ACK | 服务端按已校验 action 派生 START=Actuate、STOP=安全通道，锁止时 STOP 仍可下发 | -| Microphone | Sensor | capture lifecycle、sample freshness | activity participant | -| Speaker | Sensor | playback lifecycle、worker generation | Stop 始终允许；不视为机械执行器 | -| BioHead | Control | expression/speech activity、hardware fault | 表情可能产生机械运动，不能归入纯媒体 | -| Battery | Sensor | sample freshness、communication state | 只读；不参与 actuator StopAll | -| MuJoCo control | Control | simulation generation、active command | 使用与真机相同策略，便于故障注入，但不能代替真机验收 | -| HLC/touch | Control | 服务端解析真实 arm ID、任务 safety session、每次 arm submission 复核 | `touch` 为 Actuate；每次 move/speed/servo 前依次复核 permit、control authority、Coordinator dispatch 和设备最终检查；撤销后走 Stop lane | -| QUIC media | Sensor | Camera/Microphone source ID、snapshot freshness、media generation | 不提供执行器控制；全局 Hub 的设备 producer 启动使用与 gRPC 相同的 Sensor/StartActivity dispatch guard | - -Aubo JSON `get_di/get_do` 可以归类 Observe；`set_do` 必须归类 Configure/Control，并最终迁移 -为 typed RPC。未知 JSON command 在 Control 设备上默认拒绝。 - -## 19. 六阶段实施方案 - -| 阶段 | 核心产物 | 是否改变生产准入 | -| --- | --- | --- | -| 0 | RequestContext、AuthN/AuthZ/Audit 扩展边界 | 否，显式保持现有兼容行为 | -| 1 | 类型、快照、错误、幂等契约 | 否，旧 gate 仍权威 | -| 2 | SafetyManager shadow | 否，只比较决策 | -| 3 | 高风险设备逐个 enforce | 仅改变选中设备 | -| 4 | 泛化 StopAll、正式 Recover | 改变系统安全事务 | -| 5 | EnforceAll、真机签字、移除旧路径 | 全量切换 | - -每个阶段只有满足退出条件后才能进入下一阶段；不能为了尽快提供 Recover 而跳过阶段 0、1、 -2 或高风险设备迁移。 - -### 阶段 0：建立控制面安全扩展边界 - -#### 目标 - -在不增加现有客户端证书或 Token 配置负担的前提下，建立统一请求上下文和可替换的安全网关， -使以后增加认证只替换 provider/policy，不横向修改所有 Service。当前明文匿名暴露被显式记录， -但本阶段不改变已有普通 RPC 的允许/拒绝行为。 - -#### 实施项 - -1. 扩展 `grpc_server_config.proto`，增加 Transport、Authentication、RecoveryExposure 和 - `allow_insecure_non_loopback`；本阶段只实现 Insecure + Disabled。 -2. 定义不可变 `RequestContext`、`Principal`、与 gRPC 无关的 `CommandActor`，以及明确的 - effective principal 规则。 -3. 实现 `DisabledAuthenticationProvider`、`GrpcAuthorizationPolicy`、 - `GrpcSecurityGateway/GrpcCallGuard` 和可注入 fake provider。 -4. 建立完整 method 名驱动的 `GrpcMethodPolicyRegistry`，先固定 read/mutate/stop/recover、 - 最低未来 role；CommandIntent 最迟在阶段 2 补齐。 -5. 所有现有 handler 统一通过 call guard 取得上下文；Service 不直接读取认证 metadata， - 不从 request 读取 principal/role。 -6. Compatibility policy 保持现有 RPC 行为。`RecoverSafetyState` 标记为独立 Recovery policy， - 此阶段只预留，不注册实现。 -7. 增加 correlation ID 和统一审计事件 schema，先接入现有日志 sink；保留 error logging - interceptor，并明确多个 interceptor/call guard 的生命周期顺序。 -8. GrpcServerTask 集中校验配置组合；选择 StaticToken/JWT/mTLS 等未实现 provider 时启动失败， - 不允许静默退回 Disabled。 -9. QUIC heartbeat、GetSystemInfo、启动日志和指标发布实际 transport、authentication 和 - recovery exposure；非 loopback 明文匿名模式持续告警。 -10. 为旧配置提供一个发布周期的兼容映射，并更新部署文档和显式示例配置。 - -#### 测试 - -- Disabled provider 忽略伪造的 principal/role metadata，结果始终是未认证 `anonymous`； -- 现有客户端不增加 metadata 仍可调用已有 RPC，允许/拒绝结果不变； -- IPv4/IPv6 loopback、Unix Domain Socket 和非本机 peer 分类； -- Disabled + Authorized Recovery、未实现 provider、非 loopback insecure 未显式确认等非法组合； -- fake authenticated provider 下 Observer/Operator/SafetyAdmin 的 method policy； -- sync、stream、callback、deadline/cancellation 下 RequestContext 生命周期和清理； -- correlation ID、审计字段脱敏、audit sink 异常和 interceptor 顺序； -- 旧配置迁移及实际 security capability 上报。 - -#### 退出条件 - -- 每个现有 gRPC method 都经过统一 gateway，并有 access class； -- `anonymous` 只能由服务端生成，伪造 metadata 不会获得 role； -- 当前兼容模式行为未改变，但明文/匿名/监听范围在配置和运行状态中可见； -- 非本机匿名 Recover 没有可执行路径； -- fake provider 测试证明启用认证不需要修改业务 handler、Coordinator 或驱动； -- 部署文档、风险说明和显式示例配置已更新。 - -#### 回滚边界 - -可以让 Compatibility policy 继续保持旧行为，但 RequestContext、method registry 和配置字段 -不能删除；否则会重新引入后续横向改造。任何安全 Profile 变化只能通过本机配置和进程重启， -不能提供远程降级 RPC。 - -### 阶段 1：统一类型、快照、错误与幂等契约 - -#### 目标 - -建立后续状态机所需的数据契约，但不改变现有命令准入结果。 - -#### 实施项 - -1. 新建 `manager/safety_manager` target 和 `safety_types.h`。 -2. 定义 CommandIntent、SafetyCondition、TriState、SafetyBlocker、SafetySnapshot。 -3. 定义 DeviceSafetyDescriptor、Endpoint、Participant 接口。 -4. 在 DeviceManager 中建立 `SafetySnapshotStore`，Manager snapshot 只读缓存。 -5. 为现有设备建立 adapter；未支持设备发布 Unknown，不伪造安全。 -6. 扩展 `CommandHeader` 和 Feedback；新增 reason code 和 execution state。 -7. 生成统一 control service instance ID，并通过 GetSystemInfo 暴露。 -8. 实现进程内 CommandLedger，复用 ActionQueue 的容量和 retired-ID 设计原则。 -9. 建立 vendor result 到公共 reason code 的映射层。 -10. 给 AbstractDevice 的宽松默认生命周期能力增加弃用标记；Control 注册要求显式能力。 -11. 更新 Manager/Service/Proto 文档中已经过时的生命周期说明。 - -#### 测试 - -- snapshot 并发发布/读取、stale 计算、generation 单调性； -- endpoint 未实现、设备重连、设备重新注册； -- ledger 同 ID 同 payload、不同 payload、in-flight join、淘汰和容量耗尽； -- deterministic payload hash； -- reason code 映射完整性； -- protobuf 旧 client payload 解析和旧配置解析。 - -#### 退出条件 - -- gRPC 状态查询和 QUIC heartbeat 不再调用设备方法； -- 所有 Control 类型至少有显式 Unknown adapter，不再依赖默认 Healthy； -- 新字段保持 wire compatible； -- ledger 的 OutcomeUnknown 测试证明不会二次 dispatch； -- 此阶段生产行为仍由旧 gate 决定。 - -#### 回滚边界 - -新 Proto 字段不可删除或复用；可以停止使用新字段，但必须保留 wire schema。SnapshotStore -可以退回仅诊断模式，不影响旧 gate。 - -### 阶段 2：SafetyManager 影子运行 - -#### 目标 - -在不改变线上允许/拒绝结果的情况下，对全部命令计算新策略结果并验证分类完整性。 - -#### 实施项 - -1. DeviceManager 构造并持有 SafetyManager。 -2. 在阶段 0 的 GrpcMethodPolicyRegistry 中补齐 CommandIntent，并引入 typed CommandDescriptor。 -3. gRPC service 增加可注入构造函数；GrpcServerTask 统一传入 coordinator/ledger，沿用既有 - GrpcSecurityGateway。 -4. 每个 handler 在旧 gate 前后调用 shadow evaluation，记录旧/新决策差异。 -5. 把 StopAll gate、Motor、Media、Camera registry、ActionQueue 包装成 legacy participant。 -6. 新增只读 `GetSafetyState`，先发布 shadow decision、freshness 和 blocker。 -7. 增加 admission latency、decision mismatch、unknown snapshot、unclassified method 指标。 -8. 启动状态使用 Starting；只做诊断，不因 shadow 结果阻止旧业务。 -9. 将 Runtime/TaskManager 划分为 management-plane 和 operational 启动组；设备动态故障不再 - 使已经通过所选安全 Profile 配置校验的 SystemService 一并退出。 - -#### 影子比较分类 - -| 旧结果 | 新结果 | 处理 | -| --- | --- | --- | -| Allow | Allow | 正常 | -| Deny | Deny | 正常，比较 reason | -| Allow | Deny | 记录 `would_deny`，优先修正快照或旧行为 | -| Deny | Allow | 高风险 `would_allow`，在进入阶段 3 前必须归零或有书面解释 | - -#### 测试 - -- 所有 protobuf service method 都在 method access policy 和 command-intent registry 中； -- shadow evaluation 无硬件 I/O； -- coordinator 销毁时 participant 全部注销且无 callback； -- StopAll 与 shadow admit 并发不改变旧 gate 行为； -- GetSafetyState 在设备 endpoint 阻塞时仍快速返回。 -- 单个设备 create/init/start 失败时，GetSafetyState 仍可按当前安全 Profile 访问。 - -#### 退出条件 - -- 所有 mutating method 均有固定 intent； -- 所有 Control 设备都有 DeviceSafetyDescriptor； -- 没有未解释的 `legacy deny / new allow`； -- Shadow 准入 p99 只包含内存操作，不受硬件 RTT 影响； -- 至少完成一轮真实运行日志评审。 -- 管理面 degraded-start 和正常 shutdown 路径都有生命周期测试。 - -#### 回滚边界 - -可关闭 shadow evaluation，但保留 snapshot 和 method registry。旧 gate 仍是唯一 authority。 - -### 阶段 3：迁移高风险控制设备 - -#### 目标 - -让 Arm、Teleoperation、AGV、Motor、DexHand control 和 Camera PTZ 的普通命令由 -SafetyManager 权威准入，并统一幂等与执行结果语义。 - -#### 迁移顺序 - -1. Aubo/Huayan unary Arm； -2. ArmTeleopService 和 UME session； -3. AGV navigation/velocity； -4. Motor unary 和 cyclic stream； -5. DexHand control； -6. Camera PTZ； -7. ActionQueue step dispatch。 - -#### 单设备迁移步骤 - -1. 完成 endpoint 和新鲜 snapshot； -2. 明确每个 SDK 返回值的 Accepted/Completed/Rejected/Unknown 语义； -3. 实现 safe-stop request 和 quiescence verification； -4. handler 先通过 Coordinator，再保留 legacy gate 作为第二道 deny-only adapter； -5. dispatch 前执行 permit revalidation 和 driver final check； -6. unary mutating 命令接入 CommandLedger； -7. 流式命令绑定 safety epoch、device generation 和 lease generation； -8. 开启该设备 `ENFORCE_SELECTED`； -9. 通过 fake、并发和真机测试后，删除该设备 handler 中重复的旧状态判断。 - -#### 驱动执行契约 - -- Aubo queue full 只有在 SDK 能证明命令未接收时才能返回 Backpressure； -- 未及时观察到 exec ID 但无法证明未接收时返回 OutcomeUnknown，并 quarantine arm； -- Huayan 保留已有的停止确认和生命周期 generation； -- AGV cancel/zero command ACK 后继续等待导航终态和连续零速度样本； -- Motor Quick Stop 未确认时保留 motor resource quarantine； -- Teleop/stream 的 reconnect 总是新 session，旧轨迹不续跑； -- DexHand void 返回接口逐步改为结构化 result，不能只靠日志判断成功。 - -#### 测试 - -- admit 与 StopAll 的线性化竞态； -- snapshot 在 admit 后、dispatch 前过期； -- lease 在 dispatch 前被抢占； -- ACK 丢失、队列满、控制器断线、进程重连； -- 同 command ID 并发请求； -- stream sequence 重复、倒退、watchdog、断线； -- Stop 后旧 session/permit 无法恢复； -- 每个设备的真实停车确认。 - -#### 退出条件 - -- 上述高风险入口不存在绕过 Coordinator 的设备调用； -- 每类命令都有结构化结果，Internal 不再承载所有业务错误； -- 旧 gate 只作为兼容 deny，不再能单独 reopen 新 Coordinator； -- 设备级 OutcomeUnknown 可通过 GetSafetyState 定位并进入恢复流程； -- 新控制设备接入安全层不需要修改 SafetyManager switch。 - -#### 回滚边界 - -按 device ID 从 EnforceSelected 退回 Shadow。已产生的 quarantine 不能因回滚配置自动清除， -必须 StopAll 成功或通过当前 RecoveryExposure 允许的恢复流程处理。 - -### 阶段 4：泛化 StopAll 并开放正式恢复 RPC - -#### 目标 - -删除 SystemService 中的设备类型停止编排，使 StopAll 和 Recover 成为统一、可观察、可重试的 -安全事务。 - -#### 实施项 - -1. 实现 `SafetyOperationOrchestrator` 和 participant 分阶段执行。 -2. 所有 legacy coordinator 注册为 participant，保留其已有 barrier 语义。 -3. 设备通过注册的 safe-stop participant 加入，不再由 SystemService dynamic cast。 -4. `gRPCSystemServiceImpl::StopAll` 缩减为 RequestContext、参数适配和结果映射。 -5. StopAll 请求增加 operation ID、诊断用 service instance 和结构化 per-target response；旧或 - 缺失 instance 不能阻止安全停止。 -6. 实现 RecoveryLedger、active refresh、quiescence check 和软件 latch reconcile。 -7. 在 SystemService 注册 `GetSafetyState` 和 `RecoverSafetyState`。 -8. Recover 统一执行 RecoveryExposure：Disabled 拒绝、LocalOnly 校验实际本机 peer、 - Authorized 要求 authenticated SafetyAdmin；所有允许路径都要求非空 reason 并写持久审计。 -9. `clearForTesting()` 保持测试可见或移入 test support，运行代码无法调用。 -10. 调整 Runtime shutdown：先进入 ShuttingDown/关闭准入，再在 participant 存活时执行 - 有界 quiesce，之后停止 TaskManager 和 DeviceManager。 - -#### 并发规则 - -| 并发场景 | 规则 | -| --- | --- | -| StopAll vs normal command | epoch 线性化；命令要么被拒绝，要么被登记并停止 | -| StopAll vs StopAll | 加入同一 round，返回同一结果 | -| Recover vs normal command | Recovering 全程关闭普通准入 | -| Recover vs StopAll | process operation mutex 串行化，Stop 优先 | -| Recover vs Recover | 相同 recovery ID join；不同 ID 串行 | -| RPC cancellation vs Stop/Recover | 只取消 waiter，不取消已开始的系统安全事务 | -| late worker vs new epoch | 只能写旧 operation 诊断，不能 release 新 barrier | -| shutdown vs Recover | shutdown 终止新 recovery，保持 gate 关闭并执行 quiesce | - -#### 测试 - -- participant 注册顺序随机但执行阶段稳定； -- participant throw、timeout、永不返回、返回 Unknown； -- 部分恢复和 global blocker； -- Recover 时硬件 EStop、protective stop、fault、stale、disconnect、still moving； -- 恢复过程中状态再次变坏； -- expected epoch mismatch 和 recovery ID payload conflict； -- Disabled/LocalOnly/Authorized 三种 RecoveryExposure，尤其是非本机伪造 metadata 无法绕过； -- audit 写失败； -- service 析构、dispatcher join 和 Runtime shutdown。 - -#### 退出条件 - -- SystemService StopAll 不包含具体 DeviceKind 停止分支； -- StopAll/Recover 都返回 per-participant 结构化结果； -- 任何失败路径都保持明确 Latched/Quarantined 状态； -- Recover 无法绕过 Unknown、未结束 worker 或未确认运动； -- 一次成功 Recover 使系统能接受新命令，但不会恢复旧命令或自动使能硬件； -- Recover RPC 默认 Disabled；匿名部署只有 LocalOnly 验收通过才能开放，远程开放必须启用 - authenticated SafetyAdmin；任何开放模式都要求持久审计验收通过。 - -#### 回滚边界 - -保留旧 StopAll adapter 一个发布周期，可通过本地启动配置切换实现。正式 Recover 已产生的 -epoch 和审计不能回滚；禁止退回运行期无检查 clear。 - -### 阶段 5：全量 enforce、真机验证和旧路径移除 - -#### 目标 - -覆盖剩余外围设备和所有命令入口，验证发布产物与真实硬件，并删除重复全局状态。 - -#### 实施项 - -1. 迁移 Camera/Microphone/Speaker/BioHead/HLC 和剩余 local task； -2. 对所有 protobuf method 做 descriptor 驱动的 access-policy/intent 完整性测试； -3. Production 强制 `ENFORCE_ALL`，Control Unknown 拒绝； -4. 删除 service 对 `globalStopAllAdmissionGate()`、global media/motor registry 的业务依赖； -5. 保留必要 registry 的领域功能，但准入 authority 只属于 Coordinator； -6. 建立 build capability manifest，包含已编译的 transport/auth provider、MsQuic、硬件 SDK、 - Safety schema version； -7. GetSystemInfo 和启动日志发布实际 capability，不以源码存在推断二进制能力； -8. 建立安装产物 smoke test，而不只测试 build tree； -9. 建立真实设备台架和故障注入矩阵； -10. 完成运维 Runbook：StopAll、blocker 诊断、硬件处理、dry-run、Recover 和审计查询。 - -#### 真机矩阵 - -至少覆盖： - -- Aubo/Huayan：运动中断网、ACK 丢失、queue full、保护停、急停释放、SDK reconnect； -- AGV：导航取消 ACK 后仍移动、速度反馈丢失、地图切换中 StopAll； -- Motor：Modbus/EtherCAT 断线、bus epoch 改变、Quick Stop 未确认、stream watchdog； -- DexHand/PTZ/BioHead：连续命令中 StopAll、stop ACK 丢失、sensor stream 并发； -- 进程：控制中 SIGTERM、任务启动失败、服务重启后硬件仍活动； -- 网络：RPC 超时后相同 ID 重试、不同 payload 冲突、匿名 namespace、实际 peer 分类；如果构建 - 包含认证 provider，再覆盖 Token/证书轮换和权限变化； -- 多平台 QUIC：重连和多 heartbeat 不能改变本地控制 safety epoch。 - -#### 退出条件 - -- 所有 mutating 入口都经过 Coordinator 或显式安全通道； -- 所有 Control capability 都有 fresh snapshot、final check、safe-stop 和真机签字； -- 进程重启不会自动继续运动或接受旧请求； -- install artifact capability 与配置一致； -- 全量自动测试、TSAN/并发测试和硬件台架通过； -- 运维人员能只依赖结构化状态完成一次故障定位和恢复； -- 删除遗留 runtime clear 和无主 global gate。 - -#### 回滚边界 - -单设备可以通过本地配置从 EnforceSelected 退回 Shadow，但 RequestContext 安全边界、幂等和 -Stop/Recover epoch 不能关闭。RecoveryExposure 或网络暴露范围的任何放宽都要求重启、审计和 -现场审批，不能通过远程 RPC 修改。 - -## 20. 测试体系 - -### 20.1 单元测试 - -- Sensor/Control policy 全状态表； -- safety state transition 和非法 transition； -- snapshot freshness 和 generation； -- permit move/expiry/revalidation； -- ledger 和 recovery ledger； -- reason code 与 gRPC 映射； -- participant barrier/token epoch； -- RequestContext 构造、anonymous effective principal、provider/policy 组合； -- RecoveryExposure 和 actual peer classifier。 - -### 20.2 并发与性质测试 - -建议用可控 scheduler/fake clock 重复验证： - -1. StopAll 和 dispatch 的所有交错最终满足“不丢失活动”； -2. Recover 和 late stop callback 的所有交错都不能提前 reopen； -3. 相同 command ID 任意并发度下 dispatch count 最大为 1； -4. generation 单调增加，旧 permit 永远无法重新合法； -5. participant 异常不会跳过其他 required stop； -6. Coordinator 销毁后不存在访问已释放对象的 callback。 - -### 20.3 Service 集成测试 - -- Disabled/Fake provider、method policy 和伪造 metadata； -- legacy/new protobuf client； -- handler 分类完整性； -- Stop/Status 在 Latched 时可调用，Recover 在 exposure 允许时不受普通 gate 阻塞； -- Recover per-target response； -- gRPC deadline/cancellation 与 operation 生命周期分离。 - -### 20.4 故障注入 - -| 故障点 | 期望结果 | -| --- | --- | -| Admit 后 snapshot 过期 | final check 拒绝，无 SDK dispatch | -| SDK 调用超时且接收状态未知 | OutcomeUnknown + device quarantine | -| Stop worker 超时 | global Latched，worker token 保留 | -| Recovery refresh 超时 | blocker remains，不清 gate | -| 相同 ID 不同 payload | conflict，无第二次 dispatch | -| 进程重启期间硬件仍运动 | Starting/Latched，不自动 Open | -| 审计落盘失败 | Recover fail-closed | -| 一个设备 health worker 卡住 | 其他 Status/Stop/Recover 仍可执行 | - -### 20.5 发布产物验证 - -- 对安装目录启动二进制； -- 校验 capability manifest； -- Insecure + Disabled 的 loopback 和显式 trusted-network grpcurl smoke； -- Recovery Disabled/LocalOnly 的实际 peer smoke； -- reflection 与安全 Profile 的显式配置 smoke； -- 配置要求未编译 AuthN/Transport provider、MsQuic 或 driver capability 时启动失败； -- 构建包含可选 Token/TLS/mTLS provider 时，才执行对应认证矩阵； -- 生成 C++ 和 Java API 兼容测试。 - -## 21. 可观测性与运维 - -### 21.1 指标 - -建议至少提供： - -```text -cmvr_safety_admission_decisions_total{device,intent,decision,reason} -cmvr_safety_snapshot_age_ms{device} -cmvr_safety_device_state{device,state} -cmvr_safety_global_state{state} -cmvr_safety_stop_duration_ms{participant,result} -cmvr_safety_recovery_attempts_total{result} -cmvr_command_dedup_total{status} -cmvr_command_outcome_unknown_total{device,method} -cmvr_control_lease_conflicts_total{resource} -cmvr_grpc_authz_denied_total{method,role} -cmvr_grpc_requests_total{auth_method,authenticated} -cmvr_grpc_insecure_listener{non_loopback} -cmvr_grpc_recovery_access_denied_total{exposure,peer_kind,reason} -``` - -### 21.2 事件历史 - -Coordinator 保存有界内存事件环，并将关键事件写入持久审计： - -- global/device transition； -- snapshot stale/fresh、generation changed； -- quarantine 创建和清除； -- StopAll/Recover phase； -- permit reject、final check reject； -- OutcomeUnknown； -- participant timeout/late completion。 - -错误字符串只用于人读，自动化必须使用 reason code 和结构化 blocker。 - -### 21.3 Runbook 顺序 - -1. 调用 GetSafetyState 获取 epoch 和 blocker； -2. 确认当前 RecoveryExposure；Disabled 模式不能尝试网络绕过，需按本机变更流程处理； -3. 在现场确认物理环境和设备状态； -4. 必要时调用 typed ResetFault，不能直接 Recover 代替硬件处理； -5. 使用符合 LocalOnly/Authorized 策略的调用方执行 RecoverSafetyState `VERIFY_ONLY`； -6. blocker 全部满足后调用 `CLEAR_SOFTWARE_LATCH`； -7. 重新读取 SafetyState，确认新 epoch 和 Open/设备级状态； -8. 使用新的 command ID、service instance 和 device generation 下发后续命令。 - -## 22. 建议 PR 拆分 - -为避免一次性改动所有设备，建议按以下独立可回滚单元提交： - -1. 设计文档和安全不变量； -2. GRPCSecurityConfig、RequestContext、Disabled/Fake provider、method policy 和 call guard； -3. correlation/audit schema、实际 security capability 上报和 Compatibility Profile 测试； -4. Safety types、snapshot store 和 tests； -5. CommandHeader/reason code additive Proto； -6. CommandLedger； -7. SafetyManager shadow 和 GetSafetyState； -8. legacy participant adapters； -9. Arm + ArmTeleop migration； -10. AGV migration； -11. Motor migration； -12. DexHand/PTZ/ActionQueue migration； -13. generic StopAll； -14. RecoverSafetyState； -15. peripheral migration、EnforceAll 和 legacy removal； -16. capability manifest、install smoke 和硬件验证报告； -17. 可选后续：Server TLS + Static Token provider 和客户端 interceptor； -18. 可选后续：JWT/OIDC 或 mTLS provider、正式身份生命周期和密钥轮换。 - -每个迁移 PR 必须包含：命令分类表、snapshot 定义、stop verification、错误映射、竞态测试和 -回滚开关。不得只把 handler 前的一个 `if` 移到 Coordinator 就宣称完成迁移。 - -## 23. 最终验收清单 - -- [x] 所有 gRPC handler 都通过统一 RequestContext/call guard，业务代码不解析认证 metadata -- [x] Disabled 模式始终产生未认证 anonymous principal，实际安全 Profile 和监听风险可观测 -- [x] 所有方法都有 access policy 和 CommandIntent，未知方法不会绕过安全网关 -- [x] Recover 默认 Disabled；匿名模式只允许 LocalOnly，远程模式只允许 authenticated SafetyAdmin -- [x] Recover 所有开放方式都要求持久审计，审计失败时 fail-closed -- [ ] 启用 Authenticated Profile 时，传输/AuthN/AuthZ 按该 Profile 的独立验收矩阵通过 -- [x] 生命周期、健康和安全状态是独立字段 -- [x] Manager snapshot/status 路径只读 Manager 缓存，不执行设备 I/O -- [x] 普通 mutating unary 命令使用 command ID、service instance 和统一 ledger 语义 -- [x] 流式控制绑定 session、sequence、watchdog 和 safety epoch -- [x] 已迁移的 Control 命令经过统一准入和驱动最终检查 -- [x] Stop/Status 在软件锁止状态下可达；Recover 在 exposure 允许时不受普通 gate 阻塞 -- [x] SystemService StopAll 不按 DeviceKind 硬编码设备停止逻辑 -- [x] StopAll 失败留下结构化 blocker 和不可绕过 latch -- [x] Recover 不能忽略 Unknown、EStop、未确认运动或旧 worker -- [x] Recover 不上电、不使能、不恢复旧轨迹或旧 session -- [x] 同一 command ID 最大一次 dispatch，OutcomeUnknown 不自动重发 -- [x] 新设备只需注册 DeviceSafetyDescriptor、Endpoint、Participant 和 method intent -- [x] startup/shutdown 与 safety epoch 有明确顺序 -- [x] 单个设备动态故障不会关闭按当前安全 Profile 启动的 SystemService 管理面 -- [ ] 所有发布 capability 来自实际安装二进制 -- [ ] fake、并发、故障注入、安装产物和真机台架全部完成 - -## 24. 实施前需要确定的工程参数 - -以下参数需要通过设备手册和台架测量确定，但不改变架构： - -- 各设备 snapshot polling 周期和 maximum age； -- StopAll 总 deadline 与每类 participant timeout； -- 连续零速度/静止确认的阈值和样本数； -- Aubo、Huayan、SEER 等 SDK 对“已接受命令”的精确定义； -- ledger 容量和结果保留时间； -- 审计文件保留、轮转和上传策略； -- 当前部署允许监听的网卡/CIDR、主机防火墙规则和 `allow_insecure_non_loopback` 责任人； -- RecoveryExposure 初始选择 Disabled 还是 LocalOnly，以及本机维护入口； -- 旧安全配置兼容窗口长度，以及启用 Static Token/Server TLS 的触发条件； -- 后续认证启用时的 principal 命名、Token/证书轮换和角色绑定； -- 哪些设备是启动时 required control device。 - -这些值必须以显式配置和硬上限进入代码，不能以缺省零值表达“无限”或“允许全部”。 diff --git a/docs/teleoperation/ume_cmvr_architecture.md b/docs/teleoperation/ume_cmvr_architecture.md deleted file mode 100644 index b86f9c4f..00000000 --- a/docs/teleoperation/ume_cmvr_architecture.md +++ /dev/null @@ -1,111 +0,0 @@ -# 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 deleted file mode 100644 index 8fb1cda2..00000000 --- a/docs/teleoperation/ume_cmvr_validation.md +++ /dev/null @@ -1,118 +0,0 @@ -# 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/model/ume/README.md b/model/ume/README.md deleted file mode 100644 index c7ac5076..00000000 --- a/model/ume/README.md +++ /dev/null @@ -1,28 +0,0 @@ -# UME legacy dynamics models - -These MJCF files are exact copies from Universal-Manipulation-Exoskeleton -commit `e087df5cd3b281418722e155d9975695f163698e`: - -- `v6_bimanual/robot.xml`: fixed-base model used for - `LOCAL_WORLD_ALIGNED` shoulder/wrist rotational Jacobians. - SHA-256: - `c185fe505ab52275d1be9c5b563df5c6505a3da81e0e622cffd66324e3839f3b`. -- `v6_imu/robot.xml`: floating-base model used for the original - `rnea(q, v, 0)` compensation path. - SHA-256: - `db1dad82ebca9413edf268a980c5be5a307c990b86654db840098095d8bd5e14`. - -The source paths are -`ume/robot/ume/v6_bimanual/models/robot.xml` and -`ume/robot/ume/v6_imu/models/robot.xml`, respectively. - -Only the model topology/dynamics parser is used. Pinocchio 3.6 -`mjcf::buildModel` and the adapter tests load these files without resolving -their visual STL assets, so no geometry files are duplicated here. - -The adapter enforces the model's structural contract. Integrations that require -byte-for-byte model identity must also compare the SHA-256 values above before -arming motion. - -The repository's root install rule copies `model/` to the deployment tree at -`bin/model/`. diff --git a/model/ume/v6_bimanual/robot.xml b/model/ume/v6_bimanual/robot.xml deleted file mode 100644 index de84254c..00000000 --- a/model/ume/v6_bimanual/robot.xml +++ /dev/null @@ -1,356 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/model/ume/v6_imu/robot.xml b/model/ume/v6_imu/robot.xml deleted file mode 100644 index 684292c4..00000000 --- a/model/ume/v6_imu/robot.xml +++ /dev/null @@ -1,389 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/model/xiaoyan_description/dual_arm.urdf b/model/xiaoyan_description/dual_arm.urdf index d033f89c..7e7ab687 100644 --- a/model/xiaoyan_description/dual_arm.urdf +++ b/model/xiaoyan_description/dual_arm.urdf @@ -274,36 +274,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto index b434d500..267c88f3 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/api/agv_utils.proto"; +import "cmvr/msgs/agv.proto"; // 查询 AGV 运行状态命令。 message AgvRuntimeStateCommand { @@ -45,7 +45,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; @@ -65,7 +65,7 @@ message AgvNavigateToStationCommand { CommandHeader.Request header = 1; // 目标站点 id。 string station_id = 2; - // 通用运动约束和执行选项；默认同步阻塞至任务终态并确认停车。 + // 通用运动约束和执行选项。 cmvr.msgs.AgvMotionOptions options = 3; // AGV 适配器扩展参数，用于传递厂商特有选项。 cmvr.msgs.AgvAdapterParams adapter_params = 4; @@ -85,8 +85,6 @@ message AgvFollowPathCommand { CommandHeader.Request header = 1; // 路径段列表。每段包含起点站点 id 和终点站点 id。 repeated cmvr.msgs.AgvPathSegment path = 2; - // 通用执行选项；默认同步阻塞至整条路径终态并确认停车。 - cmvr.msgs.AgvMotionOptions options = 3; } // 反馈体。 message Feedback { @@ -95,23 +93,6 @@ 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 b1b01e7e..f0bdc5b2 100644 --- a/protos/cmvr/api/agv_service.proto +++ b/protos/cmvr/api/agv_service.proto @@ -69,8 +69,4 @@ service AgvService { // 停止当前建图/扫图会话。 rpc stopMapping(CommandHeader.Request) returns (CommandHeader.Feedback); - - - // 按指定速度平移固定距离。成功返回仅表示控制器已接受命令。 - rpc translate(AgvTranslateCommand.Request) returns (AgvTranslateCommand.Feedback); } diff --git a/protos/cmvr/api/arm_service.proto b/protos/cmvr/api/arm_service.proto index 54c3f4aa..a07018f7 100644 --- a/protos/cmvr/api/arm_service.proto +++ b/protos/cmvr/api/arm_service.proto @@ -21,7 +21,4 @@ service ArmService { 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 deleted file mode 100644 index d7095a95..00000000 --- a/protos/cmvr/api/arm_teleop_v1.proto +++ /dev/null @@ -1,133 +0,0 @@ -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/common.proto b/protos/cmvr/api/common.proto index 64988d45..5686a18a 100644 --- a/protos/cmvr/api/common.proto +++ b/protos/cmvr/api/common.proto @@ -4,59 +4,6 @@ package cmvr.api; import "google/protobuf/timestamp.proto"; -enum CommandReasonCode { - COMMAND_REASON_CODE_UNSPECIFIED = 0; - COMMAND_REASON_CODE_NONE = 1; - COMMAND_REASON_CODE_INVALID_ARGUMENT = 2; - COMMAND_REASON_CODE_UNAUTHENTICATED = 3; - COMMAND_REASON_CODE_PERMISSION_DENIED = 4; - COMMAND_REASON_CODE_RECOVERY_RPC_DISABLED = 5; - COMMAND_REASON_CODE_DEVICE_NOT_FOUND = 6; - COMMAND_REASON_CODE_DEVICE_UNAVAILABLE = 7; - COMMAND_REASON_CODE_UNSUPPORTED_COMMAND = 8; - COMMAND_REASON_CODE_SYSTEM_STARTING = 9; - COMMAND_REASON_CODE_SYSTEM_STOPPING = 10; - COMMAND_REASON_CODE_SAFETY_LATCHED = 11; - COMMAND_REASON_CODE_SAFETY_STATE_MISSING = 12; - COMMAND_REASON_CODE_SAFETY_STATE_STALE = 13; - COMMAND_REASON_CODE_HARDWARE_UNSAFE = 14; - COMMAND_REASON_CODE_EMERGENCY_STOP_ACTIVE = 15; - COMMAND_REASON_CODE_PROTECTIVE_STOP_ACTIVE = 16; - COMMAND_REASON_CODE_DEVICE_DISCONNECTED = 17; - COMMAND_REASON_CODE_DEVICE_FAULT = 18; - COMMAND_REASON_CODE_DEVICE_NOT_READY = 19; - COMMAND_REASON_CODE_DEVICE_STILL_MOVING = 20; - COMMAND_REASON_CODE_CONTROL_BUSY = 21; - COMMAND_REASON_CODE_GENERATION_MISMATCH = 22; - COMMAND_REASON_CODE_COMMAND_ID_REQUIRED = 23; - COMMAND_REASON_CODE_COMMAND_ID_CONFLICT = 24; - COMMAND_REASON_CODE_RESULT_EVICTED = 25; - COMMAND_REASON_CODE_LEDGER_EXHAUSTED = 26; - COMMAND_REASON_CODE_BACKPRESSURE = 27; - COMMAND_REASON_CODE_DEADLINE_EXCEEDED_BEFORE_DISPATCH = 28; - COMMAND_REASON_CODE_OUTCOME_UNKNOWN = 29; - COMMAND_REASON_CODE_PARTICIPANT_TIMEOUT = 30; - COMMAND_REASON_CODE_STOP_UNCONFIRMED = 31; - COMMAND_REASON_CODE_RECOVERY_EPOCH_MISMATCH = 32; - COMMAND_REASON_CODE_RECOVERY_REASON_REQUIRED = 33; - COMMAND_REASON_CODE_RECOVERY_AUDIT_FAILED = 34; - COMMAND_REASON_CODE_INTERNAL_ERROR = 35; -} - -enum CommandExecutionState { - COMMAND_EXECUTION_STATE_UNSPECIFIED = 0; - COMMAND_EXECUTION_STATE_RECEIVED = 1; - COMMAND_EXECUTION_STATE_RESERVED = 2; - COMMAND_EXECUTION_STATE_REJECTED_BEFORE_DISPATCH = 3; - COMMAND_EXECUTION_STATE_ADMITTED = 4; - COMMAND_EXECUTION_STATE_DISPATCHING = 5; - COMMAND_EXECUTION_STATE_ACCEPTED_BY_HARDWARE = 6; - COMMAND_EXECUTION_STATE_COMPLETED = 7; - COMMAND_EXECUTION_STATE_FAILED = 8; - COMMAND_EXECUTION_STATE_CANCELED_BEFORE_DISPATCH = 9; - COMMAND_EXECUTION_STATE_OUTCOME_UNKNOWN = 10; -} - message DeviceLifecycle { enum Lifecycle { STATE_INIT = 0; @@ -73,22 +20,12 @@ message CommandHeader { message Request { string device_id = 1; // 目标设备名称 google.protobuf.Timestamp timestamp = 2; // 请求时间 - string command_id = 3; - string expected_service_instance_id = 4; - optional uint64 expected_device_generation = 5; - uint32 valid_for_ms = 6; } message Feedback { bool success = 1; // 是否成功 string error_message = 2; // 错误信息（成功时为空） google.protobuf.Timestamp timestamp = 3; // 回复时间 - CommandReasonCode reason_code = 4; - string command_id = 5; - string service_instance_id = 6; - uint64 safety_epoch = 7; - uint64 device_generation = 8; - CommandExecutionState execution_state = 9; } } diff --git a/protos/cmvr/api/motor_command.proto b/protos/cmvr/api/motor_command.proto deleted file mode 100644 index a15c1811..00000000 --- a/protos/cmvr/api/motor_command.proto +++ /dev/null @@ -1,151 +0,0 @@ -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 deleted file mode 100644 index 01f127f3..00000000 --- a/protos/cmvr/api/motor_service.proto +++ /dev/null @@ -1,21 +0,0 @@ -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/safety_command.proto b/protos/cmvr/api/safety_command.proto deleted file mode 100644 index b227f842..00000000 --- a/protos/cmvr/api/safety_command.proto +++ /dev/null @@ -1,186 +0,0 @@ -syntax = "proto3"; - -package cmvr.api; - -import "cmvr/api/common.proto"; - -enum SafetyTriState { - SAFETY_TRI_STATE_UNKNOWN = 0; - SAFETY_TRI_STATE_FALSE = 1; - SAFETY_TRI_STATE_TRUE = 2; -} - -enum SafetyCondition { - SAFETY_CONDITION_UNKNOWN = 0; - SAFETY_CONDITION_NOMINAL = 1; - SAFETY_CONDITION_RESTRICTED = 2; - SAFETY_CONDITION_UNSAFE = 3; -} - -enum SystemAdmissionState { - SYSTEM_ADMISSION_STATE_UNSPECIFIED = 0; - SYSTEM_ADMISSION_STATE_STARTING = 1; - SYSTEM_ADMISSION_STATE_OPEN = 2; - SYSTEM_ADMISSION_STATE_STOPPING = 3; - SYSTEM_ADMISSION_STATE_LATCHED = 4; - SYSTEM_ADMISSION_STATE_RECOVERING = 5; - SYSTEM_ADMISSION_STATE_SHUTTING_DOWN = 6; -} - -enum DeviceAdmissionState { - DEVICE_ADMISSION_STATE_UNSPECIFIED = 0; - DEVICE_ADMISSION_STATE_OBSERVING = 1; - DEVICE_ADMISSION_STATE_OPEN = 2; - DEVICE_ADMISSION_STATE_BLOCKED = 3; - DEVICE_ADMISSION_STATE_QUARANTINED = 4; - DEVICE_ADMISSION_STATE_RECOVERING = 5; - DEVICE_ADMISSION_STATE_REMOVED = 6; -} - -enum SafetyBlockerScope { - SAFETY_BLOCKER_SCOPE_UNSPECIFIED = 0; - SAFETY_BLOCKER_SCOPE_DEVICE = 1; - SAFETY_BLOCKER_SCOPE_SYSTEM = 2; -} - -enum SafetyRecoveryRequirement { - SAFETY_RECOVERY_REQUIREMENT_UNSPECIFIED = 0; - SAFETY_RECOVERY_REQUIREMENT_REFRESH_ONLY = 1; - SAFETY_RECOVERY_REQUIREMENT_CLEAR_SOFTWARE_LATCH = 2; - SAFETY_RECOVERY_REQUIREMENT_HARDWARE_RELEASE_REQUIRED = 3; - SAFETY_RECOVERY_REQUIREMENT_MANUAL_INSPECTION_REQUIRED = 4; -} - -enum SafetyOperationResult { - SAFETY_OPERATION_RESULT_UNSPECIFIED = 0; - SAFETY_OPERATION_RESULT_SUCCEEDED = 1; - SAFETY_OPERATION_RESULT_RECOVERED = 2; - SAFETY_OPERATION_RESULT_VERIFIED_BUT_STILL_BLOCKED = 3; - SAFETY_OPERATION_RESULT_BLOCKER_REMAINS = 4; - SAFETY_OPERATION_RESULT_EPOCH_MISMATCH = 5; - SAFETY_OPERATION_RESULT_NOTHING_TO_RECOVER = 6; - SAFETY_OPERATION_RESULT_TIMED_OUT = 7; - SAFETY_OPERATION_RESULT_FAILED = 8; -} - -message DeviceIdList { - repeated string device_ids = 1; -} - -message SafetyScope { - oneof target { - bool all_devices = 1; - DeviceIdList devices = 2; - } -} - -message SafetyBlockerInfo { - CommandReasonCode reason_code = 1; - SafetyBlockerScope scope = 2; - SafetyRecoveryRequirement recovery_requirement = 3; - string source_id = 4; - string operation_id = 5; - uint64 first_observed_at_unix_ms = 6; - uint64 last_observed_at_unix_ms = 7; -} - -message DeviceSafetyStateInfo { - string device_id = 1; - string device_kind = 2; - string policy_family = 3; - string lifecycle_state = 4; - string health_state = 5; - DeviceAdmissionState admission_state = 6; - SafetyCondition condition = 7; - bool has_sample = 8; - bool snapshot_fresh = 9; - uint64 sample_age_ms = 10; - uint64 sample_sequence = 11; - uint64 observed_at_unix_ms = 12; - uint64 device_generation = 13; - SafetyTriState connected = 14; - SafetyTriState operational_ready = 15; - SafetyTriState quiescent = 16; - SafetyTriState motion_active = 17; - SafetyTriState actuator_enabled = 18; - SafetyTriState emergency_stop_active = 19; - SafetyTriState protective_stop_active = 20; - SafetyTriState fault_active = 21; - repeated SafetyBlockerInfo blockers = 22; -} - -message SafetyOperationTargetResult { - string target_id = 1; - SafetyOperationResult result = 2; - CommandReasonCode reason_code = 3; - string detail = 4; - DeviceAdmissionState before_state = 5; - DeviceAdmissionState after_state = 6; -} - -message SafetyParticipantResultInfo { - bool recorded = 1; - bool success = 2; - CommandReasonCode reason_code = 3; - string detail = 4; -} - -message SafetyParticipantStateInfo { - string participant_id = 1; - string phase = 2; - bool required = 3; - bool registered = 4; - bool barrier_active = 5; - bool barrier_retained = 6; - string operation_id = 7; - uint64 safety_epoch = 8; - SafetyParticipantResultInfo last_request = 9; - SafetyParticipantResultInfo last_verify = 10; - SafetyParticipantResultInfo last_release = 11; -} - -message GetSafetyStateCommand { - message Request { - SafetyScope scope = 1; - } - - message Feedback { - CommandHeader.Feedback header = 1; - SystemAdmissionState system_state = 2; - uint64 safety_epoch = 3; - string control_service_instance_id = 4; - string enforcement_mode = 5; - repeated DeviceSafetyStateInfo devices = 6; - string active_operation_id = 7; - string active_operation_phase = 8; - uint64 sampled_at_unix_ms = 9; - repeated SafetyParticipantStateInfo participants = 10; - } -} - -message RecoverSafetyStateCommand { - enum Mode { - MODE_UNSPECIFIED = 0; - VERIFY_ONLY = 1; - CLEAR_SOFTWARE_LATCH = 2; - } - - message Request { - string recovery_id = 1; - SafetyScope scope = 2; - uint64 expected_safety_epoch = 3; - Mode mode = 4; - string reason = 5; - uint32 timeout_ms = 6; - } - - message Feedback { - CommandHeader.Feedback header = 1; - SafetyOperationResult result = 2; - uint64 previous_safety_epoch = 3; - uint64 current_safety_epoch = 4; - SystemAdmissionState system_state = 5; - repeated SafetyOperationTargetResult targets = 6; - string recovery_id = 7; - } -} diff --git a/protos/cmvr/api/system_command.proto b/protos/cmvr/api/system_command.proto index d99f623e..52b99221 100644 --- a/protos/cmvr/api/system_command.proto +++ b/protos/cmvr/api/system_command.proto @@ -1,9 +1,6 @@ syntax = "proto3"; -import "cmvr/api/agv_command.proto"; -import "cmvr/api/arm_command.proto"; import "cmvr/api/common.proto"; -import "cmvr/api/safety_command.proto"; package cmvr.api; @@ -24,77 +21,6 @@ message DeviceList { DeviceType device_type = 2; } -// Stable device categories used by GetDeviceList. This intentionally does not -// reuse the legacy DeviceType enum above: its zero value is AGV and it does not -// cover all DeviceManager categories. -enum SystemDeviceType { - SYSTEM_DEVICE_TYPE_UNSPECIFIED = 0; - SYSTEM_DEVICE_TYPE_AGV = 1; - SYSTEM_DEVICE_TYPE_ARM = 2; - SYSTEM_DEVICE_TYPE_BATTERY = 3; - SYSTEM_DEVICE_TYPE_BIO_HEAD = 4; - SYSTEM_DEVICE_TYPE_CAMERA = 5; - SYSTEM_DEVICE_TYPE_CAN_BUS = 6; - SYSTEM_DEVICE_TYPE_DEX_HAND = 7; - SYSTEM_DEVICE_TYPE_GRIPPER = 8; - SYSTEM_DEVICE_TYPE_MICROPHONE = 9; - SYSTEM_DEVICE_TYPE_MOTOR = 10; - SYSTEM_DEVICE_TYPE_MOTOR_SYSTEM = 11; - SYSTEM_DEVICE_TYPE_MUJOCO_VIEWER = 12; - SYSTEM_DEVICE_TYPE_MUJOCO_WORLD = 13; - SYSTEM_DEVICE_TYPE_ROBOT = 14; - SYSTEM_DEVICE_TYPE_SPEAKER = 15; -} - -enum SystemDeviceState { - SYSTEM_DEVICE_STATE_UNSPECIFIED = 0; - SYSTEM_DEVICE_STATE_DISABLED = 1; - SYSTEM_DEVICE_STATE_INITIALIZING = 2; - SYSTEM_DEVICE_STATE_REGISTERED = 3; - SYSTEM_DEVICE_STATE_READY = 4; - SYSTEM_DEVICE_STATE_RUNNING = 5; - SYSTEM_DEVICE_STATE_STOPPED = 6; - SYSTEM_DEVICE_STATE_ERROR = 7; -} - -enum SystemDeviceHealth { - SYSTEM_DEVICE_HEALTH_UNSPECIFIED = 0; - SYSTEM_DEVICE_HEALTH_HEALTHY = 1; - SYSTEM_DEVICE_HEALTH_DEGRADED = 2; - SYSTEM_DEVICE_HEALTH_FAULT = 3; -} - -message SystemDeviceInfo { - string device_id = 1; - SystemDeviceType device_type = 2; - - // Concrete backend name for display and diagnostics only. Consumers must - // use device_type, rather than this free-form string, for decisions. - string type_name = 3; - - // GetDeviceList currently publishes only enabled entries. Keep this field - // explicit so each row remains self-describing and future-compatible. - bool enabled = 4; - SystemDeviceState manager_state = 5; - SystemDeviceHealth health = 6; - bool has_error = 7; - string error_message = 8; - uint64 status_updated_at_unix_ms = 9; -} - -message GetDeviceListCommand { - message Request {} - - message Feedback { - CommandHeader.Feedback header = 1; - string manager_name = 2; - string manager_version = 3; - string manager_description = 4; - repeated SystemDeviceInfo device_list = 5; - uint64 sampled_at_unix_ms = 6; - } -} - message GetSystemInfoCommand { message Request {} @@ -106,20 +32,6 @@ message GetSystemInfoCommand { string os = 5; string kernel_version = 6; string architecture = 7; - - // Changes whenever the in-process ActionQueue idempotency ledger is - // recreated. Clients bind submissions and retries to this value. - string action_service_instance_id = 8; - - // Effective server-side control-plane settings. These fields describe - // what is running, not merely what the configuration requested. - string grpc_transport_security = 9; - string grpc_authentication = 10; - string grpc_recovery_exposure = 11; - bool grpc_insecure_non_loopback = 12; - string control_service_instance_id = 13; - string safety_enforcement_mode = 14; - uint32 safety_schema_version = 15; } } @@ -152,111 +64,9 @@ message UpdateParamsCommand { message StopAllCommand { message Request { CommandHeader.Request header = 1; - string operation_id = 2; - string expected_service_instance_id = 3; - uint32 timeout_ms = 4; } message Feedback { CommandHeader.Feedback header = 1; - string operation_id = 2; - uint64 previous_safety_epoch = 3; - uint64 current_safety_epoch = 4; - SystemAdmissionState system_state = 5; - repeated SafetyOperationTargetResult targets = 6; } -} - -// Final outcome of one ActionQueue execution. -enum ActionResultCode { - ACTION_RESULT_CODE_UNSPECIFIED = 0; - ACTION_RESULT_CODE_COMPLETED = 1; - ACTION_RESULT_CODE_FAILED = 2; - ACTION_RESULT_CODE_CANCELED = 3; - ACTION_RESULT_CODE_TIMED_OUT = 4; - ACTION_RESULT_CODE_REJECTED = 5; -} - -// Describes whether this RPC admitted a new action or observed an existing -// idempotency record. Clients must not infer this from an error string. -enum ActionDeduplicationStatus { - ACTION_DEDUPLICATION_STATUS_UNSPECIFIED = 0; - ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW = 1; - ACTION_DEDUPLICATION_STATUS_JOINED_IN_FLIGHT = 2; - ACTION_DEDUPLICATION_STATUS_CACHED_RESULT = 3; - ACTION_DEDUPLICATION_STATUS_RESULT_EVICTED = 4; - ACTION_DEDUPLICATION_STATUS_LEDGER_EXHAUSTED = 5; - ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT = 6; - ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH = 7; -} - -// Edge-local delay between two device commands. -message DelayAction { - // Delay duration in milliseconds. The server applies a bounded maximum. - uint32 duration_ms = 1; -} - -// One finite, synchronous command in an ActionQueue request. -message ActionStep { - // Client-provided identifier used for diagnostics. It must be unique within - // one ActionQueue request. - string step_id = 1; - - // Per-step timeout in milliseconds. Zero inherits the remaining action - // timeout or the server default. - uint32 timeout_ms = 2; - - // Tags are grouped by domain so compatible commands can be added without - // renumbering existing alternatives: Arm 10-19, AGV 20-29, built-ins 90+. - oneof command { - MoveJ.Request arm_move_j = 10; - MoveL.Request arm_move_l = 11; - - AgvNavigateToPoseCommand.Request agv_navigate_to_pose = 20; - AgvNavigateToStationCommand.Request agv_navigate_to_station = 21; - AgvFollowPathCommand.Request agv_follow_path = 22; - - DelayAction delay = 90; - } -} - -// Atomically submits a complete command sequence for edge-local serial -// execution. Device motion alternatives must use synchronous execution. -message ActionQueueCommand { - message Request { - // Client-generated globally unique idempotency key. During one Action - // service instance, retrying an identical accepted request with the - // same action_id does not dispatch its steps a second time. Recent - // terminal results can be returned; older accepted IDs are rejected - // fail-closed after their result is evicted. Deduplication is not - // persisted across an edge-service restart; the required instance - // epoch below prevents an old retry from being replayed after restart. - string action_id = 1; - repeated ActionStep steps = 2; - - // Total queue-wait plus execution timeout in milliseconds. Zero uses a - // bounded server default. - uint32 total_timeout_ms = 3; - - // Required instance epoch obtained from GetSystemInfo. A mismatch - // means the process-local deduplication ledger was recreated, so the - // server rejects the request instead of risking a replay. - string expected_service_instance_id = 4; - } - - message Feedback { - CommandHeader.Feedback header = 1; - string action_id = 2; - - // Number of steps which completed successfully before the final result. - uint32 completed_steps = 3; - - // Present only when a particular step caused failure, cancellation, - // timeout, or rejection. Presence distinguishes index zero from no - // failed step. - optional uint32 failed_step_index = 4; - ActionResultCode result = 5; - string service_instance_id = 6; - ActionDeduplicationStatus deduplication_status = 7; - } -} +} \ No newline at end of file diff --git a/protos/cmvr/api/system_service.proto b/protos/cmvr/api/system_service.proto index 44267733..84d1c9bd 100644 --- a/protos/cmvr/api/system_service.proto +++ b/protos/cmvr/api/system_service.proto @@ -1,7 +1,7 @@ syntax = "proto3"; +import "cmvr/api/common.proto"; import "cmvr/api/system_command.proto"; -import "cmvr/api/safety_command.proto"; package cmvr.api; @@ -9,14 +9,9 @@ package cmvr.api; service SystemService { rpc GetSystemInfo(GetSystemInfoCommand.Request) returns (GetSystemInfoCommand.Feedback) {} rpc GetSystemStatus(GetSystemStatusCommand.Request) returns (GetSystemStatusCommand.Feedback) {} - 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) {} - - rpc ExecuteActionQueue(ActionQueueCommand.Request) returns (ActionQueueCommand.Feedback) {} - - rpc GetSafetyState(GetSafetyStateCommand.Request) returns (GetSafetyStateCommand.Feedback) {} - rpc RecoverSafetyState(RecoverSafetyStateCommand.Request) returns (RecoverSafetyStateCommand.Feedback) {} } diff --git a/protos/cmvr/config/agv_config/agv_config.proto b/protos/cmvr/config/agv_config/agv_config.proto index 4e579b7f..c8ec8c56 100644 --- a/protos/cmvr/config/agv_config/agv_config.proto +++ b/protos/cmvr/config/agv_config/agv_config.proto @@ -11,11 +11,11 @@ message MyAgvConfig { int32 port = 3; } -// 仙工 SEER Robokit AGV 后端配置。 -message SeerRobokitAgvConfig { +// 仙工 SRC1100 AGV 后端配置。 +message Src1100AgvConfig { // 设备 id。为空时通常由外层 AGVDeviceConfig.id 补齐。 string id = 1; - // SEER Robokit 控制器 IP 地址。 + // SRC1100 控制器 IP 地址。 string ip = 2; // 是否启用该后端配置。当前设备是否创建仍以设备管理器配置为准。 bool enable = 3; @@ -49,8 +49,6 @@ message SeerRobokitAgvConfig { int32 map_update_interval_ms = 17; // 统一地图更新缓存条数。0 表示使用适配器默认值；缓存满后会丢弃最旧更新。 uint32 map_update_history_size = 18; - // 抢占 SEER Robokit 控制权时上报的稳定昵称。为空时适配器使用 "cmvr-es:"。 - string control_nick_name = 19; } // 单个 AGV 设备配置。 @@ -62,8 +60,8 @@ message AGVDeviceConfig { oneof backend { // 示例/测试 AGV 后端。 MyAgvConfig my_agv = 10; - // 仙工 SEER Robokit AGV 后端。 - SeerRobokitAgvConfig seer_robokit_agv = 11; + // 仙工 SRC1100 AGV 后端。 + Src1100AgvConfig src1100_agv = 11; } } diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto index b717e89d..06c0c33b 100644 --- a/protos/cmvr/config/arm_config/arm_config.proto +++ b/protos/cmvr/config/arm_config/arm_config.proto @@ -6,7 +6,6 @@ import "cmvr/config/pinocchio_dls_ik_config.proto"; import "cmvr/config/pinocchio_qp_ik_config.proto"; import "cmvr/config/srs_ik_config.proto"; import "cmvr/config/cartesian_motion_validation_config.proto"; -import "cmvr/config/motor_config/motor_config.proto"; enum ToppraPathType { TOPPRA_PATH_TYPE_UNKNOWN = 0; @@ -25,10 +24,6 @@ message MotorRobotArmBackendConfig { double default_vel = 7; double default_acc = 8; repeated string motor_group_ids = 9; - // Reserved opt-in for a future atomic/timed group position-servo primitive. - // MotorRobotArm's current sequential per-joint servoJ implementation - // deliberately rejects this capability even if this field is true. - bool enable_teleop_group_servo = 10; } enum VendorRobotArmBrand { @@ -48,60 +43,6 @@ message VendorRobotArmBackendConfig { string tool_frame = 8; string username = 9; string password = 10; - // AUBO only. Automatic energization after a physical E-stop is deliberately - // opt-in because releasing brakes changes the hardware energy state. - optional bool auto_power_on_after_hardware_estop_release = 11; -} - -enum DamiaoMotorModel { - DAMIAO_MOTOR_MODEL_UNKNOWN = 0; - DAMIAO_MOTOR_MODEL_DM4310 = 1; - DAMIAO_MOTOR_MODEL_DM4310_48V = 2; - DAMIAO_MOTOR_MODEL_DM4340 = 3; - DAMIAO_MOTOR_MODEL_DM4340_48V = 4; - DAMIAO_MOTOR_MODEL_DM6006 = 5; - DAMIAO_MOTOR_MODEL_DM8006 = 6; - DAMIAO_MOTOR_MODEL_DM8009 = 7; - DAMIAO_MOTOR_MODEL_DM10010L = 8; - DAMIAO_MOTOR_MODEL_DM10010 = 9; - DAMIAO_MOTOR_MODEL_DMH3510 = 10; - DAMIAO_MOTOR_MODEL_DMH6215 = 11; - DAMIAO_MOTOR_MODEL_DMG6220 = 12; -} - -message DamiaoJointConfig { - string joint_name = 1; - uint32 command_id = 2; - uint32 feedback_id = 3; - uint32 reported_motor_id = 4; - DamiaoMotorModel model = 5; - // Only +1 and -1 are accepted. - int32 direction = 6; - // q_joint = direction * q_motor + zero_offset_rad. - double zero_offset_rad = 7; - double joint_lower_rad = 8; - double joint_upper_rad = 9; - double max_velocity_rad_s = 10; - double max_torque_nm = 11; - // Explicit whitelist for the four-bit status nibble in MIT feedback. - // The code intentionally does not guess vendor/firmware meanings. At least - // one reviewed value is required before hardware_enabled may be true. - repeated uint32 healthy_feedback_status = 12; - // Raw byte thresholds, interpreted only as ordered protocol values. Nonzero - // reviewed limits are required before hardware_enabled may be true. - uint32 max_driver_temperature_raw = 13; - uint32 max_motor_temperature_raw = 14; -} - -message UmeRobotArmBackendConfig { - SocketCanConfig can = 1; - repeated DamiaoJointConfig joints = 2; - uint32 control_frequency_hz = 3; - uint32 cycle_deadline_us = 4; - uint32 feedback_watchdog_ms = 5; - uint32 shutdown_timeout_ms = 6; - // This is deliberately false in every checked-in configuration. - bool hardware_enabled = 7; } message SpeedLPlannerConfig { @@ -192,7 +133,6 @@ message RobotArmConfig { oneof backend { MotorRobotArmBackendConfig motor = 10; VendorRobotArmBackendConfig vendor = 11; - UmeRobotArmBackendConfig ume = 12; } ArmKinematicsConfig kinematics = 20; 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 ff60aac7..d36d328b 100644 --- a/protos/cmvr/config/device_manager_config/device_manager_config.proto +++ b/protos/cmvr/config/device_manager_config/device_manager_config.proto @@ -1,25 +1,6 @@ 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; @@ -46,10 +27,6 @@ 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 { @@ -58,7 +35,6 @@ 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 f054f223..0650c735 100644 --- a/protos/cmvr/config/grpc_server_config/grpc_server_config.proto +++ b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto @@ -1,68 +1,6 @@ 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; @@ -74,8 +12,6 @@ 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/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index b0dffd61..7c166df9 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -52,21 +52,6 @@ message EtherCATDcConfig { message SocketCanConfig { string dev_id = 1; int32 channel_id = 2; - // Explicit Linux interface name (for example can0 or vcan0). When empty, - // the legacy channel_id based naming remains in use. - optional string interface_name = 3; - // Allows CAN-FD frames on the raw socket. This does not configure the - // physical interface bitrate or bring the interface up. - optional bool enable_fd = 4; - // Default BRS flag for callers that explicitly construct an FD frame. - optional bool bitrate_switch = 5; - // Bounded receive wait. Zero selects the implementation safety default. - optional uint32 receive_timeout_us = 6; - optional bool receive_own_messages = 7; - optional bool enable_error_frames = 8; - // Total bounded wait for one send() batch. Zero selects the implementation - // safety default. UME profiles set this below their control-cycle deadline. - optional uint32 send_timeout_us = 9; } message EtherCATConfig { @@ -89,8 +74,6 @@ enum MotorBusType { MOTOR_BUS_CAN = 1; MOTOR_BUS_ETHERCAT = 2; MOTOR_BUS_MUJOCO = 3; - reserved 4; - reserved "MOTOR_BUS_MODBUS_TCP"; } enum MotorVendor { @@ -98,8 +81,6 @@ enum MotorVendor { MOTOR_VENDOR_TI5 = 1; MOTOR_VENDOR_MUJOCO = 2; MOTOR_VENDOR_EYOU = 3; - reserved 4; - reserved "MOTOR_VENDOR_PLC_GENERIC"; } enum MotorProtocol { @@ -107,14 +88,9 @@ enum MotorProtocol { MOTOR_PROTOCOL_CANOPEN = 1; MOTOR_PROTOCOL_ETHERCAT_CIA402 = 2; MOTOR_PROTOCOL_MUJOCO = 3; - reserved 4; - reserved "MOTOR_PROTOCOL_CMVR_PLC_V1"; } message MotorGroupConfig { - reserved 13; - reserved "modbus_tcp"; - string id = 1; MotorBusType bus_type = 2; MotorVendor vendor = 3; 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 cf6479e5..8c60e370 100644 --- a/protos/cmvr/config/task_manager_config/task_manager_config.proto +++ b/protos/cmvr/config/task_manager_config/task_manager_config.proto @@ -8,7 +8,6 @@ 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/protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto b/protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto deleted file mode 100644 index fd2a3366..00000000 --- a/protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto +++ /dev/null @@ -1,30 +0,0 @@ -syntax = "proto3"; - -package cmvr.config; - -import "cmvr/api/arm_teleop_v1.proto"; - -message UmeTeleopReconnectConfig { - uint32 initial_delay_ms = 1; - uint32 maximum_delay_ms = 2; - double multiplier = 3; -} - -// Transport/session configuration only. Robot kinematics, SEW, FK and IK stay -// in UME and publish already-computed joint setpoints through the client API. -message UmeTeleopConfig { - string id = 1; - string server_address = 2; - - // M6 deliberately supports only an explicitly opted-in insecure channel. - // Keep the task disabled until the endpoint and deployment security policy - // are configured. TLS credentials can be added without changing the v1 API. - bool allow_insecure = 3; - - cmvr.api.armteleop.v1.OpenSession open_session = 4; - UmeTeleopReconnectConfig reconnect = 5; -} - -message UmeTeleopRootConfig { - UmeTeleopConfig ume_teleop = 1; -} diff --git a/protos/cmvr/api/agv_utils.proto b/protos/cmvr/msgs/agv.proto similarity index 88% rename from protos/cmvr/api/agv_utils.proto rename to protos/cmvr/msgs/agv.proto index c05f1a79..54439eeb 100644 --- a/protos/cmvr/api/agv_utils.proto +++ b/protos/cmvr/msgs/agv.proto @@ -1,7 +1,5 @@ syntax = "proto3"; -// 文件随 AGV API 定义统一放在 cmvr/api 下，但保留 cmvr.msgs package， -// 以兼容既有生成代码、消息完整名称和 Any type URL。 package cmvr.msgs; // AGV 在地图平面坐标系中的二维位姿。 @@ -14,29 +12,6 @@ message AgvPose2d { double theta = 3; } -// 固定距离平移使用的距离参考模式。 -enum AgvTranslationMode { - // 根据底盘里程计算运动距离。 - AGV_TRANSLATION_MODE_ODOMETRY = 0; - // 根据定位结果计算运动距离。 - AGV_TRANSLATION_MODE_LOCALIZATION = 1; -} - - - -// AGV 车体坐标系下的固定距离平移参数。 -message AgvTranslation { - // 平移距离的绝对值，单位：米，必须大于 0。 - double distance = 1; - // 车体 X 方向速度，单位：米/秒；正为向前，负为向后。 - double vx = 2; - // 车体 Y 方向速度，单位：米/秒；正为向左，负为向右。 - double vy = 3; - // 距离参考模式；默认使用里程模式。 - AgvTranslationMode mode = 4; -} - - // AGV 车体坐标系下的平面速度。 message AgvVelocity { // 车体 X 方向线速度，单位：米/秒。 @@ -47,9 +22,6 @@ message AgvVelocity { double wz = 3; } - - - // AGV 电池状态。 message AgvBatteryState { // 电量比例，范围：[0, 1]，例如 0.8 表示 80%。 @@ -80,14 +52,8 @@ message AgvMotionOptions { double reach_angle = 6; // 速度比例，范围通常为 [0, 1]；1 表示不降速。 double speed_ratio = 7; - // 是否异步执行；false（默认）表示到达、失败、取消或遇障停止后才返回， - // true 表示任务被控制器接受后立即返回。 + // 是否异步执行；true 表示下发任务后立即返回。 bool asynchronous = 8; - // 同步导航的最大等待时间，单位：毫秒；0 表示使用适配器默认值。 - // gRPC deadline 应大于该值或预计行程时间，否则服务端会安全取消导航。 - int32 wait_timeout_ms = 9; - // 同步导航的状态轮询周期，单位：毫秒；0 表示使用适配器默认值。 - int32 poll_interval_ms = 10; } // AGV 适配器扩展参数。用于传递厂商或控制器特有的参数。 diff --git a/protos/rbk/protocol/seer_robokit_map3d.proto b/protos/rbk/protocol/src1100_map3d.proto similarity index 97% rename from protos/rbk/protocol/seer_robokit_map3d.proto rename to protos/rbk/protocol/src1100_map3d.proto index 4936722f..b33474f1 100644 --- a/protos/rbk/protocol/seer_robokit_map3d.proto +++ b/protos/rbk/protocol/src1100_map3d.proto @@ -2,7 +2,7 @@ syntax = "proto3"; package rbk.protocol; -// 仙工 SEER Robokit 3D 地图文件 0.3dsmap 的最小解析结构。 +// 仙工 SRC1100 3D 地图文件 0.3dsmap 的最小解析结构。 // 这里只保留转换统一地图所需字段，未声明字段由 protobuf 作为未知字段跳过。 // 地图坐标系下的三维位置，单位：米。 diff --git a/test/e2e/CMakeLists.txt b/test/e2e/CMakeLists.txt index 15b4b148..88649bd6 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_manager) + NOT TARGET cmvr_es::media_source_hub) message(FATAL_ERROR - "Real QUIC E2E requires the production QUIC service and MediaSourceManager") + "Real QUIC E2E requires the production QUIC service and MediaSourceHub") 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_manager + cmvr_es::media_source_hub ) if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") diff --git a/test/e2e/README.md b/test/e2e/README.md index e88e8e84..e0e4e7ba 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 安全退出 | -第一项向生产 `MediaSourceManager` 注册两个有界 synthetic source： +第一项向生产 `MediaSourceHub` 注册两个有界 synthetic source： - 2500 字节的 H.264 Annex B IDR 视频帧，用于覆盖 DATAGRAM 分片； - 带 ADTS header 的 AAC-LC 48 kHz 双声道音频帧。