diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index 0ff7c443..7ef09113 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -6,11 +6,14 @@ add_subdirectory(hardware) add_subdirectory(algorithms) add_subdirectory(simulate) add_subdirectory(devices) +add_subdirectory(manager/control_authority) add_subdirectory(manager/device_manager) add_subdirectory(manager/media_source_hub) add_subdirectory(service/quic_edge) +add_subdirectory(service/arm_teleop_client) add_subdirectory(task) add_subdirectory(task/quic_edge_task) +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 1921842a..4cc76775 100644 --- a/cmvr-es/algorithms/controllers/CMakeLists.txt +++ b/cmvr-es/algorithms/controllers/CMakeLists.txt @@ -1,5 +1,6 @@ add_subdirectory(arm_control) +add_subdirectory(ume_legacy) #find_package(VISP REQUIRED) diff --git a/cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt b/cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt new file mode 100644 index 00000000..73bc703c --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt @@ -0,0 +1,78 @@ +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 new file mode 100644 index 00000000..720e0f5c --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h @@ -0,0 +1,72 @@ +#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 new file mode 100644 index 00000000..4c706704 --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h @@ -0,0 +1,34 @@ +#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 new file mode 100644 index 00000000..68a6d0a6 --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h @@ -0,0 +1,32 @@ +#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 new file mode 100644 index 00000000..fc7dc211 --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h @@ -0,0 +1,110 @@ +#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 new file mode 100644 index 00000000..f298ef3c --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp @@ -0,0 +1,585 @@ +#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 new file mode 100644 index 00000000..f3192328 --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp @@ -0,0 +1,193 @@ +#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 new file mode 100644 index 00000000..568ff28e --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp @@ -0,0 +1,392 @@ +#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 new file mode 100644 index 00000000..b5610c20 --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp @@ -0,0 +1,216 @@ +#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/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 567b503c..9d8c819b 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -103,6 +103,11 @@ 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 { @@ -165,6 +170,14 @@ 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, @@ -197,6 +210,14 @@ 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 c1414757..9fb2f0bc 100644 --- a/cmvr-es/config/README.md +++ b/cmvr-es/config/README.md @@ -18,6 +18,8 @@ cmvr_es.pb.txt 入口文件： - [`cmvr_es.pb.txt`](cmvr_es.pb.txt) +- [`cmvr_es_ume.pb.txt`](cmvr_es_ume.pb.txt)：UME 主端样例 +- [`cmvr_es_robot.pb.txt`](cmvr_es_robot.pb.txt)：人形机械臂从端样例 - [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) - [`manager/task_manager.pb.txt`](manager/task_manager.pb.txt) @@ -29,10 +31,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`，不会自动跳到安装目录。传入显式根配置时： +安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时使用 `--config`： ```bash -./output/bin/cmvr_es /etc/cmvr-es/cmvr_es.pb.txt +./output/bin/cmvr_es --config /etc/cmvr-es/cmvr_es.pb.txt ``` 设备、任务和证书等相对配置路径均以根配置文件所在目录解析。模型等资源通过 `ConfigHelper::resolveResourceFile()` 在配置根及父目录中查找；生产部署仍建议使用明确绝对路径。 @@ -109,6 +111,48 @@ output/bin/protoc \ 该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。 +## 双边遥操部署样例 + +仓库提供两个相互独立的 CMVR-ES 配置入口： + +- UME 主端：[`cmvr_es_ume.pb.txt`](cmvr_es_ume.pb.txt)，只声明 + `ume_left` 和 `ume_right`。两条机械臂在 DeviceManager 层默认关闭， + [`devices/arm/ume_arms.pb.txt`](devices/arm/ume_arms.pb.txt) 内部的 + `hardware_enabled` 也默认关闭；两层开关必须经过标定与安全验收后分别启用。 + 出站 `ume_teleop` Task 默认关闭，样例不包含机器人地址或凭据。当前 Task + 只实现会话/重连/心跳和 latest-only 指令邮箱，尚无生产算法调用 + `submitSetpoint()`，返回 effort 也尚未接入本地触觉协调器。 +- 人形机械臂从端：[`cmvr_es_robot.pb.txt`](cmvr_es_robot.pb.txt)，通用 gRPC + server 可以启动，但 `ti5_motors` 和 `right_arm` 仍默认关闭。生产 + `ArmTeleop` 已实现真实 `RobotArm` 适配，但服务配置的 `enable` 显式关闭且 + 样例哈希故意留空。`MotorRobotArm.enable_teleop_group_servo` 目前只是预留字段； + 因为现有 `servoJ` 仍是逐关节顺序写，代码即使看到该字段为 true 也会拒绝能力。 + 必须先实现并验收原子或定时的组下发原语。因此启动 gRPC server 不等于允许遥操 + 执行，也不能绕过设备层硬件门。 + +在两台边缘设备各自的源码或安装目录运行： + +```bash +# UME 主端（源码配置） +./output/bin/cmvr_es \ + --config ./cmvr-es/config/cmvr_es_ume.pb.txt + +# 人形机械臂从端（源码配置） +./output/bin/cmvr_es \ + --config ./cmvr-es/config/cmvr_es_robot.pb.txt +``` + +如果使用安装后的配置副本，则相应命令为： + +```bash +./output/bin/cmvr_es --config ./output/bin/config/cmvr_es_ume.pb.txt +./output/bin/cmvr_es --config ./output/bin/config/cmvr_es_robot.pb.txt +``` + +上线前应把两套配置分别复制到两台机器的外部配置目录。主端需要填写从端地址、 +会话 manifest 和认证配置；从端需要换成现场机械臂设备配置，并在真实硬件测试后 +逐层开启。不要把生产 IP、token、私钥或设备标定值提交到仓库样例。 + ## 生产配置 `cmake --install` 会重建 `output/bin/config/`。生产配置应复制到 `/etc/cmvr-es/` 等外部目录并显式传入。 diff --git a/cmvr-es/config/cmvr_es_robot.pb.txt b/cmvr-es/config/cmvr_es_robot.pb.txt new file mode 100644 index 00000000..44f6f00f --- /dev/null +++ b/cmvr-es/config/cmvr_es_robot.pb.txt @@ -0,0 +1,11 @@ +# Follower robot-side CMVR-ES profile. +# +# The RobotArm ArmTeleop backend is implemented but explicitly disabled. The +# physical arm and service backend remain closed. Current MotorRobotArm +# sequential joint writes are rejected as a teleop group-servo capability until +# an atomic/timed group primitive and its safety timing gates are accepted. +cmvr_es { + logger_config_file: "logger/logger.pb.txt" + device_manager_config_file: "manager/device_manager_robot.pb.txt" + task_manager_config_file: "manager/task_manager_robot.pb.txt" +} diff --git a/cmvr-es/config/cmvr_es_ume.pb.txt b/cmvr-es/config/cmvr_es_ume.pb.txt new file mode 100644 index 00000000..e20ef540 --- /dev/null +++ b/cmvr-es/config/cmvr_es_ume.pb.txt @@ -0,0 +1,10 @@ +# UME leader-side CMVR-ES profile. +# +# All relative paths below are resolved from this file's directory. This +# checked-in profile contains no production endpoint, credentials or hardware +# enablement. +cmvr_es { + logger_config_file: "logger/logger.pb.txt" + device_manager_config_file: "manager/device_manager_ume.pb.txt" + task_manager_config_file: "manager/task_manager_ume.pb.txt" +} diff --git a/cmvr-es/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt index 714c5c9b..c1570e7f 100644 --- a/cmvr-es/config/devices/arm/arm.pb.txt +++ b/cmvr-es/config/devices/arm/arm.pb.txt @@ -17,6 +17,10 @@ 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 { diff --git a/cmvr-es/config/devices/arm/ume_arms.pb.txt b/cmvr-es/config/devices/arm/ume_arms.pb.txt new file mode 100644 index 00000000..e39f94f4 --- /dev/null +++ b/cmvr-es/config/devices/arm/ume_arms.pb.txt @@ -0,0 +1,72 @@ +# 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/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 5b85ffe4..de5de1da 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -111,6 +111,23 @@ 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 diff --git a/cmvr-es/config/manager/device_manager_robot.pb.txt b/cmvr-es/config/manager/device_manager_robot.pb.txt new file mode 100644 index 00000000..efc4a383 --- /dev/null +++ b/cmvr-es/config/manager/device_manager_robot.pb.txt @@ -0,0 +1,24 @@ +# Follower robot-side devices. +device_manager { + name: "cmvr_es_robot" + version: "0.1" + description: "CMVR humanoid follower edge system" + init_all_motors_when_no_active_joints: false + + devices { + id: "ti5_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/ti5_motors.pb.txt" + # Physical motor communication remains fail-closed in this example. + enable: false + } + + devices { + id: "right_arm" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/arm.pb.txt" + # Do not enable until the motor system, URDF, limits, servoJ timing and + # independent emergency-stop path have passed the robot safety checkout. + enable: false + } +} diff --git a/cmvr-es/config/manager/device_manager_ume.pb.txt b/cmvr-es/config/manager/device_manager_ume.pb.txt new file mode 100644 index 00000000..c908e447 --- /dev/null +++ b/cmvr-es/config/manager/device_manager_ume.pb.txt @@ -0,0 +1,24 @@ +# UME leader-side devices only. +device_manager { + name: "cmvr_es_ume" + version: "0.1" + description: "UME leader edge system" + init_all_motors_when_no_active_joints: false + + devices { + id: "ume_right" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/ume_arms.pb.txt" + # Hardware gate 1/2. Gate 2/2 is ume.hardware_enabled in the arm config. + # Keep both false until CAN mapping, limits and physical safety are verified. + enable: false + } + + devices { + id: "ume_left" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/ume_arms.pb.txt" + # Hardware gate 1/2. Gate 2/2 is ume.hardware_enabled in the arm config. + enable: false + } +} diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index d6569ea6..4d5df7a7 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -30,4 +30,13 @@ task_manager { # Host-development default: no QUIC Gateway or physical media devices. enable: true } + 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/manager/task_manager_robot.pb.txt b/cmvr-es/config/manager/task_manager_robot.pb.txt new file mode 100644 index 00000000..dc53850a --- /dev/null +++ b/cmvr-es/config/manager/task_manager_robot.pb.txt @@ -0,0 +1,14 @@ +# Follower robot-side tasks. +task_manager { + tasks { + id: "grpc_server" + type: TASK_TYPE_GRPC_SERVER + run_mode: TASK_RUN_MODE_BLOCKING_SERVICE + config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt" + # The generic gRPC server may be enabled for integration. This does not + # enable a physical arm: device entries and the implemented RobotArm + # ArmTeleop adapter are explicitly disabled. Current MotorRobotArm + # sequential joint dispatch also fails the group-servo capability gate. + enable: true + } +} diff --git a/cmvr-es/config/manager/task_manager_ume.pb.txt b/cmvr-es/config/manager/task_manager_ume.pb.txt new file mode 100644 index 00000000..0793bb7d --- /dev/null +++ b/cmvr-es/config/manager/task_manager_ume.pb.txt @@ -0,0 +1,12 @@ +# UME leader-side tasks. +task_manager { + 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-closed: configure the follower endpoint, expected manifest and + # transport security before enabling outbound teleoperation. + 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 9001afb0..e351f03c 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,4 +5,22 @@ grpc_server { enable_reflection: true camera_stream_max_pending_frames: 2 camera_stream_max_frame_age_ms: 250 + + # 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 new file mode 100644 index 00000000..00ac7137 --- /dev/null +++ b/cmvr-es/config/tasks/ume_teleop_task/ume_teleop_task.pb.txt @@ -0,0 +1,35 @@ +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/arm/CMakeLists.txt b/cmvr-es/devices/arm/CMakeLists.txt index d2780c71..74f23c89 100644 --- a/cmvr-es/devices/arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/CMakeLists.txt @@ -1,6 +1,7 @@ add_subdirectory(motor_robot_arm) add_subdirectory(aubo_arm) add_subdirectory(huayan_arm) +add_subdirectory(ume_robot_arm) add_library(robot_arm INTERFACE) @@ -11,6 +12,7 @@ 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/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index 35471e29..5b1d0b9d 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,6 +36,13 @@ 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; @@ -125,6 +132,8 @@ private: mutable std::mutex mutex_; std::atomic busy_{false}; + std::atomic powered_on_{false}; + mutable std::atomic joint_state_sequence_{0}; double speed_scaling_{1.0}; bool emergency_stopped_{false}; ServoOptions servo_options_; 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 5a5db3a3..cce28a7d 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 @@ -1,6 +1,7 @@ #include "arm/motor_robot_arm/include/motor_robot_arm.h" #include +#include #include #include #include @@ -28,6 +29,23 @@ struct BusyGuard { ~BusyGuard() { busy.store(false); } }; +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) @@ -66,6 +84,39 @@ 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() @@ -134,7 +185,7 @@ ArmState MotorRobotArm::getRobotState() const { ArmState state; state.connected = motor_manager_ != nullptr; - state.powered_on = true; + state.powered_on = powered_on_.load(std::memory_order_acquire); state.brake_released = !emergency_stopped_; state.moving = busy(); state.emergency_stopped = emergency_stopped_; @@ -154,15 +205,35 @@ 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; } - state.position.push_back(motor->getQ()); - state.velocity.push_back(motor->getQd()); + 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.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; } @@ -202,11 +273,15 @@ Result MotorRobotArm::torqueOn() } } emergency_stopped_ = 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) { diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 6ab24556..071f3ce9 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -28,6 +28,15 @@ public: virtual SafetyMode getSafetyMode() const = 0; virtual ControlMode getControlMode() const = 0; + // 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; virtual Result torqueOff() = 0; virtual Result calibrateZeroQ(const std::string& joint_name) = 0; @@ -73,6 +82,27 @@ 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 0a23742c..e86598e2 100644 --- a/cmvr-es/devices/arm/robot_arm_factory.h +++ b/cmvr-es/devices/arm/robot_arm_factory.h @@ -9,6 +9,7 @@ #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 { @@ -36,6 +37,9 @@ 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 new file mode 100644 index 00000000..f92b6ecd --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt @@ -0,0 +1,92 @@ +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 new file mode 100644 index 00000000..32bfd0e7 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h @@ -0,0 +1,145 @@ +#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 new file mode 100644 index 00000000..447473c7 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h @@ -0,0 +1,135 @@ +#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 new file mode 100644 index 00000000..849277f4 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/include/ume_robot_arm.h @@ -0,0 +1,181 @@ +#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 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 new file mode 100644 index 00000000..2c4830e3 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp @@ -0,0 +1,705 @@ +#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 new file mode 100644 index 00000000..58e40e69 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp @@ -0,0 +1,254 @@ +#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 new file mode 100644 index 00000000..5ba7a926 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/src/ume_robot_arm.cpp @@ -0,0 +1,1035 @@ +#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 new file mode 100644 index 00000000..782ce46c --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp @@ -0,0 +1,407 @@ +#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 new file mode 100644 index 00000000..5a3bcef1 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp @@ -0,0 +1,150 @@ +#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 new file mode 100644 index 00000000..e7241ebc --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp @@ -0,0 +1,338 @@ +#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/canbus/CMakeLists.txt b/cmvr-es/devices/canbus/CMakeLists.txt index 76b2a460..0ce2cd40 100644 --- a/cmvr-es/devices/canbus/CMakeLists.txt +++ b/cmvr-es/devices/canbus/CMakeLists.txt @@ -31,6 +31,21 @@ target_link_libraries(socket_can_client_raw_test glog cmvr_es::proto ) +add_test( + NAME socket_can_client_raw_test + COMMAND socket_can_client_raw_test +) +set(_socket_can_client_raw_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" +) +if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _socket_can_client_raw_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") +endif() +set_tests_properties(socket_can_client_raw_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_socket_can_client_raw_test_environment}" +) add_executable(protocol_data_test @@ -93,4 +108,3 @@ target_link_libraries(can_receiver_test glog cmvr_es::proto ) - diff --git a/cmvr-es/devices/canbus/abstract_canbus.h b/cmvr-es/devices/canbus/abstract_canbus.h index b8649125..dc683f4b 100644 --- a/cmvr-es/devices/canbus/abstract_canbus.h +++ b/cmvr-es/devices/canbus/abstract_canbus.h @@ -3,6 +3,14 @@ // #pragma once +#include +#include +#include +#include +#include +#include +#include + #include "../abstract_device.h" #include "cmvr/msgs/error_code.pb.h" #include "canbus/common/byte.h" @@ -14,20 +22,26 @@ namespace cmvr::device { */ struct CanFrame { /// Message id - uint32_t id; + uint32_t id{0}; /// Message length - uint8_t len; - /// Message content - uint8_t data[8]; - /// Time stamp - struct timeval timestamp; + uint8_t len{0}; + /// Message content. Classic CAN uses at most the first 8 bytes. + uint8_t data[64]{}; + bool is_extended_id{false}; + bool is_remote_frame{false}; + bool is_error_frame{false}; + bool is_fd{false}; + bool bitrate_switch{false}; + bool error_state_indicator{false}; + /// Local host receive time used for freshness and watchdog checks. + int64_t rx_monotonic_ns{0}; + /// Legacy wall-clock field retained for source compatibility. + struct timeval timestamp{0, 0}; /** * @brief Constructor */ - CanFrame() : id(0), len(0), timestamp{0} { - std::memset(data, 0, sizeof(data)); - } + CanFrame() = default; /** * @brief CanFrame string including essential information about the message. @@ -37,10 +51,15 @@ namespace cmvr::device { std::stringstream output_stream(""); output_stream << "id:0x" << Byte::byte_to_hex(id) << ",len:" << static_cast(len) << ",data:"; - for (uint8_t i = 0; i < len; ++i) { + const auto printable_len = + std::min(len, sizeof(data)); + for (std::size_t i = 0; i < printable_len; ++i) { output_stream << Byte::byte_to_hex(data[i]); } - output_stream << ","; + output_stream << ",fd:" << is_fd + << ",brs:" << bitrate_switch + << ",extended:" << is_extended_id + << ",error:" << is_error_frame << ","; return output_stream.str(); } }; @@ -67,6 +86,28 @@ namespace cmvr::device { virtual cmvr::msgs::ErrorCode send(const std::vector &frames, int32_t *const frame_num) = 0; + /** + * @brief Send messages without starting a batch after an absolute + * local deadline. + * + * Deadline-aware transports should override this method so their + * internal blocking budget is also capped by @p deadline. The default + * preserves source compatibility and at least rejects an already + * expired request before calling send(). + */ + virtual cmvr::msgs::ErrorCode sendUntil( + const std::vector& frames, + int32_t* const frame_num, + const std::chrono::steady_clock::time_point deadline) { + if (std::chrono::steady_clock::now() >= deadline) { + if (frame_num) { + *frame_num = 0; + } + return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + return send(frames, frame_num); + } + /** * @brief Send a single message. * @param frames A single-element vector containing only one message. @@ -75,7 +116,9 @@ namespace cmvr::device { virtual cmvr::msgs::ErrorCode sendSingleFrame( const std::vector &frames) { if (frames.size() != 1U) { - CMVR_LOG(FATAL) << "frames size not equal to 1, actual frame size: " << frames.size(); + CMVR_LOG(ERROR) << "frames size not equal to 1, actual frame size: " + << frames.size(); + return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } int32_t n = 1; return send(frames, &n); @@ -91,6 +134,17 @@ namespace cmvr::device { virtual cmvr::msgs::ErrorCode receive(std::vector *const frames, int32_t *const frame_num) = 0; + /** + * @brief Discard frames already queued by the transport. + * + * Command/response protocols without a sequence field can use this + * immediately before sending a new request to reduce the risk that a + * response from an older cycle is accepted as fresh. Implementations + * must keep this call bounded. The conservative default reports that + * the transport cannot provide this guarantee. + */ + virtual bool discardPendingFrames() { return false; } + /** * @brief Get the error string. * @param status The status to get the error string. diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc index 21d9340e..61b7157f 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc @@ -12,9 +12,13 @@ #include "socket_can_client_raw.h" #include "absl/strings/str_cat.h" +#include +#include +#include +#include + namespace cmvr { namespace device { -#define CAN_ID_MASK 0x1FFFF800U // can_filter mask #define CAN_STANDARD_MAX_ID 0x7FFU using cmvr::msgs::ErrorCode; @@ -24,8 +28,25 @@ namespace cmvr { auto channel_id = cfg.channel_id(); port_ = static_cast(channel_id); interface_ = CANCardParameter::NATIVE; - - enable_can_err_check_ = false; + interface_name_ = + cfg.has_interface_name() && !cfg.interface_name().empty() + ? cfg.interface_name() + : cfg.dev_id(); + enable_fd_ = cfg.has_enable_fd() && cfg.enable_fd(); + default_bitrate_switch_ = + cfg.has_bitrate_switch() && cfg.bitrate_switch(); + receive_own_messages_ = + cfg.has_receive_own_messages() && cfg.receive_own_messages(); + receive_timeout_us_ = + cfg.has_receive_timeout_us() && cfg.receive_timeout_us() > 0 + ? cfg.receive_timeout_us() + : 100000U; + send_timeout_us_ = + cfg.has_send_timeout_us() && cfg.send_timeout_us() > 0 + ? cfg.send_timeout_us() + : 100000U; + enable_can_err_check_ = + cfg.has_enable_error_frames() && cfg.enable_error_frames(); } @@ -49,7 +70,7 @@ namespace cmvr { } SocketCanClientRaw::~SocketCanClientRaw() { - if (dev_handler_) { + if (dev_handler_ >= 0) { stop(); } } @@ -59,8 +80,8 @@ namespace cmvr { status_ = ErrorCode::OK; return true; } - struct sockaddr_can addr; - struct ifreq ifr; + struct sockaddr_can addr {}; + struct ifreq ifr {}; // open device // guss net is the device minor number, if one card is 0,1 @@ -91,17 +112,71 @@ namespace cmvr { if (ret < 0) { CMVR_LOG(ERROR) << "add receive msg id filter error code: " << ret; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } } - // 2. enable reception of can frames. - int enable = 1; - ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable, - sizeof(enable)); - if (ret < 0) { - CMVR_LOG(ERROR) << "enable reception of can frame error code: " << ret; + // 2. Explicitly opt into CAN-FD only when configured. This socket + // option does not configure the physical link bitrate or state. + if (enable_fd_) { + int enable = 1; + ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, + CAN_RAW_FD_FRAMES, &enable, sizeof(enable)); + if (ret < 0) { + CMVR_LOG(ERROR) << "enable CAN-FD frames failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + } + + const int receive_own = receive_own_messages_ ? 1 : 0; + if (::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, + &receive_own, sizeof(receive_own)) < 0) { + CMVR_LOG(ERROR) << "configure receive-own-messages failed: " + << std::strerror(errno); status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + + if (enable_can_err_check_) { + const can_err_mask_t error_mask = CAN_ERR_MASK; + if (::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, + &error_mask, sizeof(error_mask)) < 0) { + CMVR_LOG(ERROR) << "configure CAN error filter failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + } + + struct timeval receive_timeout { + static_cast(receive_timeout_us_ / 1000000U), + static_cast(receive_timeout_us_ % 1000000U) + }; + if (::setsockopt(dev_handler_, SOL_SOCKET, SO_RCVTIMEO, + &receive_timeout, sizeof(receive_timeout)) < 0) { + CMVR_LOG(ERROR) << "configure CAN receive timeout failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + + struct timeval send_timeout { + static_cast(send_timeout_us_ / 1000000U), + static_cast(send_timeout_us_ % 1000000U) + }; + if (::setsockopt(dev_handler_, SOL_SOCKET, SO_SNDTIMEO, + &send_timeout, sizeof(send_timeout)) < 0) { + CMVR_LOG(ERROR) << "configure CAN send timeout failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } @@ -115,13 +190,39 @@ namespace cmvr { interface_prefix = "can"; } - const std::string can_name = absl::StrCat(interface_prefix, port_); - std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ); - if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) { - CMVR_LOG(ERROR) << "ioctl error"; + const std::string can_name = + interface_name_.empty() + ? absl::StrCat(interface_prefix, port_) + : interface_name_; + if (can_name.size() >= IFNAMSIZ) { + CMVR_LOG(ERROR) << "CAN interface name is too long: " << can_name; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } + std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) { + CMVR_LOG(ERROR) << "CAN interface not found: " << can_name + << ", error=" << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + + if (enable_fd_) { + struct ifreq mtu_request {}; + std::strncpy(mtu_request.ifr_name, can_name.c_str(), IFNAMSIZ); + mtu_request.ifr_name[IFNAMSIZ - 1] = '\0'; + if (::ioctl(dev_handler_, SIOCGIFMTU, &mtu_request) < 0 || + mtu_request.ifr_mtu != CANFD_MTU) { + CMVR_LOG(ERROR) << "CAN-FD requested but interface MTU is not CANFD_MTU: " + << can_name; + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + } // bind socket to network interface @@ -131,8 +232,10 @@ namespace cmvr { sizeof(addr)); if (ret < 0) { - CMVR_LOG(ERROR) << "bind socket to network interface error code: " << ret; + CMVR_LOG(ERROR) << "bind socket to CAN interface failed: " + << std::strerror(errno); status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } @@ -142,10 +245,11 @@ namespace cmvr { } bool SocketCanClientRaw::stop() { - if (is_started_) { - is_started_ = false; - - int ret = close(dev_handler_); + is_started_ = false; + if (dev_handler_ >= 0) { + const int fd = dev_handler_; + dev_handler_ = -1; + int ret = close(fd); if (ret < 0) { CMVR_LOG(ERROR) << "close error code:" << ret << ", " << getErrorString(ret); return false; @@ -159,48 +263,190 @@ namespace cmvr { // Synchronous transmission of CAN messages ErrorCode SocketCanClientRaw::send(const std::vector &frames, int32_t *const frame_num) { + return sendWithDeadline_( + frames, frame_num, + std::chrono::steady_clock::now() + + std::chrono::microseconds(send_timeout_us_)); + } + + ErrorCode SocketCanClientRaw::sendUntil( + const std::vector& frames, + int32_t* const frame_num, + const std::chrono::steady_clock::time_point deadline) { + return sendWithDeadline_( + frames, frame_num, + std::min( + deadline, + std::chrono::steady_clock::now() + + std::chrono::microseconds(send_timeout_us_))); + } + + ErrorCode SocketCanClientRaw::sendWithDeadline_( + const std::vector& frames, + int32_t* const frame_num, + const std::chrono::steady_clock::time_point send_deadline) { if (frame_num == nullptr) { - CMVR_LOG(FATAL) << "frame_num is null"; + CMVR_LOG(ERROR) << "frame_num is null"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } - if (frames.size() != static_cast(*frame_num)) { - CMVR_LOG(FATAL) << "frames size does not match frame_num"; + if (*frame_num < 0 || + frames.size() != static_cast(*frame_num) || + frames.size() > static_cast(MAX_CAN_SEND_FRAME_LEN)) { + CMVR_LOG(ERROR) << "frames size does not match a valid frame_num"; + return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM; } if (!is_started_) { CMVR_LOG(ERROR) << "Nvidia can client has not been initiated! Please init first!"; return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } - for (size_t i = 0; i < frames.size() && i < MAX_CAN_SEND_FRAME_LEN; ++i) { - if (frames[i].len > CANBUS_MESSAGE_LENGTH || frames[i].len < 0) { - CMVR_LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len - << ", which is not equal to can message data length (" - << CANBUS_MESSAGE_LENGTH << ")."; - return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; - } - if (frames[i].id > CAN_STANDARD_MAX_ID) { - send_frames_[i].can_id = (frames[i].id & CAN_EFF_MASK) | CAN_EFF_FLAG; - } else { - send_frames_[i].can_id = (frames[i].id & CAN_SFF_MASK); - } - // CMVR_LOG(INFO) << "send can id is " << send_frames_[i].can_id; - send_frames_[i].can_dlc = frames[i].len; - std::memcpy(send_frames_[i].data, frames[i].data, frames[i].len); + if (std::chrono::steady_clock::now() >= send_deadline) { + *frame_num = 0; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } - // Synchronous transmission of CAN messages - int ret = static_cast( - write(dev_handler_, &send_frames_[i], sizeof(send_frames_[i]))); - if (ret <= 0) { - CMVR_LOG(ERROR) << "can " << port_ << " send message failed, error code: " << ret; - return ErrorCode::CAN_CLIENT_ERROR_BASE; + // Validate the complete batch before committing its first frame. + // This prevents a malformed later element from causing a valid + // prefix of a cyclic command batch to reach the bus. + for (size_t i = 0; i < frames.size(); ++i) { + const auto& source = frames[i]; + const auto max_length = + source.is_fd ? CANFD_MESSAGE_LENGTH + : CANBUS_MESSAGE_LENGTH; + if (source.len > max_length || + (source.is_remote_frame && source.is_fd) || + (source.is_fd && !enable_fd_)) { + *frame_num = 0; + CMVR_LOG(ERROR) << "invalid CAN frame at index " << i + << ", len=" << static_cast(source.len) + << ", fd=" << source.is_fd; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } } + int32_t sent_count = 0; + for (size_t i = 0; i < frames.size(); ++i) { + const auto& source = frames[i]; + if (std::chrono::steady_clock::now() >= send_deadline) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send batch timed out before frame " << i; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + + canid_t can_id = source.is_extended_id || + source.id > CAN_STANDARD_MAX_ID + ? (source.id & CAN_EFF_MASK) | CAN_EFF_FLAG + : (source.id & CAN_SFF_MASK); + if (source.is_remote_frame) { + can_id |= CAN_RTR_FLAG; + } + if (source.is_error_frame) { + can_id = (source.id & CAN_ERR_MASK) | CAN_ERR_FLAG; + } + + const void* payload = nullptr; + std::size_t expected = 0; + struct canfd_frame fd_frame {}; + struct can_frame classic_frame {}; + if (source.is_fd) { + fd_frame.can_id = can_id; + fd_frame.len = source.len; + if (source.bitrate_switch || default_bitrate_switch_) { + fd_frame.flags |= CANFD_BRS; + } + if (source.error_state_indicator) { + fd_frame.flags |= CANFD_ESI; + } + std::memcpy(fd_frame.data, source.data, source.len); + expected = CANFD_MTU; + payload = &fd_frame; + } else { + classic_frame.can_id = can_id; + classic_frame.can_dlc = source.len; + std::memcpy( + classic_frame.data, source.data, source.len); + expected = CAN_MTU; + payload = &classic_frame; + } + + while (true) { + const auto written = ::send( + dev_handler_, payload, expected, + MSG_DONTWAIT | MSG_NOSIGNAL); + if (written == static_cast(expected)) { + ++sent_count; + break; + } + if (written >= 0) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " sent a partial frame"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + if (errno == EINTR) { + continue; + } + if (errno != EAGAIN && errno != EWOULDBLOCK) { + *frame_num = sent_count; + CMVR_LOG(ERROR) << "can " << port_ + << " send message failed: " + << std::strerror(errno); + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + + const auto now = std::chrono::steady_clock::now(); + if (now >= send_deadline) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send batch timed out"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + const auto remaining = + std::chrono::duration_cast( + send_deadline - now); + struct timespec timeout { + static_cast( + remaining.count() / 1000000000LL), + static_cast( + remaining.count() % 1000000000LL) + }; + struct pollfd writable { + dev_handler_, POLLOUT, 0 + }; + const int ready = + ::ppoll(&writable, 1, &timeout, nullptr); + if (ready == 0) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send batch timed out"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + if (ready < 0 && errno != EINTR) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send poll failed: " + << std::strerror(errno); + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + } + } + + *frame_num = sent_count; return ErrorCode::OK; } // buf size must be 8 bytes, every time, we receive only one frame ErrorCode SocketCanClientRaw::receive(std::vector *const frames, int32_t *const frame_num) { + if (frames == nullptr || frame_num == nullptr) { + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } if (!is_started_) { CMVR_LOG(ERROR) << "Nvidia can client is not init! Please init first!"; return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; @@ -213,39 +459,109 @@ namespace cmvr { return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM; } - for (int32_t i = 0; i < *frame_num && i < MAX_CAN_RECV_FRAME_LEN; ++i) { + frames->clear(); + const int32_t requested = *frame_num; + *frame_num = 0; + for (int32_t i = 0; i < requested && i < MAX_CAN_RECV_FRAME_LEN; ++i) { CanFrame cf; - auto ret = read(dev_handler_, &recv_frames_[i], sizeof(recv_frames_[i])); + struct canfd_frame raw {}; + const auto ret = ::read(dev_handler_, &raw, CANFD_MTU); if (ret < 0) { - CMVR_LOG(ERROR) << "receive message failed, error code: " << ret; - return ErrorCode::CAN_CLIENT_ERROR_BASE; - } - if (recv_frames_[i].can_dlc > CANBUS_MESSAGE_LENGTH || - recv_frames_[i].can_dlc < 0) { - CMVR_LOG(ERROR) << "recv_frames_[" << i - << "].can_dlc = " << recv_frames_[i].can_dlc - << ", which is not equal to can message data length (" - << CANBUS_MESSAGE_LENGTH << ")."; + if (errno == EAGAIN || errno == EWOULDBLOCK || + errno == EINTR) { + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } + CMVR_LOG(ERROR) << "receive CAN message failed: " + << std::strerror(errno); return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; } - if (recv_frames_[i].can_id > CAN_STANDARD_MAX_ID) { - cf.id = enable_can_err_check_ - ? recv_frames_[i].can_id & CAN_EFF_MASK | CAN_ERR_FLAG - : recv_frames_[i].can_id & CAN_EFF_MASK; - } else { - cf.id = (recv_frames_[i].can_id & CAN_SFF_MASK); + if (ret != CAN_MTU && ret != CANFD_MTU) { + CMVR_LOG(ERROR) << "unexpected SocketCAN MTU: " << ret; + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; } - // CMVR_LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].can_id; - cf.len = recv_frames_[i].can_dlc; - std::memcpy(cf.data, recv_frames_[i].data, recv_frames_[i].can_dlc); + + const canid_t raw_id = raw.can_id; + cf.is_extended_id = (raw_id & CAN_EFF_FLAG) != 0; + cf.is_remote_frame = (raw_id & CAN_RTR_FLAG) != 0; + cf.is_error_frame = (raw_id & CAN_ERR_FLAG) != 0; + if (cf.is_error_frame) { + cf.id = raw_id & CAN_ERR_MASK; + } else if (cf.is_extended_id) { + cf.id = raw_id & CAN_EFF_MASK; + } else { + cf.id = raw_id & CAN_SFF_MASK; + } + + cf.is_fd = ret == CANFD_MTU; + if (cf.is_fd) { + cf.len = raw.len; + cf.bitrate_switch = (raw.flags & CANFD_BRS) != 0; + cf.error_state_indicator = (raw.flags & CANFD_ESI) != 0; + } else { + const auto* classic = + reinterpret_cast(&raw); + cf.len = classic->can_dlc; + } + const auto max_length = + cf.is_fd ? CANFD_MESSAGE_LENGTH : CANBUS_MESSAGE_LENGTH; + if (cf.len > max_length) { + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } + std::memcpy(cf.data, raw.data, cf.len); + + struct timespec monotonic {}; + if (::clock_gettime(CLOCK_MONOTONIC, &monotonic) == 0) { + cf.rx_monotonic_ns = + static_cast(monotonic.tv_sec) * 1000000000LL + + monotonic.tv_nsec; + } + ::gettimeofday(&cf.timestamp, nullptr); frames->push_back(cf); + ++(*frame_num); } return ErrorCode::OK; } - std::string SocketCanClientRaw::getErrorString(const int32_t /*status*/) { - return ""; + bool SocketCanClientRaw::discardPendingFrames() { + if (!is_started_ || dev_handler_ < 0) { + return false; + } + + constexpr std::size_t kMaximumDrainFrames = 4096; + const auto deadline = + std::chrono::steady_clock::now() + + std::chrono::microseconds(send_timeout_us_); + std::size_t count = 0; + while (count < kMaximumDrainFrames && + std::chrono::steady_clock::now() < deadline) { + struct canfd_frame raw {}; + const auto received = ::recv( + dev_handler_, &raw, CANFD_MTU, MSG_DONTWAIT); + if (received == CAN_MTU || received == CANFD_MTU) { + ++count; + continue; + } + if (received < 0 && + (errno == EAGAIN || errno == EWOULDBLOCK)) { + return true; + } + if (received < 0 && errno == EINTR) { + continue; + } + CMVR_LOG(ERROR) + << "failed while draining pending CAN frames: " + << (received < 0 ? std::strerror(errno) + : "unexpected MTU"); + return false; + } + CMVR_LOG(ERROR) + << "CAN receive queue did not drain within its bound"; + return false; + } + + std::string SocketCanClientRaw::getErrorString(const int32_t status) { + return std::strerror(status < 0 ? -status : status); } } } diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h index 7f478c86..ca2c01ab 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h @@ -12,11 +12,13 @@ #include #include +#include #include #include #include #include +#include #include #include @@ -51,6 +53,10 @@ namespace cmvr { */ cmvr::msgs::ErrorCode send(const std::vector &frames, int32_t *const frame_num) override; + cmvr::msgs::ErrorCode sendUntil( + const std::vector& frames, + int32_t* const frame_num, + std::chrono::steady_clock::time_point deadline) override; /** * @brief Receive messages @@ -60,6 +66,7 @@ namespace cmvr { */ cmvr::msgs::ErrorCode receive(std::vector *const frames, int32_t *const frame_num) override; + bool discardPendingFrames() override; /** * @brief Get the error string. @@ -67,14 +74,23 @@ namespace cmvr { */ std::string getErrorString(const int32_t status) override; private: - int dev_handler_ = 0; + int dev_handler_{-1}; cmvr::msgs::CANCardParameter::CANChannelId port_; cmvr::msgs::CANCardParameter::CANInterface interface_; - can_frame send_frames_[MAX_CAN_SEND_FRAME_LEN]; - can_frame recv_frames_[MAX_CAN_RECV_FRAME_LEN]; + std::string interface_name_; + bool enable_fd_{false}; + bool default_bitrate_switch_{false}; + bool receive_own_messages_{false}; + uint32_t receive_timeout_us_{100000}; + uint32_t send_timeout_us_{100000}; // bool enable_can_err_check_{false}; + + cmvr::msgs::ErrorCode sendWithDeadline_( + const std::vector& frames, + int32_t* frame_num, + std::chrono::steady_clock::time_point deadline); }; } } diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc index 391be9d3..745600eb 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc @@ -1,44 +1,226 @@ -#include "common/base/logging/logger.h" -// -// Created by lgv on 2025/7/16. -// -#include "cmvr/msgs/error_code.pb.h" -#include "cmvr/msgs/can_card_parameter.pb.h" #include "canbus/can_client/socket/socket_can_client_raw.h" -#include "gtest/gtest.h" -namespace cmvr { -namespace device { - using cmvr::msgs::ErrorCode; - using cmvr::msgs::CANCardParameter; - TEST(SocketCanClientRawTest, simple_test) { - CANCardParameter param; - param.set_brand(CANCardParameter::SOCKET_CAN_RAW); - param.set_channel_id(CANCardParameter::CHANNEL_ID_ZERO); +#include +#include +#include +#include +#include +#include +#include - cmvr::config::SocketCanConfig cfg; - cfg.set_channel_id(0); - SocketCanClientRaw socket_can_client(cfg); +#include - // EXPECT_EQ(socket_can_client.start(), ErrorCode::CAN_CLIENT_ERROR_BASE); - socket_can_client.start(); - std::vector frames; - int32_t num = 0; - EXPECT_EQ(socket_can_client.send(frames, &num), - ErrorCode::OK); - ++num; - EXPECT_EQ(socket_can_client.receive(&frames, &num), - ErrorCode::OK); - CMVR_LOG(INFO) << frames.at(0).CanFrameString(); - CanFrame can_frame; - can_frame.id = 0x123; - can_frame.len = 8; - memset(can_frame.data, 0xA3, sizeof(can_frame.data)); - frames.clear(); - frames.push_back(can_frame); - EXPECT_EQ(socket_can_client.sendSingleFrame(frames), - ErrorCode::OK); - socket_can_client.stop(); +namespace cmvr::device { +namespace { + +std::size_t openFileDescriptorCount() +{ + std::error_code error; + std::size_t count = 0; + for (std::filesystem::directory_iterator iterator( + "/proc/self/fd", error); + !error && iterator != std::filesystem::directory_iterator(); + iterator.increment(error)) { + ++count; + } + return error ? 0U : count; +} + +config::SocketCanConfig vcanConfig(const bool enable_fd) +{ + config::SocketCanConfig config; + config.set_interface_name("vcan0"); + config.set_enable_fd(enable_fd); + config.set_bitrate_switch(enable_fd); + config.set_receive_own_messages(false); + config.set_receive_timeout_us(2000U); + config.set_send_timeout_us(2000U); + return config; +} + +bool vcanAvailable() +{ + return ::if_nametoindex("vcan0") != 0U; +} + +TEST(SocketCanClientRawTest, MissingClassicInterfaceFailsWithoutLeakingFd) +{ + config::SocketCanConfig config; + config.set_interface_name("cmvr_no_such_can"); + config.set_enable_fd(false); + config.set_receive_timeout_us(100U); + config.set_send_timeout_us(100U); + SocketCanClientRaw client(config); + + const auto before = openFileDescriptorCount(); + ASSERT_GT(before, 0U); + for (int attempt = 0; attempt < 32; ++attempt) { + EXPECT_FALSE(client.start()); + EXPECT_TRUE(client.stop()); + } + const auto after = openFileDescriptorCount(); + EXPECT_LE(after, before + 1U); +} + +TEST(SocketCanClientRawTest, ClosedClientRejectsClassicSendAndReceive) +{ + config::SocketCanConfig config; + config.set_interface_name("cmvr_no_such_can"); + config.set_enable_fd(false); + SocketCanClientRaw client(config); + + CanFrame frame; + frame.id = 0x123U; + frame.len = 8U; + frame.is_fd = false; + std::fill(std::begin(frame.data), std::end(frame.data), 0xA3U); + std::vector frames{frame}; + int32_t count = 1; + EXPECT_EQ( + client.send(frames, &count), + msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED); + + count = 1; + EXPECT_EQ( + client.receive(&frames, &count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); + EXPECT_NE(frame.CanFrameString().find("fd:0"), std::string::npos); +} + +TEST(SocketCanClientRawTest, VcanTransmitsClassicAndCanFdBatches) +{ + if (!vcanAvailable()) { + GTEST_SKIP() << "vcan0 is not available in this network namespace"; + } + + SocketCanClientRaw classic_tx(vcanConfig(false)); + SocketCanClientRaw fd_rx(vcanConfig(true)); + ASSERT_TRUE(classic_tx.start()); + ASSERT_TRUE(fd_rx.start()); + + CanFrame first; + first.id = 0x123U; + first.len = 8U; + first.data[0] = 0xA1U; + CanFrame second; + second.id = 0x456U; + second.len = 3U; + second.data[0] = 0xB2U; + std::vector classic_frames{first, second}; + int32_t count = 2; + ASSERT_EQ( + classic_tx.send(classic_frames, &count), + msgs::ErrorCode::OK); + ASSERT_EQ(count, 2); + + for (const auto& expected : classic_frames) { + std::vector received; + int32_t receive_count = 1; + ASSERT_EQ( + fd_rx.receive(&received, &receive_count), + msgs::ErrorCode::OK); + ASSERT_EQ(receive_count, 1); + ASSERT_EQ(received.size(), 1U); + EXPECT_FALSE(received.front().is_fd); + EXPECT_EQ(received.front().id, expected.id); + EXPECT_EQ(received.front().len, expected.len); + EXPECT_EQ(received.front().data[0], expected.data[0]); + } + ASSERT_TRUE(classic_tx.stop()); + ASSERT_TRUE(fd_rx.stop()); + + SocketCanClientRaw fd_tx(vcanConfig(true)); + SocketCanClientRaw second_fd_rx(vcanConfig(true)); + ASSERT_TRUE(fd_tx.start()); + ASSERT_TRUE(second_fd_rx.start()); + CanFrame fd_first; + fd_first.id = 0x201U; + fd_first.len = 12U; + fd_first.is_fd = true; + fd_first.bitrate_switch = true; + fd_first.data[11] = 0xC3U; + CanFrame fd_second; + fd_second.id = 0x202U; + fd_second.len = 64U; + fd_second.is_fd = true; + fd_second.bitrate_switch = true; + fd_second.data[63] = 0xD4U; + std::vector fd_frames{fd_first, fd_second}; + count = 2; + ASSERT_EQ(fd_tx.send(fd_frames, &count), msgs::ErrorCode::OK); + ASSERT_EQ(count, 2); + + for (const auto& expected : fd_frames) { + std::vector received; + int32_t receive_count = 1; + ASSERT_EQ( + second_fd_rx.receive(&received, &receive_count), + msgs::ErrorCode::OK); + ASSERT_EQ(received.size(), 1U); + EXPECT_TRUE(received.front().is_fd); + EXPECT_TRUE(received.front().bitrate_switch); + EXPECT_EQ(received.front().id, expected.id); + EXPECT_EQ(received.front().len, expected.len); + EXPECT_EQ( + received.front().data[expected.len - 1U], + expected.data[expected.len - 1U]); } } + +TEST(SocketCanClientRawTest, VcanDrainAndBatchValidationAreFailClosed) +{ + if (!vcanAvailable()) { + GTEST_SKIP() << "vcan0 is not available in this network namespace"; + } + + SocketCanClientRaw tx(vcanConfig(false)); + SocketCanClientRaw rx(vcanConfig(false)); + ASSERT_TRUE(tx.start()); + ASSERT_TRUE(rx.start()); + + CanFrame valid; + valid.id = 0x321U; + valid.len = 8U; + valid.data[0] = 0x5AU; + std::vector one{valid}; + int32_t count = 1; + ASSERT_EQ(tx.send(one, &count), msgs::ErrorCode::OK); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + ASSERT_TRUE(rx.discardPendingFrames()); + + std::vector received; + int32_t receive_count = 1; + EXPECT_EQ( + rx.receive(&received, &receive_count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); + + CanFrame invalid = valid; + invalid.id = 0x322U; + invalid.len = 9U; + std::vector invalid_batch{valid, invalid}; + count = 2; + EXPECT_EQ( + tx.send(invalid_batch, &count), + msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED); + EXPECT_EQ(count, 0); + receive_count = 1; + EXPECT_EQ( + rx.receive(&received, &receive_count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); + + count = 1; + EXPECT_EQ( + tx.sendUntil( + one, &count, + std::chrono::steady_clock::now() - + std::chrono::microseconds(1)), + msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED); + EXPECT_EQ(count, 0); + receive_count = 1; + EXPECT_EQ( + rx.receive(&received, &receive_count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); } + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/canbus/common/canbus_consts.h b/cmvr-es/devices/canbus/common/canbus_consts.h index ed42ba19..d46f9223 100644 --- a/cmvr-es/devices/canbus/common/canbus_consts.h +++ b/cmvr-es/devices/canbus/common/canbus_consts.h @@ -26,9 +26,14 @@ namespace cmvr { namespace device { const int32_t CAN_FRAME_SIZE = 8; - const int32_t MAX_CAN_SEND_FRAME_LEN = 1; + const int32_t CAN_FD_FRAME_SIZE = 64; + // One UME cycle may submit a complete arm worth of frames. The receive + // API intentionally remains one-frame-at-a-time so a caller never + // blocks waiting to fill an artificial batch. + const int32_t MAX_CAN_SEND_FRAME_LEN = 64; const int32_t MAX_CAN_RECV_FRAME_LEN = 1; // 这个暂时改为 1 ,大量数据的时候改为 10 const int32_t CANBUS_MESSAGE_LENGTH = 8; // according to ISO-11891-1 + const int32_t CANFD_MESSAGE_LENGTH = 64; } } diff --git a/cmvr-es/devices/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 09c1284c..18d54623 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,16 +68,18 @@ bool CanMotorBusRuntime::start() return false; } - auto ret = sender_->Start(); + // Start the receiver first so a protocol response cannot arrive before the + // receive path is ready. + auto 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; } - ret = receiver_->Start(); + 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; } diff --git a/cmvr-es/main.cpp b/cmvr-es/main.cpp index 3e46bbfa..c1ac7852 100644 --- a/cmvr-es/main.cpp +++ b/cmvr-es/main.cpp @@ -1,5 +1,9 @@ #include +#include +#include +#include #include +#include #include "common/base/logging/logger.h" #include "runtime/include/cmvr_runtime.h" @@ -14,20 +18,91 @@ 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 -int main() +int main(int argc, char* argv[]) { + CommandLineOptions options; + if (!parseCommandLine(argc, argv, options)) { + printUsage(argv[0]); + return 2; + } + if (options.show_help) { + printUsage(argv[0]); + return 0; + } + sigset_t shutdown_signals; if (!blockShutdownSignals(shutdown_signals)) { return 1; } cmvr::Runtime runtime; - if (!runtime.init()) { + const bool initialized = options.config_path.empty() + ? runtime.init() + : runtime.init(options.config_path); + if (!initialized) { return 1; } - if (!runtime.startTasks()) { + if (!runtime.startTasks(options.control_period_s)) { return 1; } diff --git a/cmvr-es/manager/control_authority/CMakeLists.txt b/cmvr-es/manager/control_authority/CMakeLists.txt new file mode 100644 index 00000000..f2950a43 --- /dev/null +++ b/cmvr-es/manager/control_authority/CMakeLists.txt @@ -0,0 +1,34 @@ +add_library(control_authority STATIC + src/control_authority_manager.cpp +) +target_compile_features(control_authority PUBLIC cxx_std_17) +target_include_directories(control_authority + PUBLIC + ${PROJECT_SOURCE_DIR}/cmvr-es +) + +add_library( + cmvr_es::control_authority + ALIAS control_authority +) +install(TARGETS control_authority 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 + 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/include/control_authority_manager.h b/cmvr-es/manager/control_authority/include/control_authority_manager.h new file mode 100644 index 00000000..4e29f24f --- /dev/null +++ b/cmvr-es/manager/control_authority/include/control_authority_manager.h @@ -0,0 +1,74 @@ +#ifndef CMVR_ES_CONTROL_AUTHORITY_MANAGER_H +#define CMVR_ES_CONTROL_AUTHORITY_MANAGER_H + +#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; +}; + +// 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); + bool renew(const ControlLeaseToken& token, Duration ttl); + bool validate(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: + struct Entry { + std::string owner_id; + std::uint64_t generation{0}; + std::chrono::steady_clock::time_point deadline; + }; + + bool expired_(const Entry& entry) const noexcept; + + std::mutex mutex_; + std::unordered_map entries_; + std::uint64_t next_generation_{0}; +}; + +} // namespace cmvr::control + +#endif // CMVR_ES_CONTROL_AUTHORITY_MANAGER_H diff --git a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp new file mode 100644 index 00000000..e52d3353 --- /dev/null +++ b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp @@ -0,0 +1,152 @@ +#include "manager/control_authority/include/control_authority_manager.h" + +#include + +namespace cmvr::control { + +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()) { + if (!expired_(existing->second)) { + return { + false, + {}, + "control resource is already leased by " + + existing->second.owner_id}; + } + entries_.erase(existing); + } + + ControlLeaseToken token; + token.resource_id = resource_id; + token.owner_id = owner_id; + token.generation = ++next_generation_; + entries_.emplace( + resource_id, + Entry{ + owner_id, + token.generation, + std::chrono::steady_clock::now() + ttl}); + return {true, std::move(token), {}}; +} + +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() || + expired_(found->second) || + found->second.owner_id != token.owner_id || + found->second.generation != token.generation) { + if (found != entries_.end() && expired_(found->second)) { + entries_.erase(found); + } + 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)) { + entries_.erase(found); + return false; + } + return found->second.owner_id == token.owner_id && + found->second.generation == token.generation; +} + +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() && + found->second.owner_id == token.owner_id && + found->second.generation == token.generation) { + entries_.erase(found); + } + } 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)) { + entries_.erase(found); + return false; + } + return true; +} + +void ControlAuthorityManager::revoke( + const std::string& resource_id) noexcept +{ + try { + std::lock_guard lock(mutex_); + entries_.erase(resource_id); + } catch (...) { + } +} + +void ControlAuthorityManager::clear() noexcept +{ + try { + std::lock_guard lock(mutex_); + entries_.clear(); + } catch (...) { + } +} + +bool ControlAuthorityManager::expired_( + const Entry& entry) const noexcept +{ + return std::chrono::steady_clock::now() >= entry.deadline; +} + +} // namespace cmvr::control diff --git a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp new file mode 100644 index 00000000..b6b2b965 --- /dev/null +++ b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp @@ -0,0 +1,91 @@ +#include "manager/control_authority/include/control_authority_manager.h" + +#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, 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 a9ed557a..180e9638 100644 --- a/cmvr-es/manager/device_manager/CMakeLists.txt +++ b/cmvr-es/manager/device_manager/CMakeLists.txt @@ -42,9 +42,39 @@ 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 - "LD_LIBRARY_PATH=${_device_manager_test_library_path}" + "${_device_manager_snapshot_test_environment}" ) 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 b6726f20..8d1087cc 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -27,9 +27,10 @@ namespace cmvr::device { static DeviceManager& getInstance(); static void destroyInstance(); - void start(); - void restart(); + bool start(); + bool restart(); void stop(); + bool initialized() const noexcept { return initialized_; } void getDeviceList(std::list> &device_list); void registerDevice(const std::shared_ptr& device); @@ -54,14 +55,19 @@ namespace cmvr::device { std::unordered_map devices_; std::unordered_map device_statuses_; std::unique_ptr dev_factory_; + bool initialized_{false}; explicit DeviceManager(const config::DeviceManagerConfig &cfg); void log_device_plan_() const; - void pre_scan_robot_arm_dependencies_() const; - void init_devices_(); + bool pre_scan_robot_arm_dependencies_() const; + bool init_devices_(); void configure_mujoco_viewer_pip_(); - void start_devices_(); - void stop_devices_(); + void initialize_device_statuses_(); + void mark_initializing_statuses_error_(const std::string& error_message); + void update_device_status_(const std::string& device_id, + ManagedDeviceState state, + const std::string& error_message = {}); + void stop_devices_(bool update_status = true); }; } // cmvr diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index db84586f..123797f9 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -6,7 +6,10 @@ #include "../include/device_manager.h" #include +#include #include +#include +#include #include "devices/agv/abstract_agv.h" #include "devices/arm/robot_arm.h" @@ -31,6 +34,51 @@ namespace { using GroupJointSelection = std::unordered_map>; using MotorJointSelections = std::unordered_map; +constexpr std::size_t kMaxDeviceErrorLength = 512; + +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) { @@ -126,12 +174,24 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) { cfg_ = cfg; dev_factory_ = std::make_unique(); + initialize_device_statuses_(); logSection("Device Plan"); log_device_plan_(); - pre_scan_robot_arm_dependencies_(); + const bool dependencies_valid = pre_scan_robot_arm_dependencies_(); logSection("Initialize Devices"); - init_devices_(); - configure_mujoco_viewer_pip_(); + if (!dependencies_valid) { + mark_initializing_statuses_error_( + "device dependency validation failed"); + } + const bool devices_initialized = + dependencies_valid ? init_devices_() : false; + initialized_ = dependencies_valid && devices_initialized; + if (initialized_) { + configure_mujoco_viewer_pip_(); + } else { + CMVR_LOG(ERROR) << "[DeviceManager]: Initialization failed for at " + "least one enabled device"; + } } DeviceManager& DeviceManager::getInstance(const config::DeviceManagerConfig& cfg) { @@ -156,34 +216,133 @@ void DeviceManager::destroyInstance() { MotorManager::clearActiveJoints(); } -void DeviceManager::start(){ - for (auto& [id, record] : devices_) { - if (!record.device) { - CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; - continue; - } - if (record.device->start()) { - CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success"; - } else { - CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed"; +bool DeviceManager::start(){ + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (!initialized_) { + CMVR_LOG(ERROR) << "[DeviceManager]: Refusing to start because " + "initialization did not complete"; + stop_devices_(false); + return false; + } + + std::vector>> + devices; + { + std::shared_lock lock(devices_mutex_); + devices.reserve(devices_.size()); + for (const auto& [id, record] : devices_) { + devices.emplace_back(id, record.device); } } + + bool all_started = true; + for (const auto& [id, device] : devices) { + if (!device) { + CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; + update_device_status_( + id, ManagedDeviceState::Error, + "cannot start null device: " + id); + all_started = false; + continue; + } + bool started = false; + std::string error_message; + try { + 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) { + update_device_status_(id, ManagedDeviceState::Running); + CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success"; + } else { + update_device_status_( + id, ManagedDeviceState::Error, error_message); + CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed"; + all_started = false; + } + } + if (!all_started) { + CMVR_LOG(ERROR) << "[DeviceManager]: At least one enabled device failed " + "to start; 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); + } + return all_started; } -void DeviceManager::restart() { +bool DeviceManager::restart() { stop(); - start(); + return start(); } void DeviceManager::stop() { - for (auto& [id, record] : devices_) { - if (!record.device) { + std::lock_guard lifecycle_lock(lifecycle_mutex_); + stop_devices_(); +} + +void DeviceManager::stop_devices_(const bool update_status) { + std::vector>> + devices; + { + std::shared_lock lock(devices_mutex_); + devices.reserve(devices_.size()); + for (const auto& [id, record] : devices_) { + devices.emplace_back(id, record.device); + } + } + + for (const auto& [id, device] : devices) { + if (!device) { CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; + if (update_status) { + update_device_status_( + id, ManagedDeviceState::Error, + "cannot stop null device: " + id); + } continue; } - if (record.device->stop()) { + bool stopped = false; + std::string error_message; + try { + stopped = device->stop(); + if (!stopped) { + error_message = "device stop returned false: " + id; + } + } catch (const std::exception& error) { + error_message = + "device stop threw for " + id + ": " + error.what(); + CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id + << " threw: " << error.what(); + } catch (...) { + error_message = + "device stop threw an unknown exception: " + id; + CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id + << " threw an unknown exception"; + } + if (stopped) { + if (update_status) { + update_device_status_(id, ManagedDeviceState::Stopped); + } CMVR_LOG(INFO) << "[DeviceManager]: Stop device " << id << " Success"; } else { + if (update_status) { + update_device_status_( + id, ManagedDeviceState::Error, error_message); + } CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id << " Failed"; } } @@ -192,6 +351,7 @@ void DeviceManager::stop() { template std::shared_ptr DeviceManager::getDevice(const std::string& device_id) { + std::shared_lock lock(devices_mutex_); auto it = devices_.find(device_id); if (it == devices_.end()) { CMVR_LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " not found."; @@ -219,6 +379,7 @@ std::shared_ptr DeviceManager::getDeviceBase(const std::string& void DeviceManager::getDeviceList(std::list>& device_list){ device_list.clear(); + std::shared_lock lock(devices_mutex_); for (const auto& [device_id, record] : devices_) { device_list.emplace_back(device_id, record.type_name); } @@ -244,22 +405,115 @@ void DeviceManager::registerDevice(const std::string& device_id, CMVR_LOG(ERROR) << "[DeviceManager]: Cannot register device with empty id"; return; } - if (devices_.count(device_id)) { - CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id; - return; - } DeviceRecord record; record.id = device_id; record.kind = device->kind(); record.type_name = device->typeName(); record.device = device; - devices_.emplace(record.id, std::move(record)); + { + std::unique_lock lock(devices_mutex_); + if (devices_.count(device_id)) { + CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id; + return; + } + + ManagedDeviceSnapshot status; + status.id = record.id; + status.kind = record.kind; + status.type_name = record.type_name; + status.enabled = true; + status.state = ManagedDeviceState::Registered; + status.status_updated_at_unix_ms = unixTimeMs(); + devices_.emplace(record.id, std::move(record)); + device_statuses_[device_id] = std::move(status); + } 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) +{ + 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(); +} + DeviceManagerSnapshot DeviceManager::snapshot() const { struct SnapshotSource { @@ -270,15 +524,14 @@ DeviceManagerSnapshot DeviceManager::snapshot() const std::vector sources; { std::shared_lock lock(devices_mutex_); - sources.reserve(devices_.size()); - for (const auto& [id, record] : devices_) { + sources.reserve(device_statuses_.size()); + for (const auto& [id, stored_status] : device_statuses_) { 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; + source.status = stored_status; + const auto device_it = devices_.find(id); + if (device_it != devices_.end()) { + source.device = device_it->second.device; + } sources.push_back(std::move(source)); } } @@ -302,10 +555,23 @@ DeviceManagerSnapshot DeviceManager::snapshot() const "device health snapshot threw an unknown exception"; } } - source.status.abnormal = + source.status.health.error_message = + truncateDeviceError(source.status.health.error_message); + const bool lifecycle_error = + source.status.state == ManagedDeviceState::Error; + const bool health_error = source.status.health.state == DeviceHealthState::Degraded || source.status.health.state == DeviceHealthState::Fault; - source.status.error_message = source.status.health.error_message; + source.status.abnormal = lifecycle_error || health_error; + if (source.status.error_message.empty()) { + source.status.error_message = + source.status.health.error_message; + } + source.status.error_message = + truncateDeviceError(source.status.error_message); + if (source.status.status_updated_at_unix_ms == 0) { + source.status.status_updated_at_unix_ms = unixTimeMs(); + } result.devices.push_back(std::move(source.status)); } @@ -354,10 +620,11 @@ void DeviceManager::log_device_plan_() const CMVR_LOG(INFO) << "[DeviceManager]: Device plan end"; } -void DeviceManager::pre_scan_robot_arm_dependencies_() const +bool DeviceManager::pre_scan_robot_arm_dependencies_() const { MotorJointSelections selections; std::unordered_map motor_roots; + MotorManager::clearActiveJoints(); for (const auto& entry : cfg_.devices()) { if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) { @@ -365,22 +632,22 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty"; - return; + return false; } if (entry.config_file().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id(); - return; + return false; } config::MotorRootConfig root_cfg; if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file(); - return; + return false; } if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) { CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id() << "' does not match config id '" << root_cfg.motor().id() << "'"; - return; + return false; } motor_roots.emplace(entry.id(), std::move(root_cfg)); } @@ -391,17 +658,17 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty"; - return; + return false; } if (entry.config_file().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id(); - return; + return false; } config::ArmRootConfig root_cfg; if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file(); - return; + return false; } const config::RobotArmConfig* arm_cfg = nullptr; @@ -414,29 +681,30 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const if (!arm_cfg) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id() << "' not found in config: " << entry.config_file(); - return; + return false; } - if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) { + if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor || + arm_cfg->backend_case() == config::RobotArmConfig::kUme) { continue; } if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id(); - return; + return false; } const auto& motor_config = arm_cfg->motor(); if (motor_config.motor_system_id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id(); - return; + return false; } if (motor_config.motor_group_ids_size() == 0) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id(); - return; + return false; } if (motor_config.joint_names_size() == 0) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id(); - return; + return false; } const auto motor_root_it = motor_roots.find(motor_config.motor_system_id()); @@ -444,7 +712,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() << "' depends on disabled or missing MotorManager: " << motor_config.motor_system_id(); - return; + return false; } std::unordered_set allowed_groups; @@ -452,7 +720,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const for (const auto& group_id : motor_config.motor_group_ids()) { if (group_id.empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id(); - return; + return false; } allowed_groups.insert(group_id); } @@ -461,7 +729,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const for (const auto& joint_name : motor_config.joint_names()) { if (joint_name.empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id(); - return; + return false; } std::string matched_group; @@ -480,7 +748,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() << "' joint '" << joint_name << "' not found in configured motor_group_ids"; - return; + return false; } group_selection[matched_group].insert(joint_name); } @@ -495,46 +763,132 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } } - MotorManager::clearActiveJoints(); for (auto& [motor_system_id, group_selection] : selections) { MotorManager::setActiveJoints(motor_system_id, std::move(group_selection)); } + return true; } -void DeviceManager::init_devices_() { +bool DeviceManager::init_devices_() { + bool all_initialized = true; for (const auto& entry : cfg_.devices()) { if (!entry.enable()) { continue; } + { + std::shared_lock lock(devices_mutex_); + const auto status_it = device_statuses_.find(entry.id()); + if (status_it != device_statuses_.end() && + status_it->second.state == ManagedDeviceState::Error) { + all_initialized = false; + continue; + } + } + CMVR_LOG(INFO) << "[DeviceManager]: Initialize device begin" << ", id=" << entry.id() << ", type=" << deviceTypeToString(entry.type()) << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); - DeviceRecord record = dev_factory_->create(entry); + DeviceRecord record; + try { + record = dev_factory_->create(entry); + } catch (const std::exception& error) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "device creation threw for " + entry.id() + ": " + + error.what()); + CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw for " + << entry.id() << ": " << error.what(); + all_initialized = false; + continue; + } catch (...) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "device creation threw an unknown exception: " + + entry.id()); + CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw an " + "unknown exception for " << entry.id(); + all_initialized = false; + continue; + } if (!record.device || record.id.empty()) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "failed to create configured device: " + entry.id()); CMVR_LOG(ERROR) << "[DeviceManager]: Failed to create device for entry id=" << entry.id(); + all_initialized = false; continue; } CMVR_LOG(INFO) << "[DeviceManager]: Create device object success" << ", id=" << record.id << ", type=" << record.type_name << ", kind=" << toString(record.kind); - if (devices_.count(record.id)) { + bool duplicate_device = false; + { + std::shared_lock lock(devices_mutex_); + duplicate_device = devices_.count(record.id) != 0; + } + if (duplicate_device) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "duplicate configured device id: " + record.id); CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate " << record.type_name << " Device ID " << record.id; + all_initialized = false; continue; } CMVR_LOG(INFO) << "[DeviceManager]: Init device object begin" << ", id=" << record.id << ", type=" << record.type_name << ", kind=" << toString(record.kind); - if (!record.device->init()) { + bool device_initialized = false; + std::string init_error_message; + try { + device_initialized = record.device->init(); + if (!device_initialized) { + init_error_message = + "device init returned false: " + record.id; + } + } catch (const std::exception& error) { + init_error_message = + "device init threw for " + record.id + ": " + + error.what(); + CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw" + << ", id=" << record.id + << ", error=" << error.what(); + } catch (...) { + init_error_message = + "device init threw an unknown exception: " + record.id; + CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw an " + "unknown exception, id=" << record.id; + } + if (!device_initialized) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + init_error_message); CMVR_LOG(ERROR) << "[DeviceManager]: Init device object failed" << ", id=" << record.id << ", type=" << record.type_name << ", kind=" << toString(record.kind) << ", config_file=" << entry.config_file(); + try { + if (!record.device->stop()) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Cleanup after failed init " + "returned false, id=" << record.id; + } + } catch (const std::exception& error) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Cleanup after failed init threw" + << ", id=" << record.id + << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Cleanup after failed init threw an " + "unknown exception, id=" << record.id; + } + all_initialized = false; continue; } CMVR_LOG(INFO) << "[DeviceManager]: Init device object success" @@ -542,8 +896,36 @@ void DeviceManager::init_devices_() { << ", type=" << record.type_name << ", kind=" << toString(record.kind) << ", config_file=" << entry.config_file(); - devices_.emplace(record.id, std::move(record)); + { + 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(); + } } + return all_initialized; } void DeviceManager::configure_mujoco_viewer_pip_() 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 new file mode 100644 index 00000000..a9009350 --- /dev/null +++ b/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp @@ -0,0 +1,124 @@ +#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); +} + +} // 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 05d5bf8c..a5191824 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 @@ -253,6 +253,13 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(duplicate_status->error_message == "duplicate configured device id: duplicate_device"); + // Configuration failures deliberately make this manager ineligible for + // start(). Use a fresh, valid manager for dynamic registration and + // lifecycle transitions so the test does not weaken fail-closed startup. + DeviceManager::destroyInstance(); + cmvr::config::DeviceManagerConfig dynamic_config; + auto& dynamic_manager = DeviceManager::getInstance(dynamic_config); + auto healthy = std::make_shared("z_healthy"); auto degraded = std::make_shared("a_degraded"); degraded->health = { @@ -264,18 +271,18 @@ bool testConfiguredAndDynamicSnapshots() auto health_throw = std::make_shared("b_health_throw"); health_throw->throw_on_health = true; - manager.registerDevice(healthy); - manager.registerDevice(degraded); - manager.registerDevice(start_fail); - manager.registerDevice(stop_fail); - manager.registerDevice(health_throw); + dynamic_manager.registerDevice(healthy); + dynamic_manager.registerDevice(degraded); + dynamic_manager.registerDevice(start_fail); + dynamic_manager.registerDevice(stop_fail); + dynamic_manager.registerDevice(health_throw); // Duplicate registration must retain the original object and status. - manager.registerDevice( + dynamic_manager.registerDevice( std::make_shared("z_healthy", DeviceKind::Speaker)); - CHECK_TRUE(manager.getDeviceBase("z_healthy") == healthy); + CHECK_TRUE(dynamic_manager.getDeviceBase("z_healthy") == healthy); - const auto registered = manager.snapshot(); + const auto registered = dynamic_manager.snapshot(); CHECK_TRUE(isSorted(registered)); const auto* healthy_registered = findDevice(registered, "z_healthy"); @@ -304,8 +311,8 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(thrown_health->health.error_message.size() <= 512); CHECK_TRUE(thrown_health->error_message.size() <= 512); - manager.start(); - const auto running = manager.snapshot(); + CHECK_TRUE(!dynamic_manager.start()); + const auto running = dynamic_manager.snapshot(); CHECK_TRUE(findDevice(running, "z_healthy")->state == ManagedDeviceState::Running); CHECK_TRUE(findDevice(running, "m_start_fail")->state == @@ -319,14 +326,16 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(healthy_registered->state == ManagedDeviceState::Registered); - manager.stop(); - const auto stopped = manager.snapshot(); + dynamic_manager.stop(); + const auto stopped = dynamic_manager.snapshot(); CHECK_TRUE(findDevice(stopped, "z_healthy")->state == ManagedDeviceState::Stopped); CHECK_TRUE(findDevice(stopped, "n_stop_fail")->state == ManagedDeviceState::Error); CHECK_TRUE(findDevice(stopped, "n_stop_fail")->abnormal); - CHECK_TRUE(healthy->stop_calls.load() == 1); + // Failed start rolls back every device once; explicit stop performs the + // second best-effort stop. + CHECK_TRUE(healthy->stop_calls.load() == 2); return true; } diff --git a/cmvr-es/manager/task_manager/CMakeLists.txt b/cmvr-es/manager/task_manager/CMakeLists.txt index 40b1eb48..12768c1d 100644 --- a/cmvr-es/manager/task_manager/CMakeLists.txt +++ b/cmvr-es/manager/task_manager/CMakeLists.txt @@ -15,3 +15,29 @@ target_link_libraries(task_manager 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 + 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 b1d0609f..339f95c6 100644 --- a/cmvr-es/manager/task_manager/include/task_manager.h +++ b/cmvr-es/manager/task_manager/include/task_manager.h @@ -26,9 +26,10 @@ namespace cmvr::task { ~TaskManager(); - void startRunTask(double control_period_s = 0.001); + bool startRunTask(double control_period_s = 0.001); void stopRunTask(); bool running() const { return running_.load(); } + bool initialized() const noexcept { return initialized_; } std::shared_ptr getTask(const std::string& task_id) const; std::shared_ptr getTouchScreenTask(const std::string& task_id = "touch_screen") const; @@ -37,7 +38,7 @@ namespace cmvr::task { explicit TaskManager(const config::TaskManagerConfig& cfg); void logTaskPlan() const; - void initTasks(); + bool initTasks(); void runTaskLoop(double control_period_s); static TaskRunMode toTaskRunMode(config::TaskConfigEntry::TaskRunMode run_mode); @@ -49,8 +50,10 @@ namespace cmvr::task { std::unordered_map task_period_s_; std::unordered_map next_step_time_; mutable std::mutex tasks_mutex_; + std::mutex lifecycle_mutex_; std::atomic running_{false}; std::thread run_thread_; + bool initialized_{false}; }; } // namespace cmvr::task diff --git a/cmvr-es/manager/task_manager/src/task_manager.cpp b/cmvr-es/manager/task_manager/src/task_manager.cpp index a8af771c..cb34859d 100644 --- a/cmvr-es/manager/task_manager/src/task_manager.cpp +++ b/cmvr-es/manager/task_manager/src/task_manager.cpp @@ -40,6 +40,8 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type) return "TASK_TYPE_SELF_COLLISION"; case config::TaskConfigEntry::TASK_TYPE_QUIC_EDGE: return "TASK_TYPE_QUIC_EDGE"; + case config::TaskConfigEntry::TASK_TYPE_UME_TELEOP: + return "TASK_TYPE_UME_TELEOP"; case config::TaskConfigEntry::TASK_TYPE_UNKNOWN: default: return "TASK_TYPE_UNKNOWN"; @@ -59,6 +61,21 @@ const char* taskConfigRunModeToString(const config::TaskConfigEntry::TaskRunMode } } +void stopTaskNoThrow(const std::shared_ptr& task) +{ + if (!task) { + return; + } + try { + task->stop(); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] task stop threw: " + << error.what(); + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] task stop threw an unknown exception"; + } +} + } // namespace std::shared_ptr TaskManager::instance_ = nullptr; @@ -70,7 +87,11 @@ TaskManager::TaskManager(const config::TaskManagerConfig& cfg) logSection("Task Plan"); logTaskPlan(); logSection("Initialize Tasks"); - initTasks(); + initialized_ = initTasks(); + if (!initialized_) { + CMVR_LOG(ERROR) << "[TaskManager] Initialization failed for at least " + "one enabled task"; + } } TaskManager::~TaskManager() @@ -126,16 +147,20 @@ std::shared_ptr TaskManager::getTask(const std::string& task_id) const return it->second; } -void TaskManager::startRunTask(const double control_period_s) +bool TaskManager::startRunTask(const double control_period_s) { + std::lock_guard lifecycle_lock(lifecycle_mutex_); + 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; + return false; } - - bool expected = false; - if (!running_.compare_exchange_strong(expected, true)) { - return; + if (running_.load()) { + return true; } std::vector> tasks; @@ -151,39 +176,57 @@ void TaskManager::startRunTask(const double control_period_s) std::vector> started_tasks; for (const auto& task : tasks) { - if (!task->start()) { + bool started = false; + try { + started = task->start(); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] task start threw: " + << task->id() << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] task start threw an unknown " + "exception: " << task->id(); + } + if (!started) { CMVR_LOG(ERROR) << "[TaskManager] task start failed: " << task->id(); - for (const auto& started_task : started_tasks) { - try { - started_task->stop(); - } catch (...) { - } + stopTaskNoThrow(task); + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); } running_.store(false); - return; + return false; } started_tasks.push_back(task); } + running_.store(true); try { run_thread_ = std::thread(&TaskManager::runTaskLoop, this, control_period_s); } catch (const std::exception& e) { CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread: " << e.what(); running_.store(false); - for (const auto& task : started_tasks) { - try { - task->stop(); - } catch (...) { - } + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); } - return; + return false; + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread with an " + "unknown exception"; + running_.store(false); + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); + } + return false; } + return true; } void TaskManager::stopRunTask() { - bool expected = true; - if (!running_.compare_exchange_strong(expected, false)) { + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (!running_.exchange(false)) { return; } @@ -202,21 +245,20 @@ void TaskManager::stopRunTask() } } for (const auto& task : tasks) { - try { - task->stop(); - } catch (...) { - } + stopTaskNoThrow(task); } } -void TaskManager::initTasks() +bool TaskManager::initTasks() { + bool all_initialized = true; for (const auto& entry : cfg_.tasks()) { if (!entry.enable()) { continue; } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[TaskManager] Task ID is empty"; + all_initialized = false; continue; } @@ -226,9 +268,30 @@ void TaskManager::initTasks() << ", run_mode=" << taskConfigRunModeToString(entry.run_mode()) << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); - auto task = TaskFactory::create(entry); + std::shared_ptr task; + try { + task = TaskFactory::create(entry); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] Task creation threw: " + << entry.id() << ", error=" << error.what(); + all_initialized = false; + continue; + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] Task creation threw an unknown " + "exception: " << entry.id(); + all_initialized = false; + continue; + } if (!task || task->id() != entry.id()) { CMVR_LOG(ERROR) << "[TaskManager] Task ID mismatch: " << entry.id(); + all_initialized = false; + continue; + } + if (entry.run_mode() == + config::TaskConfigEntry::TASK_RUN_MODE_UNKNOWN) { + CMVR_LOG(ERROR) << "[TaskManager] Task run_mode is unknown: " + << entry.id(); + all_initialized = false; continue; } const TaskRunMode configured_run_mode = toTaskRunMode(entry.run_mode()); @@ -236,23 +299,39 @@ void TaskManager::initTasks() CMVR_LOG(ERROR) << "[TaskManager] Task run_mode mismatch: id=" << entry.id() << ", configured=" << taskRunModeToString(configured_run_mode) << ", actual=" << taskRunModeToString(task->runMode()); + all_initialized = false; continue; } double control_period_s = entry.control_period_s(); if (configured_run_mode == TaskRunMode::PERIODIC_STEP && (!std::isfinite(control_period_s) || control_period_s <= 0.0)) { CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s for task: " << entry.id(); + all_initialized = false; continue; } - if (!task->init()) { + bool task_initialized = false; + try { + task_initialized = task->init(); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] Task init threw: " + << entry.id() << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] Task init threw an unknown " + "exception: " << entry.id(); + } + if (!task_initialized) { CMVR_LOG(ERROR) << "[TaskManager] Task init failed: " << entry.id() << ", status=" << task->detailStatusString(); + stopTaskNoThrow(task); + all_initialized = false; continue; } { std::lock_guard lock(tasks_mutex_); if (tasks_.count(entry.id())) { CMVR_LOG(ERROR) << "[TaskManager] Duplicate task ID: " << entry.id(); + stopTaskNoThrow(task); + all_initialized = false; continue; } if (configured_run_mode == TaskRunMode::PERIODIC_STEP) { @@ -261,6 +340,7 @@ void 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 new file mode 100644 index 00000000..5efeeb12 --- /dev/null +++ b/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp @@ -0,0 +1,194 @@ +#include "manager/task_manager/include/task_manager.h" + +#include +#include + +#include + +#include "task/task_factory.h" + +namespace { + +struct TaskBehavior { + bool init_result{true}; + bool start_result{true}; + bool throw_on_start{false}; +}; + +TaskBehavior task_behavior; + +class LifecycleTask final : public cmvr::task::Task { +public: + explicit LifecycleTask(std::string id) + : id_(std::move(id)) + { + } + + const std::string& id() const override { return id_; } + cmvr::task::TaskRunMode runMode() const override + { + return cmvr::task::TaskRunMode::BLOCKING_SERVICE; + } + + 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; + 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; + state_ = cmvr::task::TaskState::STOPPED; + } + + 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}; + +private: + std::string id_; + cmvr::task::TaskState state_{ + cmvr::task::TaskState::UNINITIALIZED}; +}; + +std::shared_ptr created_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(); + task_behavior = {}; + created_task.reset(); + 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 + { + cmvr::task::TaskManager::destroyInstance(); + created_task.reset(); + } +}; + +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); +} + +} // namespace diff --git a/cmvr-es/runtime/CMakeLists.txt b/cmvr-es/runtime/CMakeLists.txt index f8a9a628..91c68a2c 100644 --- a/cmvr-es/runtime/CMakeLists.txt +++ b/cmvr-es/runtime/CMakeLists.txt @@ -13,8 +13,39 @@ 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 6f0378ae..b39d51d7 100644 --- a/cmvr-es/runtime/src/cmvr_runtime.cpp +++ b/cmvr-es/runtime/src/cmvr_runtime.cpp @@ -12,6 +12,7 @@ #include "common/io/proto_file_io.h" #include "task/grpc_server_task/include/grpc_server_task.h" #include "task/quic_edge_task/include/quic_edge_task.h" +#include "task/ume_teleop_task/include/ume_teleop_task.h" namespace cmvr { namespace { @@ -100,13 +101,27 @@ bool Runtime::init_(const std::string& config_path, return false; } CMVR_LOG(INFO) << "[Startup] Initialize DeviceManager"; - device::DeviceManager::getInstance(device_manager_root.device_manager()); + auto& device_manager = + device::DeviceManager::getInstance( + device_manager_root.device_manager()); + if (!device_manager.initialized()) { + CMVR_LOG(ERROR) << "[Startup] DeviceManager initialization failed"; + device_manager.stop(); + device::DeviceManager::destroyInstance(); + return false; + } + const auto rollback_device_manager = [&device_manager]() { + device_manager.stop(); + device::DeviceManager::destroyInstance(); + }; task::registerGrpcServerTaskFactory(); task::registerQuicEdgeTaskFactory(); + task::registerUmeTeleopTaskFactory(); if (app_config.task_manager_config_file().empty()) { CMVR_LOG(ERROR) << "TaskManager config file is empty"; + rollback_device_manager(); return false; } logSection("TaskManager"); @@ -116,11 +131,19 @@ bool Runtime::init_(const std::string& config_path, if (!ConfigHelper::loadConfigFile(app_config.task_manager_config_file(), task_manager_root)) { CMVR_LOG(ERROR) << "Failed to load TaskManager config: " << app_config.task_manager_config_file(); + rollback_device_manager(); return false; } CMVR_LOG(INFO) << "[Startup] Initialize TaskManager"; - task::TaskManager::getInstance(task_manager_root.task_manager()); + auto& task_manager = + task::TaskManager::getInstance(task_manager_root.task_manager()); + if (!task_manager.initialized()) { + CMVR_LOG(ERROR) << "[Startup] TaskManager initialization failed"; + task::TaskManager::destroyInstance(); + rollback_device_manager(); + return false; + } initialized_ = true; return true; } @@ -134,9 +157,26 @@ bool Runtime::startTasks(const double control_period_s) return true; } + // Device init() constructs and validates resources; start() owns worker + // threads. Start devices before any task can publish commands or sample + // them. UME start remains passive and never enables actuators. + logSection("Start Devices"); + CMVR_LOG(INFO) << "[Startup] Start devices"; + if (!device::DeviceManager::getInstance().start()) { + CMVR_LOG(ERROR) << "[Startup] One or more enabled devices failed " + "to start; tasks will not be started"; + return false; + } + logSection("Start Tasks"); CMVR_LOG(INFO) << "[Startup] Start tasks"; - task::TaskManager::getInstance().startRunTask(control_period_s); + if (!task::TaskManager::getInstance().startRunTask(control_period_s)) { + CMVR_LOG(ERROR) << "[Startup] One or more enabled tasks failed " + "to start; stopping devices"; + device::DeviceManager::getInstance().stop(); + tasks_started_ = false; + return false; + } tasks_started_ = true; return true; } diff --git a/cmvr-es/runtime/tests/runtime_lifecycle_test.cpp b/cmvr-es/runtime/tests/runtime_lifecycle_test.cpp new file mode 100644 index 00000000..60f8d7ca --- /dev/null +++ b/cmvr-es/runtime/tests/runtime_lifecycle_test.cpp @@ -0,0 +1,266 @@ +#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 b46b443b..6da68349 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -7,6 +7,8 @@ add_library(service grpc/src/grpc_head_service.cpp grpc/src/grpc_dexhand_service.cpp grpc/src/grpc_arm_service.cpp + grpc/src/grpc_arm_teleop_service.cpp + grpc/src/grpc_robot_arm_teleop_backend.cpp grpc/src/grpc_motor_service.cpp grpc/src/grpc_agv_service.cpp grpc/src/grpc_hlc_service.cpp @@ -18,6 +20,7 @@ target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(service PRIVATE cmvr_es::proto osqp + cmvr_es::control_authority cmvr_es::device_manager cmvr_es::task_manager cmvr_es::algorithms::controller @@ -44,6 +47,61 @@ if(BUILD_TESTING) ) set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10) + add_executable(grpc_arm_teleop_service_test + grpc/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 + grpc/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 grpc/tests/grpc_motor_service_test.cpp ) diff --git a/cmvr-es/service/arm_teleop_client/CMakeLists.txt b/cmvr-es/service/arm_teleop_client/CMakeLists.txt new file mode 100644 index 00000000..8dd1b377 --- /dev/null +++ b/cmvr-es/service/arm_teleop_client/CMakeLists.txt @@ -0,0 +1,38 @@ +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/arm_teleop_client/include/grpc_arm_teleop_client.h b/cmvr-es/service/arm_teleop_client/include/grpc_arm_teleop_client.h new file mode 100644 index 00000000..731a718d --- /dev/null +++ b/cmvr-es/service/arm_teleop_client/include/grpc_arm_teleop_client.h @@ -0,0 +1,80 @@ +#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/arm_teleop_client/src/grpc_arm_teleop_client.cpp b/cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp new file mode 100644 index 00000000..0c686937 --- /dev/null +++ b/cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp @@ -0,0 +1,223 @@ +#include "service/arm_teleop_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/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp b/cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp new file mode 100644 index 00000000..8be21e70 --- /dev/null +++ b/cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp @@ -0,0 +1,220 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "cmvr/api/arm_teleop_v1.grpc.pb.h" +#include "service/arm_teleop_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/include/grpc_arm_teleop_service.h b/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h new file mode 100644 index 00000000..8684f91d --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h @@ -0,0 +1,91 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include "cmvr/api/arm_teleop_v1.grpc.pb.h" +#include "manager/control_authority/include/control_authority_manager.h" + +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); + ~ArmTeleopServiceImpl() override = default; + + grpc::Status Teleoperate( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream) override; + +private: + std::shared_ptr backend_; + control::ControlAuthorityManager* authority_{nullptr}; +}; + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h b/cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h new file mode 100644 index 00000000..c15c3562 --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" +#include "devices/arm/robot_arm.h" +#include "service/grpc/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/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 1ae88019..6c3ca566 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -1,8 +1,13 @@ #include "service/grpc/include/grpc_arm_service.h" +#include +#include +#include + #include #include "common/base/logging/logger.h" +#include "manager/control_authority/include/control_authority_manager.h" using google::protobuf::util::TimeUtil; @@ -119,6 +124,64 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id) return grpc::Status(grpc::StatusCode::NOT_FOUND, message); } +grpc::Status setControlLeaseConflict( + api::CommandHeader_Feedback* response, + const std::string& device_id) +{ + const std::string message = + "RobotArm control is leased by another active control operation: " + + device_id; + fillFeedback(response, false, message); + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, message); +} + +template +grpc::Status setControlLeaseConflict( + Response* response, + const std::string& device_id) +{ + return setControlLeaseConflict( + response->mutable_header(), device_id); +} + +class ScopedUnaryControlLease final { +public: + ScopedUnaryControlLease( + const std::string& device_id, + const char* operation) + : 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); + auto acquired = manager_.tryAcquire( + device_id, + owner, + std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::hours(24))); + acquired_ = acquired.acquired; + token_ = std::move(acquired.token); + } + + ~ScopedUnaryControlLease() + { + manager_.release(token_); + } + + bool acquired() const noexcept { return acquired_; } + +private: + control::ControlAuthorityManager& manager_; + control::ControlLeaseToken token_; + bool acquired_{false}; +}; + } // namespace gRPCArmServiceImpl::gRPCArmServiceImpl() @@ -136,6 +199,10 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + // A safety-disable command preempts any network teleoperation lease. + // The teleoperation executor must fail its next renew before it can + // dispatch another setpoint. + control::ControlAuthorityManager::instance().revoke(device_id); const auto result = arm->torqueOff(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { @@ -158,6 +225,11 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "torqueOn"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->torqueOn(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { @@ -180,6 +252,11 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "moveJ"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->moveJ(toJointPositionCommand(request->target()), toMotionOptions(request->options())); if (result.ok()) { @@ -203,6 +280,11 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "moveL"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->moveL(toCartesianPose(request->target()), toMotionOptions(request->options()), toFrameType(request->frame())); @@ -227,6 +309,11 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "speedJ"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()), request->acceleration(), request->duration()); @@ -253,6 +340,11 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "speedL"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->speedL(toCartesianVelocity(request->velocity()), request->acceleration(), request->duration(), @@ -280,6 +372,11 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "servoJ"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->servoJ(toJointPositionCommand(request->target())); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (servoJ): success, id=" << device_id @@ -302,6 +399,7 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + control::ControlAuthorityManager::instance().revoke(device_id); const auto result = arm->stopMotion(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { @@ -379,6 +477,11 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "calibrateZeroQ"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->calibrateZeroQ(request->joint_name()); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (calibrateZeroQ): success, id=" << device_id @@ -417,6 +520,11 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "clearFault"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->clearFault(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); return resultToStatus(result); @@ -426,4 +534,3 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, } } } // namespace cmvr::service - diff --git a/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp new file mode 100644 index 00000000..21e301d1 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp @@ -0,0 +1,1111 @@ +#include "service/grpc/include/grpc_arm_teleop_service.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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) + : backend_(std::move(backend)), + authority_( + authority ? authority + : &control::ControlAuthorityManager::instance()) +{ + if (!backend_) { + backend_ = makeDisabledArmTeleopBackend(); + } +} + +grpc::Status ArmTeleopServiceImpl::Teleoperate( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream) +{ + 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(); + const auto 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); + }); + + 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 = true; + 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"); + }); + + const auto 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); + } + + 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); + const auto 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/src/grpc_robot_arm_teleop_backend.cpp b/cmvr-es/service/grpc/src/grpc_robot_arm_teleop_backend.cpp new file mode 100644 index 00000000..d839718c --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_robot_arm_teleop_backend.cpp @@ -0,0 +1,649 @@ +#include "service/grpc/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/tests/grpc_arm_teleop_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp new file mode 100644 index 00000000..1bbdc818 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp @@ -0,0 +1,671 @@ +#include "service/grpc/include/grpc_arm_teleop_service.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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 TeleopServerHarness final { +public: + explicit TeleopServerHarness( + std::shared_ptr backend) + : service_(std::move(backend)) + { + 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); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp b/cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp new file mode 100644 index 00000000..7cc01f07 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp @@ -0,0 +1,525 @@ +#include "service/grpc/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/task/grpc_server_task/include/grpc_server_task.h b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h index 33bd0ebe..fb3efbcc 100644 --- a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h +++ b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h @@ -11,6 +11,10 @@ #include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" #include "task/task.h" +namespace cmvr::service { +class ArmTeleopBackend; +} + namespace cmvr::task { class GrpcServerTask final : public Task { @@ -55,6 +59,9 @@ 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::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 962d2434..46a0bf61 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 @@ -8,8 +8,12 @@ #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/include/grpc_agv_service.h" #include "service/grpc/include/grpc_arm_service.h" +#include "service/grpc/include/grpc_arm_teleop_service.h" +#include "service/grpc/include/grpc_robot_arm_teleop_backend.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" @@ -106,6 +110,10 @@ bool GrpcServerTask::start() dexhand_service_ = std::make_unique(); biohand_service_ = std::make_unique(); arm_service_ = std::make_unique(); + arm_teleop_service_ = + std::make_unique( + arm_teleop_backend_ ? arm_teleop_backend_ + : service::makeDisabledArmTeleopBackend()); motor_service_ = std::make_unique(); agv_service_ = std::make_unique(); hlc_service_ = std::make_unique(); @@ -119,6 +127,7 @@ 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()); @@ -152,6 +161,53 @@ bool GrpcServerTask::init() 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; @@ -258,6 +314,7 @@ void GrpcServerTask::clearServices() hlc_service_.reset(); agv_service_.reset(); motor_service_.reset(); + arm_teleop_service_.reset(); arm_service_.reset(); biohand_service_.reset(); dexhand_service_.reset(); diff --git a/cmvr-es/task/quic_edge_task/CMakeLists.txt b/cmvr-es/task/quic_edge_task/CMakeLists.txt index 009388df..48d66107 100644 --- a/cmvr-es/task/quic_edge_task/CMakeLists.txt +++ b/cmvr-es/task/quic_edge_task/CMakeLists.txt @@ -21,7 +21,14 @@ if(BUILD_TESTING) get_property(_quic_task_test_library_dirs DIRECTORY PROPERTY LINK_DIRECTORIES) list(PREPEND _quic_task_test_library_dirs "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime") list(JOIN _quic_task_test_library_dirs ":" _quic_task_test_library_path) + set(_quic_task_test_environment + "LD_LIBRARY_PATH=${_quic_task_test_library_path}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _quic_task_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() set_tests_properties(quic_edge_task_test PROPERTIES - ENVIRONMENT "LD_LIBRARY_PATH=${_quic_task_test_library_path}") + TIMEOUT 10 + ENVIRONMENT "${_quic_task_test_environment}") endif() endif() diff --git a/cmvr-es/task/ume_teleop_task/CMakeLists.txt b/cmvr-es/task/ume_teleop_task/CMakeLists.txt new file mode 100644 index 00000000..d624679f --- /dev/null +++ b/cmvr-es/task/ume_teleop_task/CMakeLists.txt @@ -0,0 +1,41 @@ +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 + 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 + 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 new file mode 100644 index 00000000..0c3da0ae --- /dev/null +++ b/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h @@ -0,0 +1,99 @@ +#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/arm_teleop_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; + + 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 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 new file mode 100644 index 00000000..314102af --- /dev/null +++ b/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp @@ -0,0 +1,651 @@ +#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 "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 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() +{ + std::unique_lock lock(mutex_); + if (state_ == TaskState::RUNNING) { + return true; + } + 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; + return true; +} + +bool UmeTeleopTask::step(const double dt) +{ + (void)dt; + return !isFailed(); +} + +void UmeTeleopTask::stop() +{ + 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(); + } + + std::lock_guard lock(mutex_); + if (state_ != TaskState::FAILED) { + state_ = TaskState::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 new file mode 100644 index 00000000..60ff3e1c --- /dev/null +++ b/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp @@ -0,0 +1,429 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "cmvr/api/arm_teleop_v1.grpc.pb.h" +#include "service/arm_teleop_client/include/grpc_arm_teleop_client.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; + 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; + 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; + } + condition_.notify_all(); + return expired + ? grpc::Status( + grpc::StatusCode::DEADLINE_EXCEEDED, + "test watchdog expired") + : 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 waitForHandlerFinish(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [this] { return handler_finished_; }); + } + + 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(); + } + + 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::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() +{ + { + 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); + const std::string socket_path = + "/tmp/cmvr_ume_teleop_task_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()); + 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() || !task.start()) { + server->Shutdown(); + return fail("valid task did not initialize and start"); + } + 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(); + task.stop(); + const auto stop_elapsed = std::chrono::steady_clock::now() - stop_begin; + if (stop_elapsed > 2s) { + server->Shutdown(); + return fail("Task stop did not TryCancel and join promptly"); + } + if (task.state() != cmvr::task::TaskState::STOPPED || + task.isBusy() || !task.isFinished()) { + server->Shutdown(); + return fail("task did not reach STOPPED after joining its worker"); + } + if (!service.waitForHandlerFinish(2s)) { + server->Shutdown(); + return fail("server handler did not observe Task cancellation"); + } + if (!service.stopReceived()) { + server->Shutdown(); + return fail("Task did not send StopSession before TryCancel"); + } + if (service.watchdogExpired() || !service.sequenceValid()) { + server->Shutdown(); + return fail("heartbeat/setpoint sequence or watchdog contract failed"); + } + + server->Shutdown(); + std::remove(socket_path.c_str()); + std::cout << "ume_teleop_task_test: PASS\n"; + return 0; +} diff --git a/docs/teleoperation/ume_cmvr_architecture.md b/docs/teleoperation/ume_cmvr_architecture.md new file mode 100644 index 00000000..b86f9c4f --- /dev/null +++ b/docs/teleoperation/ume_cmvr_architecture.md @@ -0,0 +1,111 @@ +# UME to CMVR-ES Teleoperation Architecture + +## Scope + +This document freezes the first implementation stage of the wired, +cross-machine teleoperation path: + +- both edge computers run `cmvr_es`; +- the UME computer owns the Damiao SocketCAN-FD interfaces; +- `UmeRobotArm` is a `RobotArm` backend; +- the UME computer performs the leader/follower model calculations; +- the robot computer validates and executes joint-servo references; +- the transport is a versioned gRPC bidirectional stream; +- the original UME algorithm is migrated before any SEW fusion work. + +SEW fusion, passivity research, paper experiments, and physical human testing +are deliberately outside this implementation stage. + +## Target process and ownership boundary + +```text +UME cmvr_es + UmeRobotArm + Damiao SocketCAN-FD + UmeHapticLoop + local safety guard + | + +-- UmeLegacyAlgorithm + | + UmeTeleopTask + GrpcArmTeleopClient + | + | wired Ethernet / gRPC bidi stream + v +Robot cmvr_es + ArmTeleopService + control lease + latest-only command slot + watchdog + safe servo executor + | + v + RobotArm +``` + +The UME high-frequency loop never performs a network RPC. Network workers and +hardware loops exchange only bounded latest-state snapshots. + +## Implemented boundary in this revision + +This revision establishes the device, algorithm-library, session-transport and +robot-backend boundaries, but it intentionally does not connect them into a +physical end-to-end controller: + +- `UmeRobotArm` owns one eight-axis Damiao bus and a bounded local torque loop. + It starts passive, requires an explicit fresh torque command before + `torqueOn`, and latches watchdog/transport faults. +- a successful SocketCAN send means that the complete frame batch was accepted + by the local kernel before its deadline. The current MIT transport has no + reviewed Disable acknowledgement, so software must not describe that result + as actuator-confirmed torque-off. +- the original UME dynamics, friction, stiction and haptic projection code is a + standalone tested library under `algorithms/controllers/ume_legacy`; +- `UmeTeleopTask` owns reconnect, heartbeat, sequence and a capacity-one + outbound mailbox. Its `submitSetpoint()` input is deliberately an algorithm + boundary; no production source calls it yet; +- the server-side `RobotArmTeleopBackend` is implemented and tested behind an + explicit capability gate. The current `MotorRobotArm` remains unavailable + because its `servoJ` path is sequential per joint rather than an accepted + atomic/timed group primitive; +- server state is currently returned with session events. The configured + requested state rate is not yet an independent periodic publisher; +- follower effort is validated and transported when a backend declares a + verified source, but it is not yet consumed by a UME haptic coordinator. + +Accordingly, this code is an M0-M9 fail-closed framework and original-algorithm +migration, not a claim of runnable force-feedback teleoperation. The next +integration step must add a concrete UME-to-follower retargeting producer, +connect verified follower effort to the local haptic coordinator, and retain +the high-frequency/network-thread separation above. + +## Safety invariants + +1. Opening a CAN interface never enables a motor. +2. Clearing a fault never arms a motor. +3. A reconnect creates a new session and never restores active motion. +4. Cross-machine `steady_clock` values are diagnostic only. A receiver derives + command expiry from its local receive time plus `valid_for_us`. +5. Commands are strictly increasing by sequence within one session. +6. Queues on the cyclic path are latest-only and bounded. +7. Invalid or stale follower effort ramps haptic feedback to zero. +8. Model, joint order, units, and calibration hashes must match before motion. +9. New physical hardware configurations remain disabled by default. +10. A software stop does not replace an independent physical emergency stop. +11. UME hardware enable also requires an explicit firmware-reviewed feedback + status whitelist and raw temperature thresholds; empty values never mean + "accept all". +12. UME shutdown timing is diagnosed against a configured budget, but physical + torque removal still requires an independent emergency-stop path until a + reviewed actuator Disable acknowledgement exists. + +## Initial rate boundary + +- UME local hardware/haptic loop: configurable, initially 800 Hz to match the + legacy UME setup. +- network command rate: configurable independently of the local loop. +- robot servo rate: selected from the robot backend capability and never + inferred from the network rate. + +No hard real-time or stability claim is made until the target computers and +physical devices have completed staged validation. diff --git a/docs/teleoperation/ume_cmvr_validation.md b/docs/teleoperation/ume_cmvr_validation.md new file mode 100644 index 00000000..8fb1cda2 --- /dev/null +++ b/docs/teleoperation/ume_cmvr_validation.md @@ -0,0 +1,118 @@ +# UME / CMVR-ES validation gates + +This checklist is part of the first UME migration. Passing a software gate +does not authorize a physical power-on. The checked-in UME device entries and +their `hardware_enabled` fields remain `false`. + +The current revision has no production setpoint producer for +`UmeTeleopTask::submitSetpoint()` and no haptic consumer for returned follower +effort. M9 therefore validates the framework, protocol and migrated original +algorithm separately; it is not an end-to-end motion or force-feedback test. + +## M9: software and network validation + +Run these gates on both target CPU architectures before deploying: + +1. Build the complete `cmvr_es` target with tests enabled. +2. Run the UME legacy controller and Pinocchio model-adapter golden tests. +3. Run the Damiao codec, CAN-FD chain, and `UmeRobotArm` lifecycle tests. +4. Run the process-wide control-authority tests. +5. Run the gRPC client, `UmeTeleopTask`, and `ArmTeleopService` tests. +6. Repeat the concurrent client/task/service tests to screen for shutdown and + reconnect races. +7. Start each checked-in edge profile without UME hardware and verify that it + never opens `can4`/`can5` or issues actuator enable frames. + +The communication tests must demonstrate all of the following: + +- an `OpenSession` manifest mismatch is rejected before backend activation; +- a second controller cannot acquire the same robot control resource; +- sequence numbers are non-zero and strictly increasing per session; +- the sender and receiver use capacity-one, latest-only command storage; +- a setpoint whose local validity has expired is never dispatched; +- heartbeat loss, lease loss, stream cancellation, and backend failure call + the robot safe-stop boundary; +- reconnect clears pending motion intent and starts a new sequence space; +- `StopSession` is attempted before client cancellation; +- the legacy unary ArmService cannot issue motion, enable, calibration, or + fault-reset commands while the teleoperation lease is active; +- `torqueOff` and `stopMotion` remain available as safety overrides. + +Before a real follower backend can be enabled, control authority must also be +extended to every local arm task and to the underlying MotorService resources. +The current process-wide lease covers ArmTeleop and unary ArmService only. + +For a wired two-computer run, record at least: + +- one-way command age at the robot ingress; +- command mailbox overwrite and rejection counters; +- heartbeat and control-lease remaining time; +- servo apply duration and deadline misses; +- disconnect detection-to-safe-stop time; +- packet loss, reordering, and delay from an explicit network impairment + profile rather than an assumed LAN condition. + +No end-to-end stability or transparency claim is supported until those logs +are tied to a specified controller rate, robot servo period, payload, motion +envelope, and network impairment profile. + +## M10: staged physical commissioning + +Every row is a separate sign-off. Do not combine first power-on with a human +wearing the UME. + +- [ ] Independent physical emergency stop is installed and verified. +- [ ] CAN arbitration/data bitrates and CAN-FD+BRS MTU are verified for each + interface. +- [ ] Motor product, firmware, command ID, feedback ID, and reported motor ID + are read back and matched to the configuration. +- [ ] The four-bit Damiao feedback status meanings and raw temperature limits + are verified for the exact product/firmware and entered as an explicit + per-joint whitelist/threshold contract. +- [ ] Joint direction and zero reference are verified one joint at a time with + the mechanism unloaded. +- [ ] Mechanical position, velocity, and torque limits replace the checked-in + placeholders and receive an independent review. +- [ ] Motor feedback timestamps and the 800 Hz cycle are measured on the UME + target computer under load. +- [ ] The exact Damiao firmware's Disable acknowledgement semantics are + documented and verified. Until then, SocketCAN send success is only + evidence that the local kernel accepted the complete frame batch. +- [ ] Because MIT feedback has no sequence field, stale request/reply + correlation is resolved by a reviewed firmware marker or by measured, + enforced bus timing; draining only the frames already queued is not + sufficient evidence. +- [ ] Every UME control-thread I/O operation is shown to be deadline-bounded + and interruptible on the target kernel. The configured shutdown timeout + is currently a diagnostic failure threshold, not a C++ timed-join + primitive. +- [ ] With torque disabled, both edge profiles run for at least 30 minutes + without sequence, deadline, lease, or reconnect anomalies. +- [ ] With the UME fixed to a stand, each joint is enabled independently at a + low torque limit and its watchdog disable path is measured. +- [ ] Both UME arms are tested together on stands; CAN and CPU deadline margins + are recorded. +- [ ] The robot backend's group `servoJ` semantics and worst-case call duration + are measured. Sequential per-joint dispatch is not accepted as an + atomic group backend without a documented skew bound. +- [ ] gRPC writer backpressure cannot stall the independent robot watchdog, + and an expired lease cannot be regranted until safe stop is confirmed. +- [ ] TouchScreenTask and direct MotorService commands are either disabled by + the deployment profile or participate in the same resource authority. +- [ ] Robot-only low-speed setpoint execution is validated before connecting + the leader-side algorithm. +- [ ] Wired-network cable removal, peer process kill, delayed packets, stale + commands, duplicate sequences, lease theft, and robot fault injection + all lead to a bounded safe stop. +- [ ] The original UME gravity/friction/feedback controller is commissioned on + a stand with force feedback initially clamped to zero, then increased in + reviewed steps. +- [ ] Only after all previous evidence is archived may a supervised human test + be considered under a separate risk assessment. + +## Evidence record + +For each completed physical gate, archive the exact Git revision, installed +`output/` checksum, configuration files, model and calibration SHA-256 values, +test operator, hardware serial numbers, raw logs, and pass/fail decision. A +successful build or simulator run must not be recorded as physical validation. diff --git a/model/ume/README.md b/model/ume/README.md new file mode 100644 index 00000000..c7ac5076 --- /dev/null +++ b/model/ume/README.md @@ -0,0 +1,28 @@ +# 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 new file mode 100644 index 00000000..de84254c --- /dev/null +++ b/model/ume/v6_bimanual/robot.xml @@ -0,0 +1,356 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/ume/v6_imu/robot.xml b/model/ume/v6_imu/robot.xml new file mode 100644 index 00000000..684292c4 --- /dev/null +++ b/model/ume/v6_imu/robot.xml @@ -0,0 +1,389 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/protos/cmvr/api/arm_teleop_v1.proto b/protos/cmvr/api/arm_teleop_v1.proto new file mode 100644 index 00000000..d7095a95 --- /dev/null +++ b/protos/cmvr/api/arm_teleop_v1.proto @@ -0,0 +1,133 @@ +syntax = "proto3"; + +package cmvr.api.armteleop.v1; + +// Versioned, session-oriented protocol for wired arm teleoperation. Existing +// unary ArmService RPCs intentionally remain unchanged. +service ArmTeleopService { + rpc Teleoperate(stream ClientFrame) returns (stream ServerFrame); +} + +enum SessionPhase { + SESSION_PHASE_UNSPECIFIED = 0; + SESSION_PHASE_OPENED = 1; + SESSION_PHASE_READY = 2; + SESSION_PHASE_ACTIVE = 3; + SESSION_PHASE_HOLDING = 4; + SESSION_PHASE_STOPPED = 5; + SESSION_PHASE_WATCHDOG_EXPIRED = 6; + SESSION_PHASE_LEASE_LOST = 7; + SESSION_PHASE_REJECTED = 8; + SESSION_PHASE_FAILED = 9; +} + +enum StopReason { + STOP_REASON_UNSPECIFIED = 0; + STOP_REASON_OPERATOR_REQUEST = 1; + STOP_REASON_CLIENT_SHUTDOWN = 2; + STOP_REASON_WATCHDOG = 3; + STOP_REASON_LEASE_REVOKED = 4; + STOP_REASON_ROBOT_FAULT = 5; + STOP_REASON_EMERGENCY_STOP = 6; + STOP_REASON_PROTOCOL_ERROR = 7; +} + +enum EffortSource { + EFFORT_SOURCE_UNSPECIFIED = 0; + EFFORT_SOURCE_MOTOR_ESTIMATE = 1; + EFFORT_SOURCE_JOINT_SENSOR = 2; + EFFORT_SOURCE_FORCE_TORQUE_SENSOR = 3; + EFFORT_SOURCE_OBSERVER = 4; +} + +message RobotManifest { + string robot_id = 1; + string model_sha256 = 2; + string calibration_sha256 = 3; + repeated string joint_names = 4; + string position_unit = 5; + string velocity_unit = 6; + string effort_unit = 7; + string base_frame = 8; + string tool_frame = 9; +} + +message OpenSession { + uint32 protocol_major = 1; + uint32 protocol_minor = 2; + string client_instance_id = 3; + RobotManifest expected_robot = 4; + uint32 requested_command_rate_hz = 5; + uint32 requested_state_rate_hz = 6; + uint32 watchdog_timeout_ms = 7; + uint32 requested_lease_ms = 8; + bool request_force_feedback = 9; +} + +message JointSetpoint { + // Strictly increasing and non-zero within a session. + uint64 sequence = 1; + repeated double position_rad = 2; + repeated double velocity_rad_s = 3; + // The receiver computes its deadline from local arrival time plus this + // duration. Zero is invalid for an active setpoint. + uint32 valid_for_us = 4; +} + +message ClientHeartbeat { + uint64 sequence = 1; +} + +message StopSession { + StopReason reason = 1; + string detail = 2; +} + +message ClientFrame { + oneof payload { + OpenSession open = 1; + JointSetpoint setpoint = 2; + ClientHeartbeat heartbeat = 3; + StopSession stop = 4; + } +} + +message JointState { + uint64 sample_sequence = 1; + repeated double position_rad = 2; + repeated double velocity_rad_s = 3; + repeated double effort_nm = 4; + bool position_valid = 5; + bool velocity_valid = 6; + bool effort_valid = 7; + EffortSource effort_source = 8; + uint64 sample_age_us = 9; +} + +message SessionStatus { + string session_id = 1; + SessionPhase phase = 2; + uint64 received_sequence = 3; + uint64 applied_sequence = 4; + uint64 dropped_setpoints = 5; + uint64 rejected_setpoints = 6; + uint32 negotiated_watchdog_ms = 7; + uint32 lease_remaining_ms = 8; + StopReason stop_reason = 9; + string detail = 10; +} + +message RobotSafetyState { + bool connected = 1; + bool powered_on = 2; + bool protective_stopped = 3; + bool emergency_stopped = 4; + bool fault = 5; + string fault_detail = 6; +} + +message ServerFrame { + SessionStatus status = 1; + JointState joint_state = 2; + RobotSafetyState safety = 3; +} diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto index 06c0c33b..a97be752 100644 --- a/protos/cmvr/config/arm_config/arm_config.proto +++ b/protos/cmvr/config/arm_config/arm_config.proto @@ -6,6 +6,7 @@ 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; @@ -24,6 +25,10 @@ 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 { @@ -45,6 +50,57 @@ message VendorRobotArmBackendConfig { string password = 10; } +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 { double linear_velocity_max = 1; double linear_acceleration_max = 2; @@ -133,6 +189,7 @@ message RobotArmConfig { oneof backend { MotorRobotArmBackendConfig motor = 10; VendorRobotArmBackendConfig vendor = 11; + UmeRobotArmBackendConfig ume = 12; } ArmKinematicsConfig kinematics = 20; diff --git a/protos/cmvr/config/grpc_server_config/grpc_server_config.proto b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto index 0650c735..927cb7fc 100644 --- a/protos/cmvr/config/grpc_server_config/grpc_server_config.proto +++ b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto @@ -1,6 +1,29 @@ 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 GRPCServerConfig { string host = 1; string port = 2; @@ -12,6 +35,7 @@ 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; } 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 0612f821..6ec4ec3b 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -45,6 +45,21 @@ 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 { diff --git a/protos/cmvr/config/task_manager_config/task_manager_config.proto b/protos/cmvr/config/task_manager_config/task_manager_config.proto index 8c60e370..cf6479e5 100644 --- a/protos/cmvr/config/task_manager_config/task_manager_config.proto +++ b/protos/cmvr/config/task_manager_config/task_manager_config.proto @@ -8,6 +8,7 @@ message TaskConfigEntry { TASK_TYPE_GRPC_SERVER = 3; TASK_TYPE_SELF_COLLISION = 4; TASK_TYPE_QUIC_EDGE = 5; + TASK_TYPE_UME_TELEOP = 6; } enum TaskRunMode { diff --git a/protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto b/protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto new file mode 100644 index 00000000..fd2a3366 --- /dev/null +++ b/protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto @@ -0,0 +1,30 @@ +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; +}