SmartBench/cmvr-es
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

revert xtkuang integration before linbo hardware changes

Browse Source
This commit is contained in:
linbo 2026-08-24 15:23:27 +08:00
parent 3214a6a3a6
commit c4001d57ab
303 changed files with 6724 additions and 84704 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
CMakeLists.txt
View File

@ -9,29 +9,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
#set(CMAKE_CXX_STANDARD_REQUIRED True)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# Preserve the project's production-build behavior: tests are opt-in via
# -DBUILD_TESTING=ON, while still registering them with CTest when requested.
option(BUILD_TESTING "Build the test targets" OFF)
include(CTest)
if(BUILD_TESTING AND UNIX AND NOT APPLE)
# Test executables can still inherit the AUBO imported target's build-tree
# RUNPATH. Keep the active toolchain runtime ahead of that vendor path.
execute_process(
COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so.6
OUTPUT_VARIABLE CMVR_TEST_SYSTEM_LIBSTDCXX
OUTPUT_STRIP_TRAILING_WHITESPACE
)
if(EXISTS "${CMVR_TEST_SYSTEM_LIBSTDCXX}")
get_filename_component(
CMVR_TEST_SYSTEM_LIBSTDCXX
"${CMVR_TEST_SYSTEM_LIBSTDCXX}"
REALPATH
)
else()
unset(CMVR_TEST_SYSTEM_LIBSTDCXX)
endif()
endif()
# Install to <source>/output
set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE)
@ -69,22 +47,7 @@ install(
DIRECTORY "${INTEL_MEDIA_DRIVER_DIR}/"
DESTINATION lib/dri
)
install(CODE [=[
find_program(CMVR_PATCHELF_EXECUTABLE patchelf REQUIRED)
set(_cmvr_ihd_driver
"${CMAKE_INSTALL_PREFIX}/lib/dri/iHD_drv_video.so"
)
execute_process(
COMMAND "${CMVR_PATCHELF_EXECUTABLE}"
--set-rpath "$ORIGIN/.."
"${_cmvr_ihd_driver}"
COMMAND_ERROR_IS_FATAL ANY
)
]=])
if(BUILD_TESTING AND CMVR_EXTERNAL_LIBRARY_DIRS)
list(JOIN CMVR_EXTERNAL_LIBRARY_DIRS ":" CMVR_TEST_EXTERNAL_LIBRARY_PATH)
endif()
# 在调用 setup_external_libs 之后
message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}")
message(STATUS "CMAKE_SHARED_LINKER_FLAGS: ${CMAKE_SHARED_LINKER_FLAGS}")
@ -102,9 +65,8 @@ file(GLOB_RECURSE PROTO_FILES ${PROTO_IMPORT_DIR}/*.proto)
set(Protobuf_PROTOC_EXECUTABLE "${CMAKE_INSTALL_PREFIX}/bin/protoc" CACHE FILEPATH "" FORCE)
set_target_properties(gRPC::grpc_cpp_plugin PROPERTIES
IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin"
IMPORTED_LOCATION_RELEASE "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin"
set_property(TARGET gRPC::grpc_cpp_plugin
PROPERTY IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin"
)
# 1) 先做 OBJECT：只负责生成/编译 pb.cc

11
README.md
View File

@ -138,14 +138,3 @@ $IGH_ETHERCAT_ROOT/bin/ethercat pdos
sudo script/ethercat/stop_ethercat.sh eno1
sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
```
## 组件文档
具体能力、配置、协议和安全边界由对应代码目录下的 README 维护：
- [MotorService gRPC 接口](cmvr-es/service/README.md#motorservice)
- [电机设备模块](cmvr-es/devices/motor/README.md)
- [AUBO 控制柜 Standard 数字 IO](cmvr-es/devices/arm/aubo_arm/README.md)
- [配置与部署规则](cmvr-es/config/README.md)
AUBO JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。

1
assets/toppra Submodule

@ -0,0 +1 @@
Subproject commit 3089c7897a5711aceb39d25919aca8c57b5c5948

1
cmake/FindExternalLib.cmake
View File

@ -139,7 +139,6 @@ function(setup_external_libs ARCH)
list(REMOVE_DUPLICATES LIBRARY_DIRS)
link_directories(${LIBRARY_DIRS})
endif()
set(CMVR_EXTERNAL_LIBRARY_DIRS "${LIBRARY_DIRS}" PARENT_SCOPE)
# ---- install third-party shared libs into <prefix>/lib ----
if(INSTALL_SO_FILES)

7
cmvr-es/CMakeLists.txt
View File

@ -6,16 +6,11 @@ add_subdirectory(hardware)
add_subdirectory(algorithms)
add_subdirectory(simulate)
add_subdirectory(devices)
add_subdirectory(manager/control_authority_manager)
add_subdirectory(manager/safety_manager)
add_subdirectory(manager/device_manager)
add_subdirectory(service/grpc/stop_all)
add_subdirectory(manager/media_source_manager)
add_subdirectory(manager/media_source_hub)
add_subdirectory(service/quic_edge)
add_subdirectory(task)
add_subdirectory(task/quic_edge_task)
add_subdirectory(service/grpc/client)
add_subdirectory(task/ume_teleop_task)
add_subdirectory(manager/task_manager)
add_subdirectory(service)
add_subdirectory(runtime)

1
cmvr-es/algorithms/controllers/CMakeLists.txt
View File

@ -1,6 +1,5 @@
add_subdirectory(arm_control)
add_subdirectory(ume_legacy)
#find_package(VISP REQUIRED)

24
cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h
View File

@ -52,16 +52,8 @@ public:
private:
void ensureWorkerStarted_();
void workerLoop_(std::uint64_t worker_generation);
bool workerGenerationCurrent_(std::uint64_t worker_generation) const;
std::optional<Result> sendVelocityIfCurrent_(
const JointVelocityCommand& velocity,
double acceleration,
std::uint64_t worker_generation);
void finishCommandIfCurrent_(std::uint64_t command_version,
std::uint64_t worker_generation);
void sendZeroIfCurrent_(std::uint64_t worker_generation);
void sendZeroNow_();
void workerLoop_();
void sendZero_();
static double velocityNorm_(const std::vector<double>& velocity);
static double twistNorm_(const CartesianVelocity& velocity);
@ -73,25 +65,15 @@ private:
ReadStateCallback read_state_;
SendVelocityCallback send_velocity_;
// lifecycle_mutex_ serializes worker creation, join, and reset. It is held
// across join so a new command cannot start until the retired worker exits.
mutable std::mutex lifecycle_mutex_;
std::unique_ptr<std::thread> worker_;
std::atomic<bool> worker_running_{false};
mutable std::mutex mutex_;
std::condition_variable cv_;
bool stop_requested_{false};
std::atomic<bool> stop_requested_{false};
bool command_active_{false};
CartesianVelocity target_twist_{};
FrameType target_frame_{FrameType::Base};
double target_acceleration_{0.25};
std::uint64_t command_version_{0};
// Every worker output is checked while holding output_mutex_. shutdown()
// advances the generation before sending zero, fencing stale worker writes.
mutable std::mutex output_mutex_;
std::atomic<std::uint64_t> worker_generation_{0};
std::atomic<bool> busy_{false};
};

216
cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp
View File

@ -61,23 +61,20 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
}
if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) {
return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
}
ensureWorkerStarted_();
std::uint64_t command_version = 0;
{
std::lock_guard<std::mutex> lifecycle_lock(lifecycle_mutex_);
if (worker_ && worker_->joinable() && !worker_running_.load()) {
worker_->join();
worker_.reset();
}
ensureWorkerStarted_();
{
std::lock_guard<std::mutex> lock(mutex_);
target_twist_ = velocity;
target_acceleration_ = acceleration;
target_frame_ = frame;
command_active_ = true;
command_version = ++command_version_;
}
busy_.store(true);
std::lock_guard<std::mutex> lock(mutex_);
target_twist_ = velocity;
target_acceleration_ = acceleration;
target_frame_ = frame;
command_active_ = true;
command_version = ++command_version_;
}
cv_.notify_all();
@ -103,21 +100,16 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
Result CartesianVelocityController::stop(const std::optional<double> acceleration)
{
if (!worker_ || !worker_->joinable()) {
return Result::success();
}
{
std::lock_guard<std::mutex> lifecycle_lock(lifecycle_mutex_);
if (!worker_ || !worker_->joinable() || !worker_running_.load()) {
return Result::success();
}
{
std::lock_guard<std::mutex> lock(mutex_);
target_twist_ = {};
target_frame_ = FrameType::Base;
target_acceleration_ =
acceleration.has_value() ? *acceleration
: config_.stop_acceleration;
command_active_ = true;
++command_version_;
}
std::lock_guard<std::mutex> lock(mutex_);
target_twist_ = {};
target_frame_ = FrameType::Base;
target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration;
command_active_ = true;
++command_version_;
}
cv_.notify_all();
return Result::success();
@ -125,35 +117,22 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
void CartesianVelocityController::shutdown()
{
std::lock_guard<std::mutex> lifecycle_lock(lifecycle_mutex_);
if (!worker_ || !worker_->joinable()) {
busy_.store(false);
return;
}
// Revoke the worker before issuing zero. All worker outputs perform their
// final generation check under output_mutex_, so none can follow this zero.
worker_generation_.fetch_add(1, std::memory_order_acq_rel);
{
std::lock_guard<std::mutex> lock(mutex_);
stop_requested_ = true;
stop_requested_.store(true);
command_active_ = false;
target_twist_ = {};
target_frame_ = FrameType::Base;
++command_version_;
}
cv_.notify_all();
sendZeroNow_();
busy_.store(false);
worker_->join();
worker_.reset();
{
std::lock_guard<std::mutex> lock(mutex_);
stop_requested_ = false;
command_active_ = false;
}
stop_requested_.store(false);
busy_.store(false);
}
CartesianVelocity CartesianVelocityController::getCommandTwistBase() const
@ -169,26 +148,12 @@ void CartesianVelocityController::ensureWorkerStarted_()
if (worker_ && worker_->joinable()) {
return;
}
const auto worker_generation =
worker_generation_.fetch_add(1, std::memory_order_acq_rel) + 1;
{
std::lock_guard<std::mutex> lock(mutex_);
stop_requested_ = false;
command_active_ = false;
}
worker_running_.store(true);
worker_ = std::make_unique<std::thread>(
&CartesianVelocityController::workerLoop_, this, worker_generation);
stop_requested_.store(false);
worker_ = std::make_unique<std::thread>(&CartesianVelocityController::workerLoop_, this);
}
void CartesianVelocityController::workerLoop_(
const std::uint64_t worker_generation)
void CartesianVelocityController::workerLoop_()
{
struct RunningGuard {
std::atomic<bool>& running;
~RunningGuard() { running.store(false); }
} running_guard{worker_running_};
const double dt = config_.control_period_s;
auto next_tick = std::chrono::steady_clock::now();
@ -199,10 +164,9 @@ void CartesianVelocityController::workerLoop_(
{
std::unique_lock<std::mutex> lock(mutex_);
cv_.wait(lock, [&]() {
return stop_requested_ || command_active_;
return stop_requested_.load() || command_active_;
});
if (stop_requested_ ||
!workerGenerationCurrent_(worker_generation)) {
if (stop_requested_.load()) {
break;
}
target_twist = target_twist_;
@ -212,44 +176,43 @@ void CartesianVelocityController::workerLoop_(
next_tick = std::chrono::steady_clock::now();
while (true) {
bool stopping = false;
std::uint64_t active_command_version = 0;
{
std::lock_guard<std::mutex> lock(mutex_);
stopping = stop_requested_;
if (stop_requested_.load()) {
sendZero_();
busy_.store(false);
return;
}
if (!command_active_) {
break;
}
target_twist = target_twist_;
acceleration = target_acceleration_;
target_frame = target_frame_;
active_command_version = command_version_;
}
if (stopping ||
!workerGenerationCurrent_(worker_generation)) {
return;
}
if (!planner_->updateSpeedLAcceleration(acceleration)) {
if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) {
finishCommandIfCurrent_(
active_command_version, worker_generation);
std::lock_guard<std::mutex> lock(mutex_);
command_active_ = false;
sendZero_();
busy_.store(false);
break;
}
CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
<< acceleration;
finishCommandIfCurrent_(
active_command_version, worker_generation);
break;
sendZero_();
busy_.store(false);
return;
}
std::vector<double> q_now;
std::vector<double> qd_now;
if (!read_state_(q_now, qd_now)) {
CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed";
finishCommandIfCurrent_(
active_command_version, worker_generation);
break;
sendZero_();
busy_.store(false);
return;
}
std::vector<double> qd_cmd;
@ -259,31 +222,31 @@ void CartesianVelocityController::workerLoop_(
<< target_twist.vz << ", " << target_twist.wx << ", "
<< target_twist.wy << ", " << target_twist.wz
<< "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base");
finishCommandIfCurrent_(
active_command_version, worker_generation);
break;
sendZero_();
busy_.store(false);
return;
}
JointVelocityCommand velocity_command;
velocity_command.velocity = qd_cmd;
const auto send_result = sendVelocityIfCurrent_(
velocity_command, acceleration, worker_generation);
if (!send_result.has_value()) {
return;
}
if (!send_result->ok()) {
const auto send_result = send_velocity_(velocity_command, acceleration);
if (!send_result.ok()) {
CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
<< send_result->message;
finishCommandIfCurrent_(
active_command_version, worker_generation);
break;
<< send_result.message;
sendZero_();
busy_.store(false);
return;
}
if (twistNorm_(target_twist) < config_.stop_twist_norm &&
velocityNorm_(qd_cmd) < config_.stop_command_velocity_norm &&
velocityNorm_(qd_now) < config_.stop_measured_velocity_norm) {
finishCommandIfCurrent_(
active_command_version, worker_generation);
{
std::lock_guard<std::mutex> lock(mutex_);
command_active_ = false;
}
sendZero_();
busy_.store(false);
break;
}
@ -293,67 +256,12 @@ void CartesianVelocityController::workerLoop_(
}
}
sendZeroIfCurrent_(worker_generation);
if (workerGenerationCurrent_(worker_generation)) {
busy_.store(false);
}
sendZero_();
busy_.store(false);
}
bool CartesianVelocityController::workerGenerationCurrent_(
const std::uint64_t worker_generation) const
void CartesianVelocityController::sendZero_()
{
return worker_generation_.load(std::memory_order_acquire) ==
worker_generation;
}
std::optional<Result> CartesianVelocityController::sendVelocityIfCurrent_(
const JointVelocityCommand& velocity,
const double acceleration,
const std::uint64_t worker_generation)
{
std::lock_guard<std::mutex> lock(output_mutex_);
if (!workerGenerationCurrent_(worker_generation)) {
return std::nullopt;
}
return send_velocity_(velocity, acceleration);
}
void CartesianVelocityController::finishCommandIfCurrent_(
const std::uint64_t command_version,
const std::uint64_t worker_generation)
{
std::lock_guard<std::mutex> output_lock(output_mutex_);
{
std::lock_guard<std::mutex> lock(mutex_);
if (command_version_ != command_version) {
return;
}
command_active_ = false;
busy_.store(false);
}
if (!workerGenerationCurrent_(worker_generation)) {
return;
}
JointVelocityCommand zero;
zero.velocity.assign(dof_, 0.0);
(void)send_velocity_(zero, 0.0);
}
void CartesianVelocityController::sendZeroIfCurrent_(
const std::uint64_t worker_generation)
{
std::lock_guard<std::mutex> lock(output_mutex_);
if (!workerGenerationCurrent_(worker_generation) || !send_velocity_) {
return;
}
JointVelocityCommand zero;
zero.velocity.assign(dof_, 0.0);
(void)send_velocity_(zero, 0.0);
}
void CartesianVelocityController::sendZeroNow_()
{
std::lock_guard<std::mutex> lock(output_mutex_);
if (!send_velocity_) {
return;
}

78
cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt
View File

@ -1,78 +0,0 @@
add_library(ume_legacy_controller SHARED
src/ume_legacy_controller.cpp
src/pinocchio_ume_legacy_model_adapter.cpp
)
target_include_directories(ume_legacy_controller
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/include
)
target_link_libraries(ume_legacy_controller
PRIVATE
pinocchio_default
pinocchio_parsers
)
add_library(
cmvr_es::algorithms::ume_legacy
ALIAS ume_legacy_controller
)
install(TARGETS ume_legacy_controller LIBRARY DESTINATION lib)
if(BUILD_TESTING)
add_executable(ume_legacy_controller_golden_test
tests/ume_legacy_controller_golden_test.cpp
)
add_executable(pinocchio_ume_legacy_model_adapter_test
tests/pinocchio_ume_legacy_model_adapter_test.cpp
)
target_link_libraries(ume_legacy_controller_golden_test
PRIVATE
cmvr_es::algorithms::ume_legacy
gtest
gtest_main
pthread
)
target_link_libraries(pinocchio_ume_legacy_model_adapter_test
PRIVATE
cmvr_es::algorithms::ume_legacy
gtest
gtest_main
pthread
)
foreach(_ume_legacy_test_target
ume_legacy_controller_golden_test
pinocchio_ume_legacy_model_adapter_test)
target_compile_definitions(${_ume_legacy_test_target}
PRIVATE
CMVR_UME_FIXED_MODEL_PATH="${CMAKE_SOURCE_DIR}/model/ume/v6_bimanual/robot.xml"
CMVR_UME_FLOATING_MODEL_PATH="${CMAKE_SOURCE_DIR}/model/ume/v6_imu/robot.xml"
)
add_test(
NAME ${_ume_legacy_test_target}
COMMAND ${_ume_legacy_test_target}
)
endforeach()
set(_ume_legacy_test_environment
"LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
)
if(CMVR_TEST_SYSTEM_LIBSTDCXX)
list(APPEND _ume_legacy_test_environment
"LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
endif()
foreach(_ume_legacy_test_target
ume_legacy_controller_golden_test
pinocchio_ume_legacy_model_adapter_test)
set_tests_properties(${_ume_legacy_test_target} PROPERTIES
TIMEOUT 10
ENVIRONMENT "${_ume_legacy_test_environment}"
)
endforeach()
endif()

72
cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h
View File

@ -1,72 +0,0 @@
#ifndef CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H
#define CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H
#include <cstddef>
#include <memory>
#include <string>
#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> impl_;
};
} // namespace cmvr::ume_legacy
#endif // CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H

34
cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h
View File

@ -1,34 +0,0 @@
#ifndef CMVR_ES_UME_LEGACY_CONTROLLER_H
#define CMVR_ES_UME_LEGACY_CONTROLLER_H
#include "ume_legacy_types.h"
namespace cmvr::ume_legacy {
// Constants from UME commit e087df5cd3b281418722e155d9975695f163698e:
// ume/robot/ume/v6_imu/ume_leader/controller.py
// ume/robot/openarm1/teleop_leader_tuning.py
LegacyUmeTuning originalTuning() noexcept;
JointVector frictionCompensation(
const JointVector& velocity_rad_s,
const LegacyUmeTuning& tuning) noexcept;
JointVector stictionCompensation(
const JointVector& velocity_rad_s,
const LegacyUmeTuning& tuning) noexcept;
// error_norm is non-negative in the legacy path because it is produced by
// np.linalg.norm. std::abs is retained here to match the subsequent Python
// expression exactly for direct unit-level use.
double feedbackScale(
double error_norm,
const LegacyUmeTuning& tuning) noexcept;
SideControlOutput computeSideCommand(
const SideControlInput& input,
const LegacyUmeTuning& tuning = originalTuning()) noexcept;
} // namespace cmvr::ume_legacy
#endif // CMVR_ES_UME_LEGACY_CONTROLLER_H

32
cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h
View File

@ -1,32 +0,0 @@
#ifndef CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H
#define CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H
#include "ume_legacy_types.h"
namespace cmvr::ume_legacy {
// Boundary for the two model operations used by the original IMU controller:
// 1. floating-base RNEA gravity compensation;
// 2. fixed-base J_rot^T projection of shoulder/wrist moments.
//
// Concrete implementations must load and validate their model contract so the
// pure controller cannot silently substitute guessed kinematics or dynamics.
class UmeLegacyModelAdapter {
public:
virtual ~UmeLegacyModelAdapter() = default;
virtual bool computeGravityCompensation(
const BimanualModelState& state,
JointVector& right_gravity_nm,
JointVector& left_gravity_nm) const = 0;
virtual bool projectHapticFeedback(
const BimanualModelState& state,
ArmSide side,
const RawHapticFeedback& feedback,
ProjectedHapticEffort& projected) const = 0;
};
} // namespace cmvr::ume_legacy
#endif // CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H

110
cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h
View File

@ -1,110 +0,0 @@
#ifndef CMVR_ES_UME_LEGACY_TYPES_H
#define CMVR_ES_UME_LEGACY_TYPES_H
#include <array>
#include <cstddef>
namespace cmvr::ume_legacy {
inline constexpr std::size_t kArmDof = 8;
inline constexpr std::size_t kTransformElementCount = 16;
using JointVector = std::array<double, kArmDof>;
using Vector3 = std::array<double, 3>;
using Transform4x4RowMajor =
std::array<double, kTransformElementCount>;
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

585
cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp
View File

@ -1,585 +0,0 @@
#include "pinocchio_ume_legacy_model_adapter.h"
#include <array>
#include <cmath>
#include <sstream>
#include <stdexcept>
#include <utility>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/mjcf.hpp>
namespace cmvr::ume_legacy {
namespace {
using ExpectedArmJointNames = std::array<std::string, 16>;
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<pinocchio::FrameIndex, 4>& 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<pinocchio::JointIndex>(index + 1),
names[index],
static_cast<int>(index),
1,
static_cast<int>(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<pinocchio::JointIndex>(index + 2),
names[index],
static_cast<int>(index + 7),
1,
static_cast<int>(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<std::size_t>(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<std::size_t>(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<pinocchio::Data>(fixed_model);
floating_data =
std::make_unique<pinocchio::Data>(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<double, 6, Eigen::Dynamic>::Zero(
6, fixed_model.nv);
wrist_jacobian =
Eigen::Matrix<double, 6, Eigen::Dynamic>::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<std::size_t>(fixed_model.nq);
contract_info.fixed_nv =
static_cast<std::size_t>(fixed_model.nv);
contract_info.floating_nq =
static_cast<std::size_t>(floating_model.nq);
contract_info.floating_nv =
static_cast<std::size_t>(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<pinocchio::Data> fixed_data;
std::unique_ptr<pinocchio::Data> floating_data;
std::array<pinocchio::FrameIndex, 4> 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<double, 6, Eigen::Dynamic> shoulder_jacobian;
Eigen::Matrix<double, 6, Eigen::Dynamic> wrist_jacobian;
};
PinocchioUmeLegacyModelAdapter::PinocchioUmeLegacyModelAdapter(
std::string fixed_model_path,
std::string floating_model_path)
: impl_(std::make_unique<Impl>(
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<Eigen::Index>(7 + index)] =
state.right_position_rad[index];
q[static_cast<Eigen::Index>(15 + index)] =
state.left_position_rad[index];
velocity[static_cast<Eigen::Index>(6 + index)] =
state.right_velocity_rad_s[index];
velocity[static_cast<Eigen::Index>(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<Eigen::Index>(6 + index)];
left_gravity_nm[index] =
torque[static_cast<Eigen::Index>(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<Eigen::Index>(index)] =
state.right_position_rad[index];
q[static_cast<Eigen::Index>(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

193
cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp
View File

@ -1,193 +0,0 @@
#include "ume_legacy_controller.h"
#include <cmath>
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

392
cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp
View File

@ -1,392 +0,0 @@
#include "pinocchio_ume_legacy_model_adapter.h"
#include <array>
#include <cmath>
#include <cstddef>
#include <limits>
#include <stdexcept>
#include <string>
#include <gtest/gtest.h>
#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 <std::size_t Size>
void expectFinite(const std::array<double, Size>& values)
{
for (std::size_t index = 0; index < Size; ++index) {
EXPECT_TRUE(std::isfinite(values[index]))
<< "index " << index;
}
}
template <std::size_t Size>
void expectNear(
const std::array<double, Size>& actual,
const std::array<double, Size>& 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<double>::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<double>::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<ArmSide>(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

216
cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp
View File

@ -1,216 +0,0 @@
#include "ume_legacy_controller.h"
#include "ume_legacy_model_adapter.h"
#include <array>
#include <cmath>
#include <cstddef>
#include <limits>
#include <gtest/gtest.h>
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<double>::infinity()),
std::nextafter(maximum, 0.0),
2.0 * minimum,
-2.0 * minimum,
std::nextafter(
minimum, std::numeric_limits<double>::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

2
cmvr-es/common/README.md
View File

@ -54,7 +54,7 @@ AGV 通用类型应参考 [`types/agv/agv_types.h`](types/agv/agv_types.h)，机
- AAC、Opus、PCM 明确 payload format、采样率和声道数；
- 不把 QUIC、gRPC 或浏览器专有字段加入通用帧。
设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceManager 章节。
设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceHub 章节。
## 环形队列选择

23
cmvr-es/common/base/logging/CMakeLists.txt
View File

@ -13,26 +13,3 @@ target_link_libraries(logging PUBLIC
add_library(cmvr_es::logging ALIAS logging)
install(TARGETS logging ARCHIVE DESTINATION lib)
if(BUILD_TESTING)
add_executable(logger_test
tests/logger_test.cpp
)
target_link_libraries(logger_test PRIVATE
cmvr_es::logging
gtest
gtest_main
pthread
)
add_test(NAME logger_test COMMAND logger_test)
set(_logger_test_environment
"LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}")
if(CMVR_TEST_SYSTEM_LIBSTDCXX)
list(APPEND _logger_test_environment
"LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
endif()
set_tests_properties(logger_test PROPERTIES
TIMEOUT 10
ENVIRONMENT "${_logger_test_environment}"
)
endif()

12
cmvr-es/common/base/logging/logger.cpp
View File

@ -167,15 +167,6 @@ void Logger::shutdown()
initialized_ = false;
}
void Logger::flush()
{
std::lock_guard<std::mutex> lock(mutex_);
if (log_file_.is_open()) {
log_file_.flush();
last_flush_ = std::chrono::steady_clock::now();
}
}
bool Logger::enabled(const Level level) const
{
std::lock_guard<std::mutex> lock(mutex_);
@ -209,8 +200,7 @@ void Logger::write(const Level level,
rotateIfNeeded_();
log_file_ << line << '\n';
const auto now = std::chrono::steady_clock::now();
if (level == Level::ERROR || level == Level::FATAL ||
now - last_flush_ >= flush_interval_) {
if (level == Level::ERROR || level == Level::FATAL || now - last_flush_ >= flush_interval_) {
log_file_.flush();
last_flush_ = now;
}

1
cmvr-es/common/base/logging/logger.h
View File

@ -43,7 +43,6 @@ public:
const std::string& application_name,
const std::filesystem::path& executable_directory);
void shutdown();
void flush();
bool enabled(Level level) const;
void write(Level level, const char* source_file, int source_line, const std::string& message);

93
cmvr-es/common/base/logging/tests/logger_test.cpp
View File

@ -1,93 +0,0 @@
#include "common/base/logging/logger.h"
#include <algorithm>
#include <array>
#include <filesystem>
#include <fstream>
#include <iterator>
#include <string>
#include <gtest/gtest.h>
#include <unistd.h>
namespace cmvr::logging {
namespace {
class LoggerTest : public testing::Test {
protected:
void SetUp() override
{
std::array<char, 64> pattern{};
const std::string value = "/tmp/cmvr-logger-test-XXXXXX";
std::copy(value.begin(), value.end(), pattern.begin());
if (char* created = ::mkdtemp(pattern.data())) {
directory_ = created;
}
ASSERT_FALSE(directory_.empty());
}
void TearDown() override
{
shutdownLogging();
std::error_code error;
std::filesystem::remove_all(directory_, error);
}
config::LoggerConfig warningFileConfig() const
{
config::LoggerConfig config;
config.set_minimum_level(config::LOG_LEVEL_DEBUG);
config.set_directory(directory_.string());
config.set_flush_interval_seconds(3600);
auto* route = config.add_routes();
route->set_level(config::LOG_LEVEL_WARNING);
route->set_terminal(false);
route->set_file(true);
return config;
}
std::string fileContents() const
{
std::ifstream input(directory_ / "logger_test.log");
return {std::istreambuf_iterator<char>(input),
std::istreambuf_iterator<char>()};
}
std::filesystem::path directory_;
};
TEST_F(LoggerTest, ExplicitFlushMakesWarningVisibleInFile)
{
ASSERT_TRUE(initLogging(
warningFileConfig(), "logger_test", directory_));
Logger::instance().write(
Level::WARNING, __FILE__, __LINE__, "warning sentinel");
Logger::instance().flush();
EXPECT_NE(fileContents().find("warning sentinel"), std::string::npos);
}
TEST_F(LoggerTest, ErrorIsVisibleInFileImmediately)
{
auto config = warningFileConfig();
config.mutable_routes(0)->set_level(config::LOG_LEVEL_ERROR);
ASSERT_TRUE(initLogging(config, "logger_test", directory_));
Logger::instance().write(
Level::ERROR, __FILE__, __LINE__, "error sentinel");
EXPECT_NE(fileContents().find("error sentinel"), std::string::npos);
}
TEST_F(LoggerTest, FlushIsSafeOutsideInitializedLifetime)
{
Logger::instance().flush();
ASSERT_TRUE(initLogging(
warningFileConfig(), "logger_test", directory_));
shutdownLogging();
Logger::instance().flush();
}
} // namespace
} // namespace cmvr::logging

33
cmvr-es/common/types/agv/agv_types.h
View File

@ -2,7 +2,6 @@
#define CMVR_ES_AGV_TYPES_H
#include <cstdint>
#include <functional>
#include <optional>
#include <string>
#include <unordered_map>
@ -91,14 +90,6 @@ enum class AgvTaskType {
Custom
};
/**
* @brief 固定距离平移使用的距离参考模式。
*/
enum class AgvTranslationMode {
Odometry = 0,
Localization
};
/**
* @brief AGV 车体坐标系下的平面速度。
*
@ -110,20 +101,6 @@ struct AgvVelocity {
double wz{0.0};
};
/**
* @brief AGV 车体坐标系下的固定距离平移参数。
*/
struct AgvTranslation {
double distance{0.0};
double vx{0.0};
double vy{0.0};
AgvTranslationMode mode{AgvTranslationMode::Odometry};
};
/**
* @brief 导航通用运动约束和执行选项。
*
@ -137,13 +114,7 @@ struct AgvMotionOptions {
double reach_distance{0.0};
double reach_angle{0.0};
double speed_ratio{1.0};
// 导航默认同步阻塞；调用方只有显式设为 true 才在任务接受后立即返回。
bool asynchronous{false};
int wait_timeout_ms{0};
int poll_interval_ms{0};
// 不带 RPC 框架依赖的取消检查。同步导航等待期间可由
// 上层绑定 deadline/cancel；驱动不得在函数返回后保留该回调。
std::function<bool()> cancellation_requested;
bool asynchronous{true};
};
/**
@ -249,7 +220,7 @@ struct AgvPathSegment {
/**
* @brief AGV 扫图过程中产生的数据文件。
*
* content 可保存控制器返回的二进制内容，例如 SEER Robokit 的 rawmap zip 包。
* content 可保存控制器返回的二进制内容，例如 SRC1100 的 rawmap zip 包。
*/
struct AgvMappingDataFile {
std::string name;

27
cmvr-es/common/types/arm/arm_types.h
View File

@ -2,7 +2,6 @@
#define CMVR_ES_ARM_TYPES_H
#include <cstdint>
#include <functional>
#include <string>
#include <vector>
@ -104,11 +103,6 @@ struct JointGroupState {
std::vector<double> position;
std::vector<double> velocity;
std::vector<double> effort;
std::uint64_t sequence{0};
std::int64_t sample_monotonic_ns{0};
bool position_valid{false};
bool velocity_valid{false};
bool effort_valid{false};
bool validForModel(const RobotModel& model) const
{
@ -171,11 +165,6 @@ struct MotionOptions {
double jerk{5.0};
std::vector<double> joint_velocity_limits;
bool asynchronous{false};
// Framework-independent cancellation check used by queued synchronous
// motion. Cancellation after device acceptance retains a typed motion
// barrier; the owner must call stopMotion() to confirm physical idle.
// Drivers must not retain this callback after moveJ/moveL returns.
std::function<bool()> cancellation_requested;
};
struct ServoOptions {
@ -184,14 +173,6 @@ struct ServoOptions {
double gain{300.0};
};
struct TorqueServoOptions {
// The UME legacy loop runs at 800 Hz by default.
double period{0.00125};
// A producer must continuously refresh the latest torque command. A stale
// command latches a fault and disables the actuator chain.
std::uint32_t command_watchdog_ms{20};
};
enum class RobotMode {
Unknown = 0,
Disconnected,
@ -224,14 +205,6 @@ enum class ControlMode {
Freedrive
};
enum class JointEffortSource {
Unspecified = 0,
MotorEstimate,
JointSensor,
ForceTorqueSensor,
Observer
};
struct ArmState {
double timestamp{0.0};
RobotMode robot_mode{RobotMode::Unknown};

85
cmvr-es/config/README.md
View File

@ -29,10 +29,10 @@ cmvr_es.pb.txt
<cmvr_es 可执行文件所在目录>/config/cmvr_es.pb.txt
```
安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时使用 `--config`：
安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时：
```bash
./output/bin/cmvr_es --config /etc/cmvr-es/cmvr_es.pb.txt
./output/bin/cmvr_es /etc/cmvr-es/cmvr_es.pb.txt
```
设备、任务和证书等相对配置路径均以根配置文件所在目录解析。模型等资源通过 `ConfigHelper::resolveResourceFile()` 在配置根及父目录中查找；生产部署仍建议使用明确绝对路径。
@ -73,45 +73,9 @@ cmvr_es.pb.txt
- 新增 loader 对不认识的 enum 和未设置的 oneof 必须明确失败；当前个别历史路径仍有退化默认行为，不应复制；
- 设备端口、坐标系、速度和单位写入注释；
- `enable` 应由 manager 层控制，后端内部的 enable 字段不能替代 manager 开关；
- QUIC 任务需要在 TaskManager 中显式开启；
- QUIC 需要 TaskManager 与 `QuicEdgeConfig.enable` 同时开启；
- QUIC 零媒体轨道是合法配置。
### QUIC 多平台
`QuicEdgeTask` 可以同时连接多个平台。原有顶层
`server_host`、`server_port`、`tls` 继续表示主平台；每个 `platforms` 条目会与主平台
并行运行。也可以不配置顶层目标，只使用一个或多个 `platforms` 条目：
```protobuf
platforms {
id: "operations"
server_host: "192.168.0.222"
server_port: 4433
enable_media: false
tls {
ca_file: "certs/cmvr-quic-ca.crt"
server_name: "192.168.0.222"
}
}
platforms {
id: "analytics"
server_host: "192.168.0.223"
server_port: 4433
enable_media: true
tls {
ca_file: "certs/cmvr-quic-ca.crt"
server_name: "192.168.0.223"
}
}
```
平台 ID 和 `host:port` 必须分别唯一；配置兼容主平台时，其平台 ID 使用任务
`QuicEdgeConfig.id`，新增条目也不能与它重名。每个平台拥有独立的 QUIC 连接、
注册会话、心跳序号、ACK 超时和重连退避，一个平台断线不会阻塞其他平台。
`enable_media` 默认为 `false`，此时仍发送注册、心跳、网络接口和设备状态，但不会
复制音视频；设为 `true` 才会把全局 `tracks` 转发到该平台。兼容的顶层主平台保持
原有媒体行为。
### gRPC 相机实时流
[`tasks/grpc_server_task/grpc_server_task.pb.txt`](tasks/grpc_server_task/grpc_server_task.pb.txt)
@ -145,49 +109,6 @@ output/bin/protoc \
该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。
## 双边遥操配置
源码仓库保持唯一根入口 [`cmvr_es.pb.txt`](cmvr_es.pb.txt)。统一的
[`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) 已声明
`ume_left`、`ume_right`、`ti5_motors` 和 `right_arm`；统一的
[`manager/task_manager.pb.txt`](manager/task_manager.pb.txt) 已声明
`ume_teleop` 和 gRPC server。角色差异不通过增加新的源码根配置文件表达，而由
两台机器各自的外部部署配置决定。
UME 主端部署配置应只启用本机需要的 UME 设备和 `ume_teleop` Task：
- `ume_left`、`ume_right` 在 DeviceManager 层默认关闭；
- [`devices/arm/ume_arms.pb.txt`](devices/arm/ume_arms.pb.txt) 内部的
`hardware_enabled` 也默认关闭；
- 两层硬件门必须在完成 CAN 映射、限位标定和安全验收后分别启用；
- 当前 Task 只实现会话、重连、心跳和 latest-only 指令邮箱，尚无生产算法调用
`submitSetpoint()`，返回 effort 也尚未接入本地触觉协调器。
机器人从端部署配置应只启用经过验收的机械臂设备以及所需的 gRPC server：
- `ti5_motors`、`right_arm` 默认关闭；
- `ArmTeleop` 服务后端默认关闭，模型哈希必须由部署配置明确给出；
- 当前 `MotorRobotArm::servoJ()` 仍是逐关节顺序写，不满足遥操作组伺服能力门；
- 启动 gRPC server 不代表允许遥操作执行，也不能绕过设备层硬件门。
部署时应把完整配置树分别复制到两台机器的外部目录，并继续使用相同的标准文件名：
```text
/etc/cmvr-es/ume/cmvr_es.pb.txt
/etc/cmvr-es/robot/cmvr_es.pb.txt
```
两套根配置都继续引用各自目录下同名的
`manager/device_manager.pb.txt` 和 `manager/task_manager.pb.txt`。运行命令为：
```bash
./output/bin/cmvr_es --config /etc/cmvr-es/ume/cmvr_es.pb.txt
./output/bin/cmvr_es --config /etc/cmvr-es/robot/cmvr_es.pb.txt
```
UME 主端需要填写从端地址、会话 manifest 和认证配置；机器人从端需要填写现场
机械臂配置。不要把生产 IP、token、私钥或设备标定值提交到仓库默认配置。
## 生产配置
`cmake --install` 会重建 `output/bin/config/`。生产配置应复制到 `/etc/cmvr-es/` 等外部目录并显式传入。

4
cmvr-es/config/devices/agv/seer_robokit.pb.txt → cmvr-es/config/devices/agv/src1100.pb.txt
View File

@ -8,9 +8,8 @@ agv {
}
agvs {
# 当前部署的控制器型号为 SRC1100；该值是设备实例 ID，不是后端类型名。
id: "src1100"
seer_robokit_agv {
src1100_agv {
ip: "192.168.192.5"
port_status: 19204
port_control: 19205
@ -19,7 +18,6 @@ agv {
port_other: 19210
port_push: 19301
recv_timeout_ms: 1000
control_nick_name: "cmvr-es"
enable_state_push: true
state_push_interval_ms: 200
state_push_included_fields: "x"

12
cmvr-es/config/devices/arm/arm.pb.txt
View File

@ -17,10 +17,6 @@ arm {
buffer_size: 50
default_vel: 1.0
default_acc: 2.0
# Reserved only: current MotorRobotArm servoJ writes joints sequentially,
# so code rejects the teleop group-servo capability even if this is true.
# A reviewed atomic/timed group primitive is required before changing it.
enable_teleop_group_servo: false
}
kinematics {
@ -160,15 +156,9 @@ arm {
motor {
motor_system_id: "ethercat_motors"
motor_group_ids: "dual_arm_ethercat"
motor_group_ids: "right_arm_ethercat"
dof: 7
joint_names: "R_SHOULDER_P"
joint_names: "R_SHOULDER_R"
joint_names: "R_SHOULDER_Y"
joint_names: "R_ELBOW_R"
joint_names: "R_WRIST_P"
joint_names: "R_WRIST_Y"
joint_names: "R_WRIST_R"
upd_freq: 1000
buffer_size: 50
default_vel: 1.0

152
cmvr-es/config/devices/arm/arm_eyou_left.pb.txt
View File

@ -1,152 +0,0 @@
arm {
robot_arms {
id: "eyou_left_arm"
motor {
motor_system_id: "ethercat_motors"
motor_group_ids: "dual_arm_ethercat"
dof: 7
joint_names: "L_SHOULDER_P"
joint_names: "L_SHOULDER_R"
joint_names: "L_SHOULDER_Y"
joint_names: "L_ELBOW_R"
joint_names: "L_WRIST_P"
joint_names: "L_WRIST_Y"
joint_names: "L_WRIST_R"
upd_freq: 1000
buffer_size: 50
default_vel: 1.0
default_acc: 2.0
}
kinematics {
pinocchio_dls_ik_solver {
urdf_path: "model/xiaoyan_description/dual_arm.urdf"
base_frame_name: "PELVIS_S"
flange_frame_name: "L_WRIST_R_S"
tcp_frame_name: "L_FINGER_TIP_FIXED"
max_iters: 100
pos_eps: 1e-6
rot_eps: 1e-6
damping: 1e-6
joint_limit_policy {
limits {
enable: true
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
}
soft_limit {
enable: true
margin_ratio: 0.01
min_margin_rad: 0.01
}
avoidance {
enable: false
gain: 0.2
margin_ratio: 0.15
max_push: 0.25
weight: 0.05
}
}
}
}
motion {
move_j {
toppra_joint_motion_planner {
path_type: TOPPRA_PATH_TYPE_QUINTIC
sample_period_s: 0.001
grid_size: 150
high_grid_size: 300
}
}
move_l {
pinocchio_cartesian_motion_planner {
sample_period_s: 0.001
position_gain: 4.0
rotation_gain: 4.0
line_deviation_check {
enable: true
line_deviation_warn_m: 0.01
line_deviation_stop_m: 0.03
line_direction_warn_deg: 20.0
line_direction_stop_deg: 45.0
line_direction_reset_deg: 10.0
line_check_min_distance_m: 0.01
}
joint_continuity_check {
enable: true
max_joint_delta_rad: 0.05
max_joint_velocity_rad_s: 10.0
max_joint_acceleration_rad_s2: 5000.0
}
cartesian_step_feasibility_check {
enable: true
min_linear_speed_ratio: 0.2
max_linear_direction_deviation_deg: 45.0
min_angular_speed_ratio: 0.2
max_angular_direction_deviation_deg: 45.0
min_desired_linear_speed: 1e-4
min_desired_angular_speed: 1e-4
}
}
}
speed_l {
pinocchio_cartesian_motion_planner {
linear_velocity_max: 0.55
linear_acceleration_max: 5.0
linear_jerk_max: 10.0
angular_velocity_max: 1.0
angular_acceleration_max: 5.0
angular_jerk_max: 12.0
linear_target_replan_threshold: 1e-4
angular_target_replan_threshold: 1e-4
linear_reverse_cos_threshold: -0.8660254037844386
linear_reverse_switch_speed_threshold: 1e-3
enforce_joint_acceleration_limits: true
line_deviation_check {
enable: true
line_deviation_warn_m: 0.01
line_deviation_stop_m: 0.03
line_direction_warn_deg: 20.0
line_direction_stop_deg: 45.0
line_direction_reset_deg: 10.0
line_check_min_distance_m: 0.01
}
joint_velocity_check {
enable: true
max_joint_velocity_rad_s: 30.0
max_joint_acceleration_rad_s2: 10000.0
}
cartesian_velocity_feasibility_check {
enable: true
min_linear_speed_ratio: 0.2
max_linear_direction_deviation_deg: 5.0
min_angular_speed_ratio: 0.2
max_angular_direction_deviation_deg: 5.0
min_desired_linear_speed: 1e-4
min_desired_angular_speed: 1e-4
}
}
speed_l_controller {
cartesian_velocity_controller {
control_period_s: 0.001
stop_twist_norm: 1e-9
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 10
}
}
}
}
}
}

1
cmvr-es/config/devices/arm/aubo_arm.pb.txt
View File

@ -17,7 +17,6 @@ arm {
tool_frame: "tool0"
username: "aubo"
password: "123456"
auto_power_on_after_hardware_estop_release: true
}
}
}

72
cmvr-es/config/devices/arm/ume_arms.pb.txt
View File

@ -1,72 +0,0 @@
# UME leader-arm device templates. They are deliberately disabled in
# manager/device_manager.pb.txt and hardware_enabled remains false here.
#
# Before real hardware use, independently verify interface bitrate
# (1 Mbit/s arbitration, 5 Mbit/s data, FD+BRS), motor/feedback IDs,
# direction, zero offsets, mechanical joint limits and safe torque limits.
# Each joint must also receive reviewed healthy_feedback_status and raw
# temperature thresholds. They are deliberately absent below, so changing
# hardware_enabled alone is insufficient to arm these placeholder profiles.
arm {
robot_arms {
id: "ume_right"
ume {
can {
dev_id: "can4"
channel_id: 4
interface_name: "can4"
enable_fd: true
bitrate_switch: true
send_timeout_us: 100
receive_timeout_us: 100
receive_own_messages: false
enable_error_frames: true
}
control_frequency_hz: 800
cycle_deadline_us: 1000
feedback_watchdog_ms: 20
shutdown_timeout_ms: 50
hardware_enabled: false
joints { joint_name: "RJ1" command_id: 1 feedback_id: 17 reported_motor_id: 1 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
joints { joint_name: "RJ2" command_id: 2 feedback_id: 18 reported_motor_id: 2 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
joints { joint_name: "RJ3" command_id: 3 feedback_id: 19 reported_motor_id: 3 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
joints { joint_name: "RJ4" command_id: 4 feedback_id: 20 reported_motor_id: 4 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
joints { joint_name: "RJ5" command_id: 5 feedback_id: 21 reported_motor_id: 5 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
joints { joint_name: "RJ6" command_id: 6 feedback_id: 22 reported_motor_id: 6 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
joints { joint_name: "RJ7" command_id: 7 feedback_id: 23 reported_motor_id: 7 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
joints { joint_name: "RJ8" command_id: 8 feedback_id: 24 reported_motor_id: 8 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
}
}
robot_arms {
id: "ume_left"
ume {
can {
dev_id: "can5"
channel_id: 5
interface_name: "can5"
enable_fd: true
bitrate_switch: true
send_timeout_us: 100
receive_timeout_us: 100
receive_own_messages: false
enable_error_frames: true
}
control_frequency_hz: 800
cycle_deadline_us: 1000
feedback_watchdog_ms: 20
shutdown_timeout_ms: 50
hardware_enabled: false
joints { joint_name: "LJ1" command_id: 1 feedback_id: 17 reported_motor_id: 1 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
joints { joint_name: "LJ2" command_id: 2 feedback_id: 18 reported_motor_id: 2 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
joints { joint_name: "LJ3" command_id: 3 feedback_id: 19 reported_motor_id: 3 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
joints { joint_name: "LJ4" command_id: 4 feedback_id: 20 reported_motor_id: 4 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 }
joints { joint_name: "LJ5" command_id: 5 feedback_id: 21 reported_motor_id: 5 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
joints { joint_name: "LJ6" command_id: 6 feedback_id: 22 reported_motor_id: 6 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
joints { joint_name: "LJ7" command_id: 7 feedback_id: 23 reported_motor_id: 7 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
joints { joint_name: "LJ8" command_id: 8 feedback_id: 24 reported_motor_id: 8 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 }
}
}
}

37
cmvr-es/config/devices/motor/ethercat_motors.pb.txt
View File

@ -2,7 +2,7 @@ motor {
id: "ethercat_motors"
motor_groups {
id: "dual_arm_ethercat"
id: "right_arm_ethercat"
bus_type: MOTOR_BUS_ETHERCAT
vendor: MOTOR_VENDOR_EYOU
protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
@ -38,20 +38,13 @@ motor {
sync_monitor_period_ms: 1000
}
slaves { motor_id: 1 alias: 0 position: 1 }
slaves { motor_id: 2 alias: 0 position: 2 }
slaves { motor_id: 3 alias: 0 position: 3 }
slaves { motor_id: 4 alias: 0 position: 4 }
slaves { motor_id: 5 alias: 0 position: 5 }
slaves { motor_id: 6 alias: 0 position: 6 }
slaves { motor_id: 7 alias: 0 position: 7 }
slaves { motor_id: 8 alias: 0 position: 8 }
slaves { motor_id: 9 alias: 0 position: 9 }
slaves { motor_id: 10 alias: 0 position: 10 }
slaves { motor_id: 11 alias: 0 position: 11 }
slaves { motor_id: 12 alias: 0 position: 12 }
slaves { motor_id: 13 alias: 0 position: 13 }
slaves { motor_id: 14 alias: 0 position: 14 }
slaves { motor_id: 1 alias: 0 position: 0 }
slaves { motor_id: 2 alias: 0 position: 1 }
slaves { motor_id: 3 alias: 0 position: 2 }
slaves { motor_id: 4 alias: 0 position: 3 }
slaves { motor_id: 5 alias: 0 position: 4 }
slaves { motor_id: 6 alias: 0 position: 5 }
slaves { motor_id: 7 alias: 0 position: 6 }
}
joint_limits {
@ -64,13 +57,6 @@ motor {
joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
}
motors {
@ -81,13 +67,6 @@ motor {
motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 8 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 9 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 10 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 11 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 12 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 13 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 14 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
}

6
cmvr-es/config/logger/logger.pb.txt
View File

@ -13,17 +13,17 @@ logger {
routes {
level: LOG_LEVEL_WARNING
terminal: true
file: true
file: false
}
routes {
level: LOG_LEVEL_ERROR
terminal: true
file: true
file: false
}
routes {
level: LOG_LEVEL_FATAL
terminal: true
file: true
file: false
}
directory: "../log"

47
cmvr-es/config/manager/device_manager.pb.txt
View File

@ -4,18 +4,6 @@ device_manager {
description: "cmvr edge system version 0.1"
init_all_motors_when_no_active_joints: true
# The unified safety coordinator observes all decisions while the legacy
# gates remain authoritative during staged hardware migration.
safety {
mode: SHADOW
stop_all_timeout_ms: 15000
recovery_timeout_ms: 10000
command_ledger_result_capacity: 4096
command_ledger_total_id_capacity: 262144
event_history_capacity: 2048
fail_startup_on_missing_control_capability: false
}
devices {
id: "mujoco_world"
type: DEVICE_TYPE_MUJOCO_WORLD
@ -91,7 +79,7 @@ device_manager {
id: "ethercat_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/ethercat_motors.pb.txt"
enable: true
enable: false
}
devices {
@ -105,21 +93,14 @@ device_manager {
id: "eyou_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm.pb.txt"
enable: true
}
devices {
id: "eyou_left_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm_eyou_left.pb.txt"
enable: true
enable: false
}
devices {
id: "aubo_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/aubo_arm.pb.txt"
enable: true
enable: false
}
devices {
@ -129,23 +110,6 @@ device_manager {
enable: false
}
devices {
id: "ume_right"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/ume_arms.pb.txt"
# Two gates must be explicitly changed after the physical safety review:
# this entry and ume.hardware_enabled in the arm config.
enable: false
}
devices {
id: "ume_left"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/ume_arms.pb.txt"
# Two gates must be explicitly changed after the physical safety review.
enable: false
}
devices {
id: "bio_head"
type: DEVICE_TYPE_BIO_HEAD_ROBOT
@ -154,11 +118,10 @@ device_manager {
}
devices {
# 当前部署的控制器型号为 SRC1100；该值是设备实例 ID，不是后端类型名。
id: "src1100"
type: DEVICE_TYPE_AGV
config_file: "devices/agv/seer_robokit.pb.txt"
enable: true
config_file: "devices/agv/src1100.pb.txt"
enable: false
}
devices {

9
cmvr-es/config/manager/task_manager.pb.txt
View File

@ -30,13 +30,4 @@ task_manager {
# Host-development default: no QUIC Gateway or physical media devices.
enable: false
}
tasks {
id: "ume_teleop"
type: TASK_TYPE_UME_TELEOP
run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
config_file: "tasks/ume_teleop_task/ume_teleop_task.pb.txt"
# Fail-safe default: configure the remote robot endpoint, manifest and
# deployment security policy before enabling this outbound control task.
enable: false
}
}

27
cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt
View File

@ -5,31 +5,4 @@ grpc_server {
enable_reflection: true
camera_stream_max_pending_frames: 2
camera_stream_max_frame_age_ms: 250
# Current small-scope deployment intentionally keeps the existing clients
# certificate-free. Recovery remains unavailable over the network.
security {
transport_mode: INSECURE
authentication_mode: DISABLED
recovery_exposure: RECOVERY_DISABLED
allow_insecure_non_loopback: true
}
# The RobotArm adapter is implemented, but remains explicitly closed until
# the device itself enables teleop group servo, real hashes are provisioned,
# and group-write timing and independent stop behavior pass hardware review.
arm_teleop_backend {
enable: false
device_id: "right_arm"
# Deliberately empty placeholders are invalid when enable=true.
model_sha256: ""
calibration_sha256: ""
base_frame: "PELVIS_S"
tool_frame: "R_FINGER_TIP_FIXED"
servo_period_s: 0.001
max_apply_duration_us: 800
require_powered: true
max_initial_position_step_rad: 0.02
max_position_step_rad: 0.003
}
}

35
cmvr-es/config/tasks/ume_teleop_task/ume_teleop_task.pb.txt
View File

@ -1,35 +0,0 @@
ume_teleop {
id: "ume_teleop"
# Deliberately left empty. The TaskManager entry is disabled by default, and
# init fails closed if it is enabled before a robot endpoint is configured.
server_address: ""
# M6 implements explicit insecure transport for isolated development only.
# Production deployment must add and configure channel credentials first.
allow_insecure: false
open_session {
protocol_major: 1
protocol_minor: 0
client_instance_id: "ume-controller"
requested_command_rate_hz: 250
requested_state_rate_hz: 250
watchdog_timeout_ms: 100
requested_lease_ms: 500
# Replace with the manifest exported by the CMVR-ES robot instance.
expected_robot {
robot_id: ""
position_unit: "rad"
velocity_unit: "rad/s"
effort_unit: "N*m"
}
}
reconnect {
initial_delay_ms: 100
maximum_delay_ms: 5000
multiplier: 2.0
}
}

16
cmvr-es/devices/README.md
View File

@ -36,13 +36,13 @@ config/cmvr_es.pb.txt
| 大类 | 抽象接口 | 类别工厂 | 当前可选后端 |
| --- | --- | --- | --- |
| Camera | [`camera/abstract_camera.h`](camera/abstract_camera.h) | [`camera/camera_factory.h`](camera/camera_factory.h) | UVC、RealSense、Hikvision |
| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SEER Robokit |
| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、[AUBO](arm/aubo_arm/README.md)、Huayan、UME |
| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SRC1100 |
| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、AUBO、Huayan |
| DexHand | [`dexhand/abstract_dexhand.h`](dexhand/abstract_dexhand.h) | [`dexhand/dexhand_factory.h`](dexhand/dexhand_factory.h) | RH56DFTP、PX6AXGen3 |
| Microphone | [`microphone/abstract_microphone.h`](microphone/abstract_microphone.h) | [`microphone/microphone_factory.h`](microphone/microphone_factory.h) | FFmpeg |
| Speaker | [`speaker/abstract_speaker.h`](speaker/abstract_speaker.h) | [`speaker/speaker_factory.h`](speaker/speaker_factory.h) | FFmpeg |
| BioHead | [`biohead/abstract_biohead.h`](biohead/abstract_biohead.h) | DeviceFactory 直接创建 | BioHeadRobot |
| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT |
| MotorSystem | `motor/motor_system/` | DeviceFactory 直接创建 | CAN/MuJoCo motor group |
代码目录存在不等于已经接入配置创建链：
@ -222,7 +222,7 @@ CameraDeviceConfig / AGVDeviceConfig / ... 的外层 id
## 摄像头与麦克风实时流
设备实现抽象流接口后，由 [`../manager/media_source_manager/`](../manager/media_source_manager/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。
设备实现抽象流接口后，由 [`../manager/media_source_hub/`](../manager/media_source_hub/) 适配给 gRPC 和 QUIC，不应在设备后端实现两套协议代码。
当前 Hub 轨道：
@ -312,7 +312,7 @@ adapter 检测到描述变化后创建新 descriptor，设备后端不要自行
- 满队列覆盖旧数据是实时媒体的预期行为；
- `waitEncodedFrame()` 必须有有限 timeout，不能永久阻塞。
MediaSourceManager Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。
MediaSourceHub Subscription 同样是单消费者对象，不同协议或客户端必须各自订阅。
发布后的 `MediaFrame`、`TrackDescriptor` 和 payload 不可再修改。
@ -334,19 +334,19 @@ MediaSourceManager Subscription 同样是单消费者对象，不同协议或客
无硬件参考测试：
- [`camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp`](camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp)
- [`../manager/media_source_manager/tests/media_source_manager_test.cpp`](../manager/media_source_manager/tests/media_source_manager_test.cpp)
- [`../manager/media_source_hub/tests/media_source_hub_test.cpp`](../manager/media_source_hub/tests/media_source_hub_test.cpp)
```bash
cmake -S . -B build \
-DCMVR_ARCH=x86 \
-DBUILD_TESTING=ON \
-DCMVR_MEDIA_SOURCE_MANAGER_BUILD_TESTS=ON
-DCMVR_MEDIA_SOURCE_HUB_BUILD_TESTS=ON
cmake --build build -j"$(nproc)"
ctest \
--test-dir build \
-R 'hikvision_camera_callback_test|media_source_manager_test' \
-R 'hikvision_camera_callback_test|media_source_hub_test' \
--output-on-failure
```

4
cmvr-es/devices/agv/CMakeLists.txt
View File

@ -1,5 +1,5 @@
add_subdirectory(my_agv)
add_subdirectory(seer_robokit)
add_subdirectory(src1100)
add_library(agv INTERFACE)
@ -8,7 +8,7 @@ target_include_directories(agv INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(agv
INTERFACE
cmvr_es::device::my_agv
cmvr_es::device::seer_robokit_agv
cmvr_es::device::src1100_agv
cmvr_es::proto
)

60
cmvr-es/devices/agv/abstract_agv.h
View File

@ -15,12 +15,6 @@
namespace cmvr::device {
enum class AgvActionKind {
NavigateToPose,
NavigateToStation,
FollowPath,
};
/**
* @brief AGV/移动底盘设备抽象基类。
*
@ -34,16 +28,6 @@ public:
DeviceKind kind() const noexcept override { return DeviceKind::AGV; }
/**
* @brief Whether this backend provides terminal-state and stopped-motion
* confirmation plus bounded cancellation suitable for synchronous
* Action execution.
*/
virtual bool supportsSynchronousAction(AgvActionKind) const noexcept
{
return false;
}
/**
* @brief 获取 AGV 运行状态快照。
*/
@ -101,41 +85,12 @@ public:
/**
* @brief 发起显式站点到站点路径导航任务。
*/
virtual AgvResult followPath(
const std::vector<AgvPathSegment>& path)
virtual AgvResult followPath(const std::vector<AgvPathSegment>& path)
{
(void)path;
return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented");
}
/**
* @brief 按指定速度执行固定距离平移。
*
* 返回成功表示控制器已经接受命令，不表示运动已经完成。
*/
virtual AgvResult translate(const AgvTranslation& translation)
{
(void)translation;
return AgvResult::failure(
AgvErrorCode::UnsupportedCommand,
"translate not implemented");
}
/**
* @brief 发起显式站点到站点路径导航任务，并指定同步/异步选项。
*
* 保留单参数虚函数以兼容已有派生类；旧实现会由本重载转发。
*/
virtual AgvResult followPath(
const std::vector<AgvPathSegment>& path,
const AgvMotionOptions& options)
{
(void)options;
return followPath(path);
}
/**
* @brief 暂停当前导航任务，如果设备支持。
*/
@ -183,19 +138,6 @@ public:
return setVelocity(AgvVelocity{});
}
/**
* @brief 确认 AGV 已进入可安全释放控制权的停止状态。
*
* 该接口只在导航任务已终止且底盘速度经过连续采样确认为零后返回
* 成功；仅收到取消、停止或零速度命令的应答不构成成功。
*/
virtual AgvResult confirmMotionStopped()
{
return AgvResult::failure(
AgvErrorCode::UnsupportedCommand,
"confirmMotionStopped not implemented");
}
/**
* @brief 查询 AGV 可用地图名称列表。
*/

10
cmvr-es/devices/agv/agv_factory.h
View File

@ -7,7 +7,7 @@
#include "common/base/logging/logger.h"
#include "devices/agv/abstract_agv.h"
#include "devices/agv/my_agv/include/my_agv.h"
#include "seer_robokit_agv.h"
#include "devices/agv/src1100/include/src1100_agv.h"
namespace cmvr::device {
@ -31,15 +31,15 @@ public:
backend.set_id(cfg.id());
return std::make_shared<MyAgv>(backend);
}
case config::AGVDeviceConfig::kSeerRobokitAgv:
case config::AGVDeviceConfig::kSrc1100Agv:
{
if (!cfg.seer_robokit_agv().id().empty() && cfg.seer_robokit_agv().id() != cfg.id()) {
if (!cfg.src1100_agv().id().empty() && cfg.src1100_agv().id() != cfg.id()) {
CMVR_LOG(ERROR) << "[AGVFactory]: AGV id does not match backend id: " << cfg.id();
return nullptr;
}
auto backend = cfg.seer_robokit_agv();
auto backend = cfg.src1100_agv();
backend.set_id(cfg.id());
return std::make_shared<SeerRobokitAgv>(backend);
return std::make_shared<Src1100Agv>(backend);
}
case config::AGVDeviceConfig::BACKEND_NOT_SET:

57
cmvr-es/devices/agv/seer_robokit/CMakeLists.txt
View File

@ -1,57 +0,0 @@
add_library(seer_robokit_agv SHARED
src/seer_robokit_agv.cpp
src/seer_robokit_transport.cpp
src/seer_robokit_control.cpp
src/seer_robokit_status.cpp
src/seer_robokit_navigation.cpp
src/seer_robokit_navigation_wait.cpp
src/seer_robokit_map.cpp
include/seer_robokit_agv.h
include/seer_robokit_protocol.h
include/seer_robokit_utils.h
include/seer_robokit_navigation_utils.h
include/seer_robokit_pgv_utils.h
)
target_include_directories(seer_robokit_agv
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/include
${PROJECT_SOURCE_DIR}/cmvr-es
)
target_link_libraries(seer_robokit_agv
PUBLIC
cmvr_es::proto
jsoncpp
)
add_library(cmvr_es::device::seer_robokit_agv ALIAS seer_robokit_agv)
install(TARGETS seer_robokit_agv LIBRARY DESTINATION lib)
if(BUILD_TESTING)
add_executable(seer_robokit_control_authority_test
tests/seer_robokit_control_authority_test.cpp
)
target_link_libraries(seer_robokit_control_authority_test
PRIVATE
cmvr_es::device::seer_robokit_agv
gtest
gtest_main
pthread
)
add_test(
NAME seer_robokit_control_authority_test
COMMAND seer_robokit_control_authority_test
)
set(_seer_robokit_control_authority_test_environment
"LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
)
if(CMVR_TEST_SYSTEM_LIBSTDCXX)
list(APPEND _seer_robokit_control_authority_test_environment
"LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
endif()
set_tests_properties(seer_robokit_control_authority_test PROPERTIES
TIMEOUT 180
ENVIRONMENT "${_seer_robokit_control_authority_test_environment}"
)
endif()

395
cmvr-es/devices/agv/seer_robokit/README.md
View File

@ -1,395 +0,0 @@
# 仙工 SEER Robokit AGV 适配器
`SeerRobokitAgv` 将仙工 SEER Robokit TCP/IP API 适配为 CMVR 的通用
`AbstractAGV`/`cmvr.api.AgvService`。厂商命令号、端口、抢占控制权、状态轮询、
地图格式转换和错误码解析都封装在本目录内。
本项目现场使用的控制器型号仍是 SRC1100，所以设备实例 ID 保持为
`src1100`；它只用于配置关联和 gRPC 路由，不再作为驱动实现名称。后端配置字段
使用 `seer_robokit_agv`，目录、类、库和测试统一使用 `seer_robokit` /
`SeerRobokitAgv` 命名。
从旧版本升级时，外部部署配置必须同步使用 `seer_robokit_agv { ... }`，并把
配置路径更新为 `devices/agv/seer_robokit.pb.txt`；设备实例 ID 保持不变。程序、
外部配置和部署脚本需要原子升级，不能把旧字段或旧路径与新二进制混用。
返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。
## 代码与配置
所有驱动头文件统一放在 `include/`，实现文件统一放在 `src/`；测试源码独立放在
`tests/`。除 `seer_robokit_agv.h` 外，其余头文件均为驱动内部实现细节。
- 公共类声明：[`include/seer_robokit_agv.h`](include/seer_robokit_agv.h)
- 导航轮询工具：
[`include/seer_robokit_navigation_utils.h`](include/seer_robokit_navigation_utils.h)
- PGV 参数转换：
[`include/seer_robokit_pgv_utils.h`](include/seer_robokit_pgv_utils.h)
- 协议常量：[`include/seer_robokit_protocol.h`](include/seer_robokit_protocol.h)
- 通用解析工具：[`include/seer_robokit_utils.h`](include/seer_robokit_utils.h)
- 生命周期和连接：[`src/seer_robokit_agv.cpp`](src/seer_robokit_agv.cpp)
- TCP 帧与收发：[`src/seer_robokit_transport.cpp`](src/seer_robokit_transport.cpp)
- 控制权与受控命令：[`src/seer_robokit_control.cpp`](src/seer_robokit_control.cpp)
- 状态与推送缓存：[`src/seer_robokit_status.cpp`](src/seer_robokit_status.cpp)
- 导航命令：[`src/seer_robokit_navigation.cpp`](src/seer_robokit_navigation.cpp)
- 阻塞等待与停车确认：
[`src/seer_robokit_navigation_wait.cpp`](src/seer_robokit_navigation_wait.cpp)
- 地图和建图：[`src/seer_robokit_map.cpp`](src/seer_robokit_map.cpp)
- 假控制器测试：
[`tests/seer_robokit_control_authority_test.cpp`](tests/seer_robokit_control_authority_test.cpp)
- 设备配置：
[`../../../config/devices/agv/seer_robokit.pb.txt`](../../../config/devices/agv/seer_robokit.pb.txt)
- DeviceManager 配置：
[`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt)
- gRPC API：
[`../../../../protos/cmvr/api/agv_service.proto`](../../../../protos/cmvr/api/agv_service.proto)、
[`../../../../protos/cmvr/api/agv_command.proto`](../../../../protos/cmvr/api/agv_command.proto)、
[`../../../../protos/cmvr/api/agv_utils.proto`](../../../../protos/cmvr/api/agv_utils.proto)
## 配置和启动
现场配置至少需要修改控制器 IP；端口通常保持仙工默认值：
```textproto
agv {
agvs {
id: "src1100"
seer_robokit_agv {
ip: "192.168.192.5"
port_status: 19204
port_control: 19205
port_nav: 19206
port_config: 19207
port_other: 19210
port_push: 19301
recv_timeout_ms: 1000
control_nick_name: "cmvr-es"
enable_state_push: true
state_push_interval_ms: 200
enable_map_update: true
map_update_interval_ms: 1000
map_update_history_size: 8
}
}
}
```
还要在 `device_manager.pb.txt` 中确认同一个设备 id，并在完成现场安全检查后把
`enable` 改为 `true`。源码默认配置故意保持关闭。
```textproto
devices {
id: "src1100"
type: DEVICE_TYPE_AGV
config_file: "devices/agv/seer_robokit.pb.txt"
enable: true
}
```
构建、安装并启动：
```bash
cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build -j2
cmake --install build
./output/bin/cmvr_es
```
`output/bin/cmvr_es` 默认读取 `output/bin/config/`。修改源码配置后需要重新安装，
或通过程序支持的外部配置入口启动，不能只修改源码文件后继续使用旧的
`output/` 配置。
## 控制器端口和命令
| 端口 | 主要用途 | 当前使用的命令 |
| --- | --- | --- |
| `19204` | 状态、站点、地图和建图文件 | `1004`、`1007`、`1020`、`1101`、`1110`、`1300`、`1301`、`1780`、`1800` |
| `19205` | 底盘控制 | `2000`、`2010`、`2022` |
| `19206` | 导航任务 | `3001`、`3002`、`3003`、`3051`、`3066`、`3067` |
| `19207` | 控制权、地图上传下载 | `4005`、`4010`、`4011` |
| `19210` | 开始/停止建图 | `6100`、`6101` |
| `19301` | 机器人状态推送 | `9300`/`19300` 配置，`19301` 推送 |
所有会改变机器人或控制器状态的调用都在 SEER Robokit 子类内部先通过 `4005`
抢权，负载为稳定的 `nick_name`，成功后才发送实际命令。普通命令集中走
`sendControlledCommand_`；`emergencyStop` 为保证 `2000` 和导航取消之间不被
插入其他命令，会在同一个控制序列锁内只抢一次权。只读查询不抢权。不要在
gRPC 客户端另做一套租约逻辑。
## gRPC 接口概览
默认示例端点为 `127.0.0.1:50052`；远程部署时替换为 CMVR 服务所在主机，
不是 SEER Robokit 原生 TCP 端口。
| gRPC 方法 | SEER Robokit 行为 | 说明 |
| --- | --- | --- |
| `getRuntimeState` | 推送缓存，缺失时查询 `1004/1007/1300` | 只读 |
| `getNavigationStatus` | 跟踪任务查询 `1110`，无精确上下文时回退 `1020` | 只读；同步等待另用 `1101` 确认停车 |
| `emergencyStop` | `2000`，再执行 `3003` 或 `3067` | 软件停止，不替代硬件急停 |
| `clearFault` | 未实现 | 返回 `UnsupportedCommand` |
| `navigateToPose` | `3051` + `freeGo` | 地图绝对位姿，仅双轮差速底盘 |
| `navigateToStation` | `3051` | 站点路径导航；PGV 二次定位也使用此方法 |
| `followPath` | `3066` | 仙工“指定路径导航”，与 `3051` 不同 |
| `pauseNavigation` / `resumeNavigation` | `3001` / `3002` | 导航控制 |
| `cancelNavigation` | `3003`，路径队列使用 `3067` | 取消当前跟踪任务 |
| `setVelocity` / `stopVelocityControl` | `2010` | 车体速度；停止时发送全零速度 |
| `listMaps` / `listStations` | `1300` / `1301` | 只读 |
| `switchMap` | `2022` | 会改变定位所用地图 |
| `uploadMap` / `downloadMap` | `4010` / `4011` | 上传会抢权，下载只读 |
| `startMapping` / `stopMapping` | `6100` / `6101` | 建图控制 |
| `streamMap` | `1780/1800` 加内部解析和缓存 | 对外发送统一 2D/3D 地图，不暴露 `.smap` 原始格式 |
查询运行状态：
```bash
grpcurl -plaintext \
-d '{"header":{"deviceId":"src1100"}}' \
127.0.0.1:50052 \
cmvr.api.AgvService/getRuntimeState
```
查询导航状态：
```bash
grpcurl -plaintext \
-d '{"header":{"deviceId":"src1100"}}' \
127.0.0.1:50052 \
cmvr.api.AgvService/getNavigationStatus
```
列出地图和当前地图站点：
```bash
grpcurl -plaintext \
-d '{"header":{"deviceId":"src1100"}}' \
127.0.0.1:50052 \
cmvr.api.AgvService/listMaps
grpcurl -plaintext \
-d '{"header":{"deviceId":"src1100"}}' \
127.0.0.1:50052 \
cmvr.api.AgvService/listStations
```
## 导航的同步语义
`navigateToPose`、`navigateToStation` 和 `followPath` 默认同步阻塞。控制器接受
命令后，适配器继续轮询精确任务状态，并结合 `1101` 状态确认底盘已经停车；
到达、失败、取消、遇障停止或超时后才返回。`waitTimeoutMs` 为 `0` 时使用
适配器默认值，当前为 10 分钟；`pollIntervalMs` 为 `0` 时当前使用 200 ms。
连续观察到障碍阻挡且底盘已经停止后，适配器会主动取消该导航；清理结果不明确
时还可能发送软件停止。任务不会在障碍消失后由本次调用自动恢复。等待超时、
RPC cancel 和 deadline 到期也会进入安全取消及停车确认，因此函数返回时间可能
晚于最初发现障碍或取消请求的时刻。
调用方的 gRPC deadline 必须大于预计行程时间和 `waitTimeoutMs`。RPC 被取消或
deadline 到期时，适配器会进入安全取消/停车确认流程。显式设置
`"asynchronous":true` 后不会等待任务终态：站点导航和指定路径导航在控制器
接受后返回；自由导航仍会做最长约 1.5 秒的启动确认。异步成功不代表已经到点。
`AgvMotionOptions` 中，SEER Robokit 的 `3051` 导航当前支持：
| gRPC 字段 | 控制器字段 | 单位 |
| --- | --- | --- |
| `maxSpeed` | `max_speed` | m/s |
| `maxAngularSpeed` | `max_wspeed` | rad/s |
| `maxAcceleration` | `max_acc` | m/s² |
| `maxAngularAcceleration` | `max_wacc` | rad/s² |
| `reachDistance` | `reach_dist` | m |
| `reachAngle` | `reach_angle` | rad |
`asynchronous`、`waitTimeoutMs` 和 `pollIntervalMs` 由适配器本地执行。
`speedRatio` 当前没有对应的 SEER Robokit 序列化字段。`followPath` 的运动选项当前只
控制同步/异步等待、超时和轮询；在没有确认 `3066` 的速度字段前，不会猜测性地
写入每个路径段。
## 固定路径导航的 PGV 二次定位
仙工文档 [“路径导航 / 2. 固定路径导航 PGV 二次定位调整”](https://seer-group.feishu.cn/wiki/Q26SwaNoGisuLWk2vCxcPfVWn2e)
说明 PGV 参数是 `3051 / robot_task_gotarget_req` 的顶层可选字段。因此在 CMVR
中应调用 `navigateToStation`，不是 `followPath`。后者对应另一条
`3066 / 指定路径导航` 协议，现有仙工资料和仓库历史都没有证明 `3066` 支持
PGV 字段。
PGV 参数通过 `adapterParams.values` 传入。protobuf map 的值是字符串，
SEER Robokit 适配器会在任何状态查询、抢权和运动命令之前完成校验，再转换为控制器
要求的 JSON `bool`/`number`：
| `adapterParams.values` 键 | 输出 JSON 类型 | 含义 |
| --- | --- | --- |
| `use_pgv` | `bool` | 使用上视 PGV |
| `use_down_pgv` | `bool` | 使用下视 PGV |
| `pgv_adjust_dist` | `number` | 最大调整半径，必须为有限非负数；用于仙工第 3/4 种调整方式 |
| `pgv_adjust_cx` | `number` | 调整范围圆心在二维码坐标系下的 X 偏移；用于第 4 种方式 |
| `pgv_adjust_cy` | `number` | 调整范围圆心在二维码坐标系下的 Y 偏移；用于第 4 种方式 |
| `pgv_x_adjust` | `number` | 仅调整小车 X 方向误差；用于第 2 种方式 |
所有数字都必须是完整、有限的数字字符串；偏移量允许正负。适配器不臆造
调整半径上限，也不假定上视和下视一定互斥，这些约束应由实际 PGV 安装、标定和
当前控制器版本确定。显式的 `"false"` 和 `"0"` 仍会作为原生布尔值和数值
发给控制器；没有给出的字段不会发送。第 2/3/4 种方式由控制器和站点配置决定，
本接口只传递与所选方式匹配的调整参数。
一旦请求中出现任意 PGV 键，适配器只允许同时出现 `source_id`、`task_id` 和
上述 PGV 字段；`operation`、`jack_height`、脚本名或未知扩展字段都会在状态
查询和抢权前被拒绝，避免一次 PGV 导航意外夹带顶升、货叉、IO 或脚本动作。
没有 PGV 键的既有站点导航扩展语义保持不变。
上视 PGV 示例。该命令会让机器人导航到 `AP1`，只能在确认地图、站点、PGV
标定、行驶区域和急停人员后执行：
```bash
grpcurl -plaintext \
-d '{
"header":{"deviceId":"src1100"},
"stationId":"AP1",
"options":{
"maxSpeed":0.15,
"maxAcceleration":0.15,
"asynchronous":false,
"waitTimeoutMs":300000,
"pollIntervalMs":200
},
"adapterParams":{"values":{
"use_pgv":"true",
"pgv_adjust_dist":"0.3",
"pgv_adjust_cx":"-0.3",
"pgv_adjust_cy":"0"
}}
}' \
127.0.0.1:50052 \
cmvr.api.AgvService/navigateToStation
```
下视 PGV 使用同一接口，把 `use_down_pgv` 设为字符串 `"true"`；其他调整
字段是否需要传入取决于现场定位方案。如果控制器版本要求明确起点，可在同一个
map 中增加 `"source_id":"实际起点站点"`；默认起点为 `SELF_POSITION`。
仙工在线文档当前有两处拼写不一致：
- 代码块出现了损坏字段 `pgv_adjustuse_pgv_dist`；适配器会拒绝它，正确字段是
`pgv_adjust_dist`；
- 表格写成 `pgv_ajdust_cy`，而示例和仓库旧版序列化代码使用
`pgv_adjust_cy`。适配器兼容接收前者，但只向控制器输出规范字段
`pgv_adjust_cy`；两个拼写同时出现会因歧义被拒绝。
C++ 调用同样复用通用扩展参数：
```cpp
cmvr::device::AgvMotionOptions options;
options.max_speed = 0.15;
options.max_acceleration = 0.15;
cmvr::device::AgvAdapterParams adapter;
adapter.values["use_pgv"] = "true";
adapter.values["pgv_adjust_dist"] = "0.3";
adapter.values["pgv_adjust_cx"] = "-0.3";
adapter.values["pgv_adjust_cy"] = "0";
const auto result = agv.navigateToStation("AP1", options, adapter);
```
## 其他导航和控制示例
自由导航使用地图绝对坐标，不是“相对当前位置移动多少米”。示例只展示请求
结构，发送前必须读取当前位姿并确认目标在同一地图的安全区域：
```bash
grpcurl -plaintext \
-d '{
"header":{"deviceId":"src1100"},
"pose":{"x":1.0,"y":0.0,"theta":0.0},
"options":{"maxSpeed":0.15,"maxAcceleration":0.15}
}' \
127.0.0.1:50052 \
cmvr.api.AgvService/navigateToPose
```
显式站点路径使用 `3066`：
```bash
grpcurl -plaintext \
-d '{
"header":{"deviceId":"src1100"},
"path":[
{"sourceStation":"LM1","targetStation":"LM2"},
{"sourceStation":"LM2","targetStation":"AP1"}
]
}' \
127.0.0.1:50052 \
cmvr.api.AgvService/followPath
```
暂停、继续和取消的请求体直接是 `CommandHeader.Request`，没有外层 `header`：
```bash
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
127.0.0.1:50052 cmvr.api.AgvService/pauseNavigation
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
127.0.0.1:50052 cmvr.api.AgvService/resumeNavigation
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
127.0.0.1:50052 cmvr.api.AgvService/cancelNavigation
```
差速底盘的 `vy` 应保持 `0`。低层速度控制不等价于导航，并可能与已有任务
冲突；只应在专门的速度控制测试流程中使用：
```bash
grpcurl -plaintext \
-d '{"header":{"deviceId":"src1100"},"velocity":{"vx":0.05,"vy":0,"wz":0}}' \
127.0.0.1:50052 \
cmvr.api.AgvService/setVelocity
grpcurl -plaintext -d '{"deviceId":"src1100"}' \
127.0.0.1:50052 cmvr.api.AgvService/stopVelocityControl
```
## 错误返回
控制器响应中的非零 `ret_code` 和 `err_msg` 会保留在 `AgvResult.message`，并由
gRPC 同时写入 transport status message 和反馈头的 `errorMessage`。非 OK RPC
下，标准客户端通常不会交付响应体，因此跨客户端应以 transport status message
为准，不要依赖反馈头仍然可见。例如：
```text
SEER Robokit command failed: ret_code=43051, err_msg=planner_rejected_pose
```
控制器仅返回“已接收”不等于导航完成；同步接口仍要等待精确任务终态和停车
确认。若发送后连接中断且控制器是否执行已无法确定，错误会明确提示 outcome
unknown，调用方不能自动重发运动命令，应先查询状态并取消或停止。
## 安全边界
- 仙工文档明确把 `3051` 定位为任务链或验证测试等单车场景接口；不要把它当作
多车调度接口，否则可能出现路径/速度不连续等危险行为。
- `emergencyStop` 是控制器软件停止，不是功能安全急停；真实系统必须保留可达的
硬件急停、安全激光、碰撞条和独立安全链。
- 首次 PGV 测试应在低速、空载、隔离区域进行，并先核对二维码坐标系、传感器
上/下视方向、调整半径和中心偏移的标定值。
- PGV 同步成功目前能证明精确 `3051` 任务进入终态，并连续确认两次零速度；
仙工文档没有明确 `Completed` 是否一定覆盖 PGV 二次调整的全部阶段，仍需实机
验证后才能据此联动机械臂。异步成功更不代表 PGV 调整完成。
- 地图切换、地图上传和开始建图会改变控制器状态，也会先抢占控制权；不要和
现场调度系统并行操作。
- 本目录的假控制器测试验证软件协议、错误路径和并发逻辑，不代表真实 SEER Robokit、
底盘、PGV、地图或安全链已经验收。
## 测试
```bash
cmake --build build \
--target seer_robokit_control_authority_test grpc_agv_service_test \
-j2
ctest --test-dir build \
-R '^(seer_robokit_control_authority_test|grpc_agv_service_test)$' \
--output-on-failure
```
`seer_robokit_control_authority_test` 使用本机回环 TCP 假控制器，需要允许本地
bind/listen。受限沙箱若禁止创建 socket，只能证明编译通过，不能把未执行的
fake-controller 场景报告为测试通过。测试过程不会连接真实 AGV。

354
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h
View File

@ -1,354 +0,0 @@
#ifndef CMVR_ES_SEER_ROBOKIT_AGV_H
#define CMVR_ES_SEER_ROBOKIT_AGV_H
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <cstdint>
#include <deque>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include <json/json.h>
#include "cmvr/config/agv_config/agv_config.pb.h"
#include "devices/agv/abstract_agv.h"
namespace cmvr::device {
class SeerRobokitAgvTestPeer;
class SeerRobokitAgv final : public AbstractAGV {
public:
explicit SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg);
~SeerRobokitAgv() override;
std::string typeName() const override { return "SeerRobokitAgv"; }
bool supportsSynchronousAction(AgvActionKind kind) const noexcept override
{
switch (kind) {
case AgvActionKind::NavigateToPose:
case AgvActionKind::NavigateToStation:
case AgvActionKind::FollowPath:
return true;
}
return false;
}
bool init() override;
bool start() override;
bool stop() override;
bool update() override;
AgvRuntimeState runtimeState() const override;
AgvNavigationStatus navigationStatus() const override;
AgvResult emergencyStop() override;
AgvResult clearFault() override;
AgvResult navigateToPose(
const math::Pose2d& pose,
const AgvMotionOptions& options = {},
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
AgvResult navigateToStation(
const std::string& station_id,
const AgvMotionOptions& options = {},
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
AgvResult followPath(
const std::vector<AgvPathSegment>& path) override;
AgvResult followPath(
const std::vector<AgvPathSegment>& path,
const AgvMotionOptions& options) override;
AgvResult translate(const AgvTranslation& translation) override;
AgvResult pauseNavigation() override;
AgvResult resumeNavigation() override;
AgvResult cancelNavigation() override;
AgvResult setVelocity(const AgvVelocity& velocity) override;
AgvResult confirmMotionStopped() override;
AgvResult listMaps(std::vector<std::string>& maps) const override;
AgvResult listStations(std::vector<AgvStation>& stations) const override;
AgvResult switchMap(const std::string& map_name) override;
AgvResult uploadMap(const std::string& map_name, const std::string& content) override;
AgvResult downloadMap(const std::string& map_name, std::string& content) const override;
AgvResult startMapping(const AgvMappingOptions& options = {}) override;
AgvResult getMappingData(int start_index, AgvMappingData& data) const override;
AgvResult getUnifiedMapUpdate(
std::uint64_t after_sequence,
const AgvMapStreamOptions& options,
AgvUnifiedMapUpdate& update) const override;
AgvResult stopMapping() override;
private:
friend class SeerRobokitAgvTestPeer;
struct Ports {
int status{19204};
int control{19205};
int navigation{19206};
int config{19207};
int other{19210};
int push{19301};
};
struct PoseTaskStatus {
bool found{false};
int state{0};
int type{0};
bool type_present{false};
double progress{0.0};
std::string detail;
};
struct NavigationSnapshot {
int task_status{0};
int task_type{0};
bool task_status_present{false};
bool task_type_present{false};
bool blocked{false};
bool blocked_present{false};
int block_reason{-1};
std::string block_reason_raw;
bool velocity_present{false};
double vx{0.0};
double vy{0.0};
double w{0.0};
bool emergency{false};
std::string target_id;
std::string active_faults;
std::string detail;
};
enum class CommandTransmissionState {
NotSent,
PossiblySent,
};
struct TrackedNavigationContext {
std::string token;
std::vector<std::string> task_ids;
AgvTaskType type{AgvTaskType::None};
std::string target_id;
std::vector<std::string> target_ids;
std::uint64_t navigation_generation{0};
std::chrono::steady_clock::time_point accepted_at{};
bool synchronous_wait{false};
};
struct PoseTaskContext {
std::string task_id;
math::Pose2d target{};
double reach_distance{0.0};
double reach_angle{0.0};
std::uint64_t navigation_generation{0};
std::uint64_t controller_fault_sequence_at_start{0};
std::uint64_t control_attempt_sequence_at_start{0};
std::uint64_t controller_fault_channel_epoch_at_start{0};
};
AgvResult connect_();
AgvResult disconnect_();
AgvResult emergencyStopTrackedNavigation_(
const TrackedNavigationContext* expected_navigation);
AgvResult connectSocket_(int& sock, int port);
AgvResult ensureOtherSocket_();
void closeSocket_(int& sock) const;
bool connected_() const;
AgvResult acquireControl_() const;
AgvResult confirmPoseNavigationStarted_(
const PoseTaskContext& context,
bool accept_paused,
const AgvMotionOptions& options) const;
AgvResult waitForPoseNavigationTerminal_(
const PoseTaskContext& pose_context,
const TrackedNavigationContext& navigation_context,
const AgvMotionOptions& options);
AgvResult waitForTrackedNavigationTerminal_(
const TrackedNavigationContext& context,
const AgvMotionOptions& options);
AgvResult queryNavigationSnapshot_(NavigationSnapshot& snapshot) const;
AgvResult cancelTrackedNavigation_(
const TrackedNavigationContext& context,
std::uint64_t& accepted_generation);
AgvResult waitForCanceledTaskToStop_(
const TrackedNavigationContext& context,
const AgvMotionOptions& options,
const std::string& reason,
bool require_global_stopped = false);
AgvResult failAndCancelTrackedNavigation_(
const TrackedNavigationContext& context,
const AgvMotionOptions& options,
AgvErrorCode error_code,
const std::string& reason);
AgvResult queryPoseTaskStatus_(
const std::string& task_id,
PoseTaskStatus& status) const;
AgvResult queryTaskStatuses_(
const std::vector<std::string>& task_ids,
std::vector<PoseTaskStatus>& statuses) const;
bool poseTargetReached_(
const PoseTaskContext& context,
std::string& detail) const;
std::string cachedControllerFaultDetail_(
std::uint64_t after_sequence = 0,
int wait_ms = 0,
std::uint64_t* associated_control_attempt = nullptr) const;
int controllerFaultCaptureGraceMs_() const;
int controllerFaultStateMaxAgeMs_() const;
std::string freeNavigationFaultStateUnavailableDetail_() const;
void rememberPoseTask_(const PoseTaskContext& context) const;
void advancePoseTaskGeneration_(
std::uint64_t navigation_generation,
std::uint64_t control_attempt_sequence) const;
void advancePoseTaskControlAttempt_(
std::uint64_t control_attempt_sequence) const;
void clearPoseTask_(std::uint64_t navigation_generation) const;
void clearPoseTaskIfTaskId_(const std::string& task_id) const;
bool currentPoseTask_(PoseTaskContext& context) const;
void rememberTrackedNavigation_(
const TrackedNavigationContext& context) const;
void advanceTrackedNavigationGeneration_(
std::uint64_t navigation_generation) const;
void clearTrackedNavigation_(std::uint64_t navigation_generation) const;
void clearTrackedNavigationIfToken_(const std::string& token) const;
bool currentTrackedNavigation_(
TrackedNavigationContext& context) const;
AgvResult sendControlledCommand_(int sock,
std::uint16_t command,
const Json::Value& payload,
Json::Value* response,
std::uint64_t* accepted_navigation_generation = nullptr,
std::uint64_t* controller_fault_sequence_at_attempt = nullptr,
std::uint64_t* control_attempt_sequence = nullptr,
PoseTaskContext* pose_context_to_publish = nullptr,
bool reject_if_active_controller_fault = false,
TrackedNavigationContext* navigation_context_to_publish = nullptr,
bool preserve_tracked_navigation = false,
const std::string* expected_navigation_token = nullptr,
const std::function<bool()>* cancellation_requested = nullptr,
const TrackedNavigationContext* expected_active_navigation = nullptr) const;
AgvResult sendCommand_(int sock,
std::uint16_t command,
const Json::Value& payload,
Json::Value* response,
CommandTransmissionState* transmission_state = nullptr) const;
AgvResult sendCommandRaw_(int sock,
std::uint16_t command,
const Json::Value& payload,
std::string* response_payload,
CommandTransmissionState* transmission_state = nullptr) const;
AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const;
AgvResult configurePush_();
void startPushThread_();
void stopPushThread_();
void pushLoop_();
void invalidateControllerFaultState_();
AgvRuntimeState queryRuntimeState_() const;
void updateCachedRuntimeState_(const Json::Value& payload);
void startMapUpdateThread_();
void stopMapUpdateThread_();
void mapUpdateLoop_();
AgvResult refreshMapCacheOnce_(const AgvMapStreamOptions& options) const;
AgvResult parseMapFileToUpdates_(
const std::string& file_name,
const std::string& content,
const AgvMapStreamOptions& options,
std::vector<AgvUnifiedMapUpdate>& updates) const;
AgvResult parseSeerRobokitMapArchive_(
const std::string& file_name,
const std::string& content,
const AgvMapStreamOptions& options,
std::vector<AgvUnifiedMapUpdate>& updates) const;
AgvResult parseSeerRobokitMap2D_(
const std::string& file_name,
const std::string& content,
const AgvMapStreamOptions& options,
AgvUnifiedMapUpdate& update) const;
AgvResult parseSeerRobokitMap3D_(
const std::string& file_name,
const std::string& content,
const AgvMapStreamOptions& options,
AgvUnifiedMapUpdate& update) const;
void cacheMapUpdates_(std::vector<AgvUnifiedMapUpdate> updates) const;
bool findCachedMapUpdate_(
std::uint64_t after_sequence,
const AgvMapStreamOptions& options,
AgvUnifiedMapUpdate& update) const;
bool mapUpdateMatches_(
const AgvUnifiedMapUpdate& update,
const AgvMapStreamOptions& options) const;
static std::vector<std::uint8_t> buildFrame_(std::uint16_t command, const std::string& payload);
static std::string toJsonString_(const Json::Value& value);
static bool parseJson_(const std::string& input, Json::Value& output, std::string& error);
static std::string extractJson_(const std::string& raw);
static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload);
static void applyMotionOptions_(
Json::Value& payload,
const AgvMotionOptions& options,
bool include_reach_options = true);
static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
static AgvResult resultFromResponse_(const Json::Value& response);
config::SeerRobokitAgvConfig config_;
std::string ip_;
std::string control_nick_name_;
int recv_timeout_ms_{1000};
Ports ports_;
bool state_push_enabled_{false};
bool map_update_enabled_{false};
int map_update_interval_ms_{1000};
std::size_t map_update_history_size_{8};
mutable std::mutex mutex_;
mutable std::mutex status_io_mutex_;
mutable std::mutex control_sequence_mutex_;
mutable std::atomic<std::uint64_t> navigation_generation_{0};
mutable std::atomic<std::uint64_t> pose_task_sequence_{0};
mutable std::atomic<std::uint64_t> control_attempt_sequence_{0};
mutable std::atomic<std::uint64_t> controller_fault_channel_epoch_{0};
mutable std::mutex pose_task_mutex_;
mutable PoseTaskContext pose_task_context_;
mutable std::mutex tracked_navigation_mutex_;
mutable TrackedNavigationContext tracked_navigation_context_;
mutable int sock_status_{-1};
mutable int sock_control_{-1};
mutable int sock_navigation_{-1};
mutable int sock_config_{-1};
mutable int sock_other_{-1};
mutable int sock_push_{-1};
std::string last_error_;
std::atomic<bool> push_running_{false};
std::thread push_thread_;
mutable std::mutex runtime_state_mutex_;
mutable std::condition_variable runtime_state_cv_;
AgvRuntimeState cached_runtime_state_;
bool cached_runtime_state_valid_{false};
std::uint64_t controller_fault_sequence_{0};
bool controller_fault_state_observed_{false};
std::chrono::steady_clock::time_point controller_fault_state_observed_at_{};
std::string active_controller_fault_detail_;
double last_controller_fault_timestamp_{0.0};
std::string last_controller_fault_detail_;
std::uint64_t last_controller_fault_control_attempt_{0};
mutable std::atomic<bool> map_update_running_{false};
mutable std::thread map_update_thread_;
mutable std::mutex map_update_mutex_;
mutable std::condition_variable map_update_cv_;
mutable std::deque<AgvUnifiedMapUpdate> cached_map_updates_;
mutable std::uint64_t map_sequence_{0};
mutable int next_mapping_index_{0};
mutable std::size_t last_map_content_hash_{0};
mutable std::string map_session_id_;
};
} // namespace cmvr::device
#endif // CMVR_ES_SEER_ROBOKIT_AGV_H

257
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h
View File

@ -1,257 +0,0 @@
#ifndef CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H
#define CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <string>
#include <thread>
#include "devices/agv/abstract_agv.h"
namespace cmvr::device::seer_robokit::navigation {
constexpr auto kPoseNavigationStartTimeout = std::chrono::milliseconds(1500);
constexpr auto kPoseNavigationPollInterval = std::chrono::milliseconds(50);
constexpr int kPoseNavigationRequiredRunningSamples = 2;
constexpr auto kDefaultNavigationWaitTimeout =
std::chrono::milliseconds(600000);
constexpr auto kDefaultNavigationPollInterval =
std::chrono::milliseconds(200);
constexpr auto kMaximumNavigationPollInterval =
std::chrono::milliseconds(5000);
constexpr auto kNavigationCancellationCheckInterval =
std::chrono::milliseconds(50);
constexpr auto kNavigationCancelPollInterval =
std::chrono::milliseconds(100);
constexpr auto kNavigationCancelConfirmationTimeout =
std::chrono::milliseconds(3000);
constexpr int kRequiredBlockedStopSamples = 2;
constexpr int kRequiredCompletedStopSamples = 2;
constexpr double kNavigationStopVelocityTolerance = 0.005;
constexpr int kMinimumControllerFaultCaptureGraceMs = 250;
constexpr int kMaximumControllerFaultCaptureGraceMs = 5000;
constexpr int kDefaultControllerFaultPushIntervalMs = 1000;
constexpr int kControllerFaultPushJitterMs = 100;
constexpr int kMinimumControllerFaultStateMaxAgeMs = 2000;
constexpr int kControllerFaultStateMaxAgeIntervals = 5;
constexpr double kDefaultPoseReachDistance = 0.05;
constexpr double kDefaultPoseReachAngle = 0.10;
constexpr double kTwoPi = 6.28318530717958647692;
static inline bool exactTaskStateIsActive(const int state)
{
return state >= 1 && state <= 3;
}
static inline bool exactTaskStateIsKnownTerminal(const int state)
{
return state >= 4 && state <= 7;
}
static inline bool globalTaskStateIsKnownTerminal(const int state)
{
return state == 0 || exactTaskStateIsKnownTerminal(state);
}
static inline double angleDistance(const double lhs, const double rhs)
{
return std::abs(std::remainder(lhs - rhs, kTwoPi));
}
static inline AgvResult withUnknownControllerOutcome(AgvResult result)
{
const auto code = result.ok() ? AgvErrorCode::CommandFailed : result.code;
std::string detail = result.message.empty() ? "unknown transport or protocol error" : result.message;
detail +=
"; SEER Robokit controller outcome is unknown after the command attempt; "
"the command may already have taken effect; do not issue another motion "
"command automatically; query status and cancel or stop first";
return AgvResult::failure(code, detail);
}
static inline std::string makePoseTaskId(
const std::string& device_id,
const std::uint64_t task_sequence)
{
const auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
std::chrono::system_clock::now().time_since_epoch()).count();
const std::string prefix = device_id.empty() ? "cmvr-es" : device_id;
return prefix + "_pose_" + std::to_string(timestamp)
+ "_" + std::to_string(task_sequence);
}
static inline std::string makeNavigationTaskId(
const std::string& device_id,
const char* kind,
const std::uint64_t task_sequence)
{
const auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
std::chrono::system_clock::now().time_since_epoch()).count();
const std::string prefix = device_id.empty() ? "cmvr-es" : device_id;
return prefix + "_" + kind + "_" + std::to_string(timestamp)
+ "_" + std::to_string(task_sequence);
}
static inline const char* blockReasonName(const int reason)
{
switch (reason) {
case 0:
return "ultrasonic";
case 1:
return "laser";
case 2:
return "fallingdown";
case 3:
return "collision";
case 4:
return "infrared";
case 5:
return "locked";
default:
return "unknown";
}
}
static inline std::string invalidMotionOption(const AgvMotionOptions& options)
{
const auto non_negative_error = [](const double value, const char* field) {
if (!std::isfinite(value)) {
return std::string(field) + " must be finite";
}
if (value < 0.0) {
return std::string(field) + " must be non-negative";
}
return std::string{};
};
if (auto error = non_negative_error(options.max_speed, "max_speed");
!error.empty()) return error;
if (auto error = non_negative_error(
options.max_angular_speed,
"max_angular_speed");
!error.empty()) return error;
if (auto error = non_negative_error(
options.max_acceleration,
"max_acceleration");
!error.empty()) return error;
if (auto error = non_negative_error(
options.max_angular_acceleration,
"max_angular_acceleration");
!error.empty()) return error;
if (auto error = non_negative_error(
options.reach_distance,
"reach_distance");
!error.empty()) return error;
if (auto error = non_negative_error(options.reach_angle, "reach_angle");
!error.empty()) return error;
if (auto error = non_negative_error(options.speed_ratio, "speed_ratio");
!error.empty()) return error;
if (options.wait_timeout_ms < 0) {
return "wait_timeout_ms must be non-negative";
}
if (options.poll_interval_ms < 0) {
return "poll_interval_ms must be non-negative";
}
if (options.poll_interval_ms
> kMaximumNavigationPollInterval.count()) {
return "poll_interval_ms must not exceed "
+ std::to_string(kMaximumNavigationPollInterval.count());
}
if (options.wait_timeout_ms > 0
&& options.poll_interval_ms > options.wait_timeout_ms) {
return "poll_interval_ms must not exceed wait_timeout_ms";
}
return {};
}
static inline std::chrono::milliseconds navigationWaitTimeout(
const AgvMotionOptions& options)
{
return options.wait_timeout_ms > 0
? std::chrono::milliseconds(options.wait_timeout_ms)
: kDefaultNavigationWaitTimeout;
}
static inline std::chrono::milliseconds navigationPollInterval(
const AgvMotionOptions& options)
{
if (options.poll_interval_ms <= 0) {
return kDefaultNavigationPollInterval;
}
return std::chrono::milliseconds(
std::max(options.poll_interval_ms, 20));
}
static inline bool navigationCancellationRequested(const AgvMotionOptions& options)
{
return options.cancellation_requested
&& options.cancellation_requested();
}
static inline void sleepForNavigationPoll(
const std::chrono::milliseconds poll_interval,
const std::chrono::steady_clock::time_point overall_deadline,
const AgvMotionOptions& options)
{
const auto poll_deadline = std::min(
overall_deadline,
std::chrono::steady_clock::now() + poll_interval);
while (std::chrono::steady_clock::now() < poll_deadline
&& !navigationCancellationRequested(options)) {
const auto remaining = std::chrono::duration_cast<std::chrono::milliseconds>(
poll_deadline - std::chrono::steady_clock::now());
if (remaining <= std::chrono::milliseconds::zero()) {
break;
}
std::this_thread::sleep_for(std::min(
kNavigationCancellationCheckInterval,
remaining));
}
}
template <typename Snapshot>
static inline bool navigationStopped(const Snapshot& snapshot)
{
return snapshot.velocity_present
&& std::abs(snapshot.vx) <= kNavigationStopVelocityTolerance
&& std::abs(snapshot.vy) <= kNavigationStopVelocityTolerance
&& std::abs(snapshot.w) <= kNavigationStopVelocityTolerance;
}
static inline AgvResult reconciledNavigationResult(
const AgvResult& command_result,
AgvResult terminal_result)
{
if (command_result.ok()) {
return terminal_result;
}
if (terminal_result.ok()) {
terminal_result.message =
"SEER Robokit navigation completed after an indeterminate command "
"acknowledgment; initial_detail=" + command_result.message;
return terminal_result;
}
terminal_result.message =
"SEER Robokit navigation command acknowledgment was indeterminate: "
+ command_result.message + "; status reconciliation: "
+ terminal_result.message;
return terminal_result;
}
static inline bool parseFiniteDouble(const std::string& value, double& parsed)
{
std::size_t consumed = 0;
try {
parsed = std::stod(value, &consumed);
} catch (...) {
return false;
}
return consumed == value.size() && std::isfinite(parsed);
}
} // namespace cmvr::device::seer_robokit::navigation
#endif // CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H

141
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_pgv_utils.h
View File

@ -1,141 +0,0 @@
#ifndef CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H
#define CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H
#include <string>
#include <json/json.h>
#include "devices/agv/abstract_agv.h"
#include "seer_robokit_navigation_utils.h"
#include "seer_robokit_utils.h"
namespace cmvr::device::seer_robokit::pgv {
constexpr char kUsePgv[] = "use_pgv";
constexpr char kPgvAdjustDist[] = "pgv_adjust_dist";
constexpr char kPgvAdjustCx[] = "pgv_adjust_cx";
constexpr char kPgvAdjustCy[] = "pgv_adjust_cy";
constexpr char kPgvXAdjust[] = "pgv_x_adjust";
constexpr char kUseDownPgv[] = "use_down_pgv";
// These spellings currently appear in the vendor document, but conflict with
// its own field table/example and the repository's older working serializer.
constexpr char kMalformedAdjustDist[] = "pgv_adjustuse_pgv_dist";
constexpr char kAdjustCyDocumentAlias[] = "pgv_ajdust_cy";
static inline bool isPgvAdjustmentKey(const std::string& key)
{
return key == kUsePgv
|| key == kPgvAdjustDist
|| key == kPgvAdjustCx
|| key == kPgvAdjustCy
|| key == kPgvXAdjust
|| key == kUseDownPgv
|| key == kMalformedAdjustDist
|| key == kAdjustCyDocumentAlias;
}
static inline bool hasPgvAdjustmentParams(const AgvAdapterParams& params)
{
for (const auto& [key, value] : params.values) {
(void)value;
if (isPgvAdjustmentKey(key)) {
return true;
}
}
return false;
}
/**
* Parse the string-valued generic adapter parameters into the native JSON
* types required by SEER Robokit API 3051. Returns an error string without
* modifying controller state; an empty string means success.
*/
static inline std::string applyPgvAdjustmentParams(
Json::Value& payload,
const AgvAdapterParams& params)
{
if (params.getString(kMalformedAdjustDist)) {
return std::string(kMalformedAdjustDist)
+ " is a vendor-document typo; use " + kPgvAdjustDist;
}
if (hasPgvAdjustmentParams(params)) {
for (const auto& [key, value] : params.values) {
(void)value;
if (!isPgvAdjustmentKey(key)
&& key != "source_id"
&& key != "task_id") {
return "PGV adjustment must not be combined with adapter "
"field " + key;
}
}
}
const auto adjust_cy = params.getString(kPgvAdjustCy);
const auto adjust_cy_alias = params.getString(kAdjustCyDocumentAlias);
if (adjust_cy && adjust_cy_alias) {
return std::string(kPgvAdjustCy) + " and its vendor-document alias "
+ kAdjustCyDocumentAlias + " must not both be set";
}
const auto apply_bool = [&payload, &params](const char* key) {
if (!params.getString(key)) {
return std::string{};
}
const auto parsed = params.getBool(key);
if (!parsed) {
return std::string(key)
+ " must be a boolean string such as true or false";
}
detail::jsonMember(payload, key) = *parsed;
return std::string{};
};
if (auto error = apply_bool(kUsePgv); !error.empty()) {
return error;
}
if (auto error = apply_bool(kUseDownPgv); !error.empty()) {
return error;
}
const auto apply_number = [&payload, &params](
const char* key,
const bool non_negative) {
const auto raw = params.getString(key);
if (!raw) {
return std::string{};
}
double parsed = 0.0;
if (!navigation::parseFiniteDouble(*raw, parsed)) {
return std::string(key) + " must be a complete finite number";
}
if (non_negative && parsed < 0.0) {
return std::string(key) + " must be non-negative";
}
detail::jsonMember(payload, key) = parsed;
return std::string{};
};
if (auto error = apply_number(kPgvAdjustDist, true); !error.empty()) {
return error;
}
if (auto error = apply_number(kPgvAdjustCx, false); !error.empty()) {
return error;
}
if (adjust_cy_alias) {
double parsed = 0.0;
if (!navigation::parseFiniteDouble(*adjust_cy_alias, parsed)) {
return std::string(kAdjustCyDocumentAlias)
+ " must be a complete finite number";
}
detail::jsonMember(payload, kPgvAdjustCy) = parsed;
} else if (auto error = apply_number(kPgvAdjustCy, false);
!error.empty()) {
return error;
}
if (auto error = apply_number(kPgvXAdjust, false); !error.empty()) {
return error;
}
return {};
}
} // namespace cmvr::device::seer_robokit::pgv
#endif // CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H

39
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h
View File

@ -1,39 +0,0 @@
#ifndef CMVR_ES_SEER_ROBOKIT_PROTOCOL_H
#define CMVR_ES_SEER_ROBOKIT_PROTOCOL_H
#include <cstdint>
namespace cmvr::device::seer_robokit::protocol {
constexpr std::uint16_t kRobotStatusLoc = 1004;
constexpr std::uint16_t kRobotStatusBattery = 1007;
constexpr std::uint16_t kRobotStatusAll2 = 1101;
constexpr std::uint16_t kRobotStatusTask = 1020;
constexpr std::uint16_t kRobotStatusTaskPackage = 1110;
constexpr std::uint16_t kRobotStatusMap = 1300;
constexpr std::uint16_t kRobotStatusStation = 1301;
constexpr std::uint16_t kRobotStatusMappingFileList = 1780;
constexpr std::uint16_t kRobotStatusDownloadFile = 1800;
constexpr std::uint16_t kRobotControlStop = 2000;
constexpr std::uint16_t kRobotControlMotion = 2010;
constexpr std::uint16_t kRobotControlLoadMap = 2022;
constexpr std::uint16_t kRobotTaskPause = 3001;
constexpr std::uint16_t kRobotTaskResume = 3002;
constexpr std::uint16_t kRobotTaskCancel = 3003;
constexpr std::uint16_t kRobotTaskGoTarget = 3051;
constexpr std::uint16_t kRobotTaskTranslate = 3055;
constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
constexpr std::uint16_t kRobotTaskClearTargetList = 3067;
constexpr std::uint16_t kRobotConfigLock = 4005;
constexpr std::uint16_t kRobotConfigUploadMap = 4010;
constexpr std::uint16_t kRobotConfigDownloadMap = 4011;
constexpr std::uint16_t kRobotOtherStartMapping = 6100;
constexpr std::uint16_t kRobotOtherStopMapping = 6101;
constexpr std::uint16_t kRobotPushConfigReq = 9300;
constexpr std::uint16_t kRobotPushConfigRes = 19300;
constexpr std::uint16_t kRobotPush = 19301;
constexpr std::uint32_t kMaxFramePayloadBytes = 512U * 1024U * 1024U;
} // namespace cmvr::device::seer_robokit::protocol
#endif // CMVR_ES_SEER_ROBOKIT_PROTOCOL_H

92
cmvr-es/devices/agv/seer_robokit/include/seer_robokit_utils.h
View File

@ -1,92 +0,0 @@
#ifndef CMVR_ES_SEER_ROBOKIT_UTILS_H
#define CMVR_ES_SEER_ROBOKIT_UTILS_H
#include <cerrno>
#include <chrono>
#include <cstring>
#include <string>
#include <json/json.h>
namespace cmvr::device::seer_robokit::detail {
static inline std::string systemError()
{
return std::strerror(errno);
}
static inline Json::Value& jsonMember(
Json::Value& value,
const char* key)
{
return *value.demand(key, key + std::strlen(key));
}
static inline Json::Value& jsonMember(
Json::Value& value,
const std::string& key)
{
return *value.demand(key.data(), key.data() + key.size());
}
static inline const Json::Value* jsonFind(
const Json::Value& value,
const char* key)
{
return value.find(key, key + std::strlen(key));
}
static inline Json::Value jsonGet(
const Json::Value& value,
const char* key,
const Json::Value& fallback)
{
const auto* found = jsonFind(value, key);
return found ? *found : fallback;
}
static inline double nowSeconds()
{
const auto now = std::chrono::system_clock::now().time_since_epoch();
return std::chrono::duration<double>(now).count();
}
static inline bool jsonHas(
const Json::Value& value,
const char* key)
{
return jsonFind(value, key) != nullptr;
}
static inline bool hasNumericControllerRetCode(
const Json::Value& response)
{
const auto* ret_code = jsonFind(response, "ret_code");
return ret_code
&& (ret_code->isInt()
|| ret_code->isUInt()
|| ret_code->isInt64()
|| ret_code->isUInt64());
}
static inline std::string jsonValueToString(const Json::Value& value)
{
if (value.isString()) return value.asString();
if (value.isBool()) return value.asBool() ? "true" : "false";
if (value.isInt64() || value.isInt()) {
return std::to_string(value.asInt64());
}
if (value.isUInt64() || value.isUInt()) {
return std::to_string(value.asUInt64());
}
if (value.isDouble()) return std::to_string(value.asDouble());
if (value.isNull()) return {};
Json::StreamWriterBuilder builder;
builder["indentation"] = "";
return Json::writeString(builder, value);
}
} // namespace cmvr::device::seer_robokit::detail
#endif // CMVR_ES_SEER_ROBOKIT_UTILS_H

175
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp
View File

@ -1,175 +0,0 @@
#include "seer_robokit_agv.h"
#include <atomic>
#include <cstddef>
#include <mutex>
#include "common/base/logging/logger.h"
namespace cmvr::device {
namespace {
constexpr int kDefaultMapUpdateIntervalMs = 1000;
constexpr std::size_t kDefaultMapUpdateHistorySize = 8;
} // namespace
SeerRobokitAgv::SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg)
: config_(cfg),
ip_(cfg.ip()),
control_nick_name_(
cfg.control_nick_name().empty()
? (cfg.id().empty() ? "cmvr-es" : "cmvr-es:" + cfg.id())
: cfg.control_nick_name()),
recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000),
state_push_enabled_(cfg.enable_state_push()),
map_update_enabled_(cfg.enable_map_update()),
map_update_interval_ms_(cfg.map_update_interval_ms() > 0 ? cfg.map_update_interval_ms() : kDefaultMapUpdateIntervalMs),
map_update_history_size_(cfg.map_update_history_size() > 0 ? cfg.map_update_history_size() : kDefaultMapUpdateHistorySize)
{
id_ = cfg.id();
if (cfg.port_status() > 0) ports_.status = cfg.port_status();
if (cfg.port_control() > 0) ports_.control = cfg.port_control();
if (cfg.port_nav() > 0) ports_.navigation = cfg.port_nav();
if (cfg.port_config() > 0) ports_.config = cfg.port_config();
if (cfg.port_other() > 0) ports_.other = cfg.port_other();
if (cfg.port_push() > 0) ports_.push = cfg.port_push();
const auto result = connect_();
if (!result.ok()) {
CMVR_LOG(ERROR) << "[SeerRobokitAgv] Auto connect failed"
<< ", id=" << id_
<< ", ip=" << ip_
<< ", error=" << result.message;
}
}
SeerRobokitAgv::~SeerRobokitAgv()
{
(void)disconnect_();
}
bool SeerRobokitAgv::init()
{
return !id_.empty() && !ip_.empty();
}
bool SeerRobokitAgv::start()
{
return true;
}
bool SeerRobokitAgv::stop()
{
return true;
}
bool SeerRobokitAgv::update()
{
return true;
}
AgvResult SeerRobokitAgv::connect_()
{
const auto lifecycle_generation =
navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
clearPoseTask_(lifecycle_generation);
clearTrackedNavigation_(lifecycle_generation);
stopPushThread_();
stopMapUpdateThread_();
{
// Status requests may wait for a controller receive timeout without
// holding mutex_. Serialize lifecycle changes with that channel before
// replacing or closing its descriptor.
std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
std::lock_guard<std::mutex> lock(mutex_);
closeSocket_(sock_status_);
closeSocket_(sock_control_);
closeSocket_(sock_navigation_);
closeSocket_(sock_config_);
closeSocket_(sock_other_);
closeSocket_(sock_push_);
if (ip_.empty()) {
return AgvResult::failure(AgvErrorCode::InvalidArgument, "SEER Robokit AGV ip is empty");
}
const auto close_all = [this]() {
closeSocket_(sock_status_);
closeSocket_(sock_control_);
closeSocket_(sock_navigation_);
closeSocket_(sock_config_);
closeSocket_(sock_other_);
closeSocket_(sock_push_);
};
if (auto result = connectSocket_(sock_status_, ports_.status); !result.ok()) {
close_all();
return result;
}
if (auto result = connectSocket_(sock_control_, ports_.control); !result.ok()) {
close_all();
return result;
}
if (auto result = connectSocket_(sock_navigation_, ports_.navigation); !result.ok()) {
close_all();
return result;
}
if (auto result = connectSocket_(sock_config_, ports_.config); !result.ok()) {
close_all();
return result;
}
if (state_push_enabled_) {
const auto result = connectSocket_(sock_push_, ports_.push);
if (!result.ok()) {
CMVR_LOG(ERROR) << "[SeerRobokitAgv] Connect push port failed"
<< ", id=" << id_
<< ", port=" << ports_.push
<< ", error=" << result.message;
closeSocket_(sock_push_);
}
}
last_error_.clear();
}
if (state_push_enabled_ && sock_push_ >= 0) {
const auto result = configurePush_();
if (result.ok()) {
startPushThread_();
} else {
CMVR_LOG(ERROR) << "[SeerRobokitAgv] Configure push failed"
<< ", id=" << id_
<< ", error=" << result.message;
std::lock_guard<std::mutex> lock(mutex_);
closeSocket_(sock_push_);
}
}
if (map_update_enabled_) {
startMapUpdateThread_();
}
return AgvResult::success();
}
AgvResult SeerRobokitAgv::disconnect_()
{
const auto lifecycle_generation =
navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
clearPoseTask_(lifecycle_generation);
clearTrackedNavigation_(lifecycle_generation);
stopMapUpdateThread_();
stopPushThread_();
std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
std::lock_guard<std::mutex> lock(mutex_);
closeSocket_(sock_status_);
closeSocket_(sock_control_);
closeSocket_(sock_navigation_);
closeSocket_(sock_config_);
closeSocket_(sock_other_);
closeSocket_(sock_push_);
return AgvResult::success();
}
} // namespace cmvr::device

378
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp
View File

@ -1,378 +0,0 @@
#include "seer_robokit_agv.h"
#include "seer_robokit_navigation_utils.h"
#include "seer_robokit_protocol.h"
#include "seer_robokit_utils.h"
#include <algorithm>
#include <chrono>
#include <cstdint>
#include <functional>
#include <mutex>
#include <string>
#include <utility>
#include <vector>
namespace cmvr::device {
using namespace seer_robokit::navigation;
using namespace seer_robokit::protocol;
using namespace seer_robokit::detail;
AgvResult SeerRobokitAgv::acquireControl_() const
{
Json::Value payload(Json::objectValue);
jsonMember(payload, "nick_name") = control_nick_name_;
Json::Value response;
auto result = sendCommand_(sock_config_, kRobotConfigLock, payload, &response);
return result.ok() ? resultFromResponse_(response) : result;
}
AgvResult SeerRobokitAgv::sendControlledCommand_(
const int sock,
const std::uint16_t command,
const Json::Value& payload,
Json::Value* response,
std::uint64_t* accepted_navigation_generation,
std::uint64_t* controller_fault_sequence_at_attempt,
std::uint64_t* control_attempt_sequence,
PoseTaskContext* pose_context_to_publish,
const bool reject_if_active_controller_fault,
TrackedNavigationContext* navigation_context_to_publish,
const bool preserve_tracked_navigation,
const std::string* expected_navigation_token,
const std::function<bool()>* cancellation_requested,
const TrackedNavigationContext* expected_active_navigation) const
{
const auto canceled_before_send = [cancellation_requested]() {
return cancellation_requested
&& *cancellation_requested
&& (*cancellation_requested)();
};
if (canceled_before_send()) {
return AgvResult::failure(
AgvErrorCode::TaskCanceled,
"SEER Robokit command was not sent because the caller canceled the "
"operation before control authority was acquired");
}
const auto expected_context_is_current =
[this, expected_active_navigation]() {
if (!expected_active_navigation) {
return true;
}
TrackedNavigationContext active_context;
return currentTrackedNavigation_(active_context)
&& active_context.token
== expected_active_navigation->token
&& active_context.navigation_generation
== expected_active_navigation->navigation_generation
&& active_context.type
== expected_active_navigation->type
&& navigation_generation_.load(std::memory_order_relaxed)
== expected_active_navigation->navigation_generation;
};
// Conditional cancel ownership checks are deliberately performed without
// the control sequencing mutex. A slow 1110/1101 response must never
// prevent emergencyStop() from acquiring authority and sending 2000.
// The exact local token/generation/type is revalidated under the control
// lock both before and after authority acquisition below.
if (expected_active_navigation) {
if (!expected_context_is_current()) {
return AgvResult::failure(
AgvErrorCode::TaskCanceled,
"SEER Robokit did not start conditional navigation cancel "
"preflight because the tracked task was already replaced or "
"ended");
}
std::vector<PoseTaskStatus> statuses;
const auto exact_result = queryTaskStatuses_(
expected_active_navigation->task_ids,
statuses);
if (!exact_result.ok()) {
return AgvResult::failure(
exact_result.code,
"SEER Robokit did not send the conditional navigation cancel "
"because exact task ownership preflight failed: "
+ exact_result.message);
}
const bool all_exact_tasks_terminal = !statuses.empty()
&& std::all_of(
statuses.begin(),
statuses.end(),
[](const PoseTaskStatus& status) {
return status.found
&& exactTaskStateIsKnownTerminal(status.state);
});
if (all_exact_tasks_terminal) {
return AgvResult::success();
}
const bool exact_task_still_active = std::any_of(
statuses.begin(),
statuses.end(),
[](const PoseTaskStatus& status) {
return status.found
&& exactTaskStateIsActive(status.state);
});
NavigationSnapshot snapshot;
const auto snapshot_result = queryNavigationSnapshot_(snapshot);
if (!snapshot_result.ok()) {
return AgvResult::failure(
snapshot_result.code,
"SEER Robokit did not send the conditional navigation cancel "
"because 1101 ownership preflight was unavailable: "
+ snapshot_result.message);
}
const int expected_type = expected_active_navigation->type
== AgvTaskType::NavigateToPose
? 1
: (expected_active_navigation->type
== AgvTaskType::NavigateToStation
? 2
: 3);
const bool global_active =
exactTaskStateIsActive(snapshot.task_status);
const bool target_conflicts = global_active
&& !snapshot.target_id.empty()
&& !expected_active_navigation->target_ids.empty()
&& std::find(
expected_active_navigation->target_ids.begin(),
expected_active_navigation->target_ids.end(),
snapshot.target_id)
== expected_active_navigation->target_ids.end();
if (global_active
&& (snapshot.task_type != expected_type
|| target_conflicts)) {
return AgvResult::failure(
AgvErrorCode::TaskCanceled,
"SEER Robokit did not send the conditional navigation cancel "
"because 1101 reports another active task: "
+ snapshot.detail);
}
if (!exact_task_still_active) {
const bool clearing_path_queue =
expected_active_navigation->type
== AgvTaskType::FollowPath;
const bool terminal_target_matches =
expected_active_navigation->type
!= AgvTaskType::NavigateToStation
|| (!snapshot.target_id.empty()
&& snapshot.target_id
== expected_active_navigation->target_id);
if (globalTaskStateIsKnownTerminal(snapshot.task_status)
&& snapshot.task_status != 0
&& !clearing_path_queue
&& snapshot.task_type == expected_type
&& terminal_target_matches) {
return AgvResult::success();
}
}
}
// Keep the permission acquisition and the following write ordered with
// respect to other control RPCs in this process. Channel I/O serialization
// is separate, so this must remain a distinct lock.
std::lock_guard<std::mutex> sequence_lock(control_sequence_mutex_);
if (expected_navigation_token) {
TrackedNavigationContext active_context;
const bool has_active_context =
currentTrackedNavigation_(active_context);
const bool expected_context_matches = expected_active_navigation
? (has_active_context
&& active_context.token
== expected_active_navigation->token
&& active_context.navigation_generation
== expected_active_navigation->navigation_generation
&& active_context.type
== expected_active_navigation->type
&& navigation_generation_.load(std::memory_order_relaxed)
== expected_active_navigation->navigation_generation)
: (has_active_context
&& active_context.token == *expected_navigation_token);
if (!expected_context_matches) {
return AgvResult::failure(
AgvErrorCode::TaskCanceled,
"SEER Robokit did not send the conditional navigation cancel "
"because the tracked task was already replaced or ended; "
"expected_token=" + *expected_navigation_token
+ (active_context.token.empty()
? std::string(", active_token=<none>")
: ", active_token=" + active_context.token));
}
}
const auto attempt_sequence =
control_attempt_sequence_.fetch_add(
1,
std::memory_order_relaxed) + 1;
if (control_attempt_sequence) {
*control_attempt_sequence = attempt_sequence;
}
if (pose_context_to_publish) {
pose_context_to_publish->control_attempt_sequence_at_start =
attempt_sequence;
}
const auto authority = acquireControl_();
if (!authority.ok()) {
const std::string detail = authority.message.empty() ? "unknown error" : authority.message;
return AgvResult::failure(
authority.code,
"SEER Robokit acquire control authority failed: " + detail);
}
if (expected_active_navigation && !expected_context_is_current()) {
return AgvResult::failure(
AgvErrorCode::TaskCanceled,
"SEER Robokit did not send the conditional navigation cancel because "
"the tracked token, generation, or type changed while control "
"authority was being acquired");
}
if (canceled_before_send()) {
return AgvResult::failure(
AgvErrorCode::TaskCanceled,
"SEER Robokit command was not sent because the caller canceled the "
"operation while control authority was being acquired");
}
std::string controller_fault_gate_error;
if (controller_fault_sequence_at_attempt
|| pose_context_to_publish
|| reject_if_active_controller_fault) {
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
if (controller_fault_sequence_at_attempt) {
*controller_fault_sequence_at_attempt =
controller_fault_sequence_;
}
if (pose_context_to_publish) {
pose_context_to_publish->controller_fault_sequence_at_start =
controller_fault_sequence_;
pose_context_to_publish
->controller_fault_channel_epoch_at_start =
controller_fault_channel_epoch_.load(
std::memory_order_relaxed);
}
if (reject_if_active_controller_fault) {
if (!state_push_enabled_) {
controller_fault_gate_error =
"controller fault state is unavailable because state push "
"is disabled";
} else if (!active_controller_fault_detail_.empty()) {
controller_fault_gate_error =
"the controller reported a fault or invalid fault state: "
+ active_controller_fault_detail_;
} else if (!controller_fault_state_observed_) {
controller_fault_gate_error =
"no state push containing fatals/errors has been observed";
} else {
const auto fault_state_age =
std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::steady_clock::now()
- controller_fault_state_observed_at_)
.count();
if (fault_state_age > controllerFaultStateMaxAgeMs_()) {
controller_fault_gate_error =
"the most recent fatals/errors state push is stale "
"(age_ms=" + std::to_string(fault_state_age)
+ ", max_age_ms="
+ std::to_string(controllerFaultStateMaxAgeMs_())
+ ")";
}
}
}
}
if (!controller_fault_gate_error.empty()) {
return AgvResult::failure(
AgvErrorCode::Fault,
"SEER Robokit free-navigation command was not sent because "
+ controller_fault_gate_error);
}
if (canceled_before_send()) {
return AgvResult::failure(
AgvErrorCode::TaskCanceled,
"SEER Robokit command was not sent because the caller canceled the "
"operation before the controller command write");
}
if (canceled_before_send()) {
return AgvResult::failure(
AgvErrorCode::TaskCanceled,
"SEER Robokit command was not sent because the caller canceled the "
"operation immediately before the controller command write");
}
const auto publish_navigation_generation =
[this,
accepted_navigation_generation,
pose_context_to_publish,
navigation_context_to_publish,
preserve_tracked_navigation]() {
const auto generation =
navigation_generation_.fetch_add(
1,
std::memory_order_relaxed) + 1;
*accepted_navigation_generation = generation;
if (pose_context_to_publish) {
pose_context_to_publish->navigation_generation = generation;
rememberPoseTask_(*pose_context_to_publish);
}
if (navigation_context_to_publish) {
navigation_context_to_publish->navigation_generation =
generation;
navigation_context_to_publish->accepted_at =
std::chrono::steady_clock::now();
rememberTrackedNavigation_(
*navigation_context_to_publish);
} else if (preserve_tracked_navigation) {
advanceTrackedNavigationGeneration_(generation);
} else {
clearTrackedNavigation_(generation);
}
};
CommandTransmissionState transmission_state =
CommandTransmissionState::NotSent;
auto result = sendCommand_(
sock,
command,
payload,
response,
&transmission_state);
if (!result.ok()) {
if (accepted_navigation_generation
&& transmission_state
== CommandTransmissionState::PossiblySent) {
// Once the control write has been attempted, a timeout, disconnect,
// wrong response opcode, or malformed JSON cannot prove rejection:
// the controller may already have executed the command.
publish_navigation_generation();
return withUnknownControllerOutcome(std::move(result));
}
return result;
}
if (!accepted_navigation_generation) {
return result;
}
if (!response) {
publish_navigation_generation();
return withUnknownControllerOutcome(AgvResult::failure(
AgvErrorCode::CommandFailed,
"SEER Robokit cannot confirm navigation command without a response"));
}
if (!hasNumericControllerRetCode(*response)) {
publish_navigation_generation();
return withUnknownControllerOutcome(resultFromResponse_(*response));
}
result = resultFromResponse_(*response);
if (!result.ok()) {
return result;
}
// Advance only after the controller accepted the command, and do it before
// releasing control_sequence_mutex_. This prevents a failed cancel/pause or
// failed authority acquisition from falsely reporting a pose task canceled,
// while preserving the controller's actual command order under concurrency.
publish_navigation_generation();
return result;
}
} // namespace cmvr::device

1047
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp
View File

File diff suppressed because it is too large Load Diff

1440
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp
View File

File diff suppressed because it is too large Load Diff

1619
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp
View File

File diff suppressed because it is too large Load Diff

725
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp
View File

@ -1,725 +0,0 @@
#include "seer_robokit_agv.h"
#include "seer_robokit_protocol.h"
#include "seer_robokit_utils.h"
#include <chrono>
#include <cmath>
#include <cstdint>
#include <mutex>
#include <sstream>
#include <string>
#include <sys/socket.h>
#include <thread>
#include <google/protobuf/repeated_ptr_field.h>
namespace cmvr::device {
using namespace seer_robokit::protocol;
using namespace seer_robokit::detail;
namespace {
bool hasFaultArray(const Json::Value& value, const char* key)
{
const auto* found = jsonFind(value, key);
return found && found->isArray() && !found->empty();
}
void appendStringArray(Json::Value& value, const char* key, const google::protobuf::RepeatedPtrField<std::string>& strings)
{
if (strings.empty()) {
return;
}
Json::Value array(Json::arrayValue);
for (const auto& item : strings) {
array.append(item);
}
jsonMember(value, key) = array;
}
AgvMode modeFromTaskState(const int state)
{
switch (state) {
case 2:
return AgvMode::Auto;
case 3:
return AgvMode::Paused;
case 5:
return AgvMode::Fault;
case 6:
return AgvMode::Stopped;
default:
return AgvMode::Idle;
}
}
AgvTaskState toTaskState(const int value)
{
switch (value) {
case 1:
return AgvTaskState::Waiting;
case 2:
return AgvTaskState::Running;
case 3:
return AgvTaskState::Paused;
case 4:
return AgvTaskState::Completed;
case 5:
case 7:
return AgvTaskState::Failed;
case 6:
return AgvTaskState::Canceled;
case 0:
default:
return AgvTaskState::None;
}
}
AgvTaskType toTaskType(const int value)
{
switch (value) {
case 1:
return AgvTaskType::NavigateToPose;
case 2:
return AgvTaskType::NavigateToStation;
case 3:
return AgvTaskType::FollowPath;
case 100:
return AgvTaskType::Custom;
default:
return AgvTaskType::None;
}
}
} // namespace
AgvRuntimeState SeerRobokitAgv::runtimeState() const
{
AgvRuntimeState cached_state;
bool has_cached_state = false;
if (state_push_enabled_) {
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
if (cached_runtime_state_valid_) {
cached_state = cached_runtime_state_;
has_cached_state = true;
}
}
if (has_cached_state) {
std::string adapter_error;
{
std::lock_guard<std::mutex> lock(mutex_);
cached_state.connected = connected_();
adapter_error = last_error_;
}
if (!adapter_error.empty()) {
if (cached_state.last_error.empty()) {
cached_state.last_error = adapter_error;
} else if (cached_state.last_error != adapter_error) {
cached_state.last_error += "; adapter_error=" + adapter_error;
}
}
if (!cached_state.connected) {
cached_state.mode = AgvMode::Disconnected;
}
return cached_state;
}
return queryRuntimeState_();
}
AgvRuntimeState SeerRobokitAgv::queryRuntimeState_() const
{
AgvRuntimeState state;
{
std::lock_guard<std::mutex> lock(mutex_);
state.connected = connected_();
state.last_error = last_error_;
}
state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected;
Json::Value loc;
if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) {
state.pose.x = jsonGet(loc, "x", 0.0).asDouble();
state.pose.y = jsonGet(loc, "y", 0.0).asDouble();
state.pose.theta = jsonGet(loc, "angle", 0.0).asDouble();
state.localized = jsonGet(loc, "confidence", 0.0).asDouble() > 0.0;
state.current_station = jsonGet(loc, "current_station", "").asString();
}
Json::Value battery;
if (sendCommand_(sock_status_, kRobotStatusBattery, Json::Value(Json::objectValue), &battery).ok()) {
state.battery.percentage = jsonGet(battery, "battery_level", 0.0).asDouble();
state.battery.temperature = jsonGet(battery, "battery_temp", 0.0).asDouble();
state.battery.charging = jsonGet(battery, "charging", false).asBool();
state.battery.voltage = jsonGet(battery, "voltage", 0.0).asDouble();
state.battery.current = jsonGet(battery, "current", 0.0).asDouble();
}
Json::Value map;
if (sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &map).ok()) {
state.current_map = jsonGet(map, "current_map", "").asString();
}
const auto nav = navigationStatus();
state.moving = nav.state == AgvTaskState::Running;
state.fault = nav.state == AgvTaskState::Failed;
state.mode = state.fault ? AgvMode::Fault : modeFromTaskState(static_cast<int>(nav.state));
return state;
}
AgvNavigationStatus SeerRobokitAgv::navigationStatus() const
{
AgvNavigationStatus status;
std::string missing_pose_task_detail;
for (int attempt = 0; attempt < 2; ++attempt) {
PoseTaskContext pose_context;
if (!currentPoseTask_(pose_context)) {
break;
}
const auto observed_navigation_generation =
navigation_generation_.load(std::memory_order_relaxed);
if (pose_context.navigation_generation
!= observed_navigation_generation) {
continue;
}
PoseTaskStatus task_status;
const auto result = queryPoseTaskStatus_(pose_context.task_id, task_status);
PoseTaskContext latest_context;
if (navigation_generation_.load(std::memory_order_relaxed)
!= observed_navigation_generation
|| !currentPoseTask_(latest_context)
|| latest_context.navigation_generation
!= pose_context.navigation_generation
|| latest_context.task_id != pose_context.task_id) {
continue;
}
status.type = AgvTaskType::NavigateToPose;
const auto fault_monitoring_unavailable =
[this, &pose_context]() {
if (controller_fault_channel_epoch_.load(
std::memory_order_relaxed)
!= pose_context
.controller_fault_channel_epoch_at_start) {
return std::string(
"the controller fault push channel changed or was "
"invalidated after the free-navigation command was "
"accepted");
}
return freeNavigationFaultStateUnavailableDetail_();
};
if (!result.ok()) {
status.state = AgvTaskState::Failed;
status.message = result.message;
return status;
}
if (!task_status.found || task_status.state == 404) {
std::uint64_t missing_task_fault_control_attempt = 0;
const std::string missing_task_fault =
cachedControllerFaultDetail_(
pose_context.controller_fault_sequence_at_start,
controllerFaultCaptureGraceMs_(),
&missing_task_fault_control_attempt);
PoseTaskContext post_missing_context;
if (navigation_generation_.load(std::memory_order_relaxed)
!= observed_navigation_generation
|| !currentPoseTask_(post_missing_context)
|| post_missing_context.navigation_generation
!= pose_context.navigation_generation
|| post_missing_context.task_id != pose_context.task_id) {
continue;
}
if (!missing_task_fault.empty()) {
std::string attribution;
if (missing_task_fault_control_attempt != 0
&& missing_task_fault_control_attempt
!= pose_context.control_attempt_sequence_at_start) {
attribution =
"controller_fault_attribution=ambiguous because the "
"fault was observed after another control command "
"attempt had begun, ";
}
status.state = AgvTaskState::Failed;
status.message =
"SEER Robokit tracked free-navigation task disappeared from "
"1110 task_status_package while a new controller fault "
"was observed: " + task_status.detail + ", "
+ attribution + missing_task_fault;
clearPoseTask_(pose_context.navigation_generation);
return status;
}
if (const std::string unavailable =
fault_monitoring_unavailable();
!unavailable.empty()) {
status.state = AgvTaskState::Failed;
status.message =
"SEER Robokit tracked free-navigation status is unsafe to "
"accept because controller fault monitoring is "
"unavailable: " + unavailable
+ "; query the controller and cancel or stop before "
"another motion command";
return status;
}
missing_pose_task_detail = task_status.detail;
clearPoseTask_(pose_context.navigation_generation);
break;
}
if (task_status.type_present && task_status.type != 1) {
status.state = AgvTaskState::Failed;
status.type = toTaskType(task_status.type);
status.message =
"SEER Robokit returned an unexpected task type for the tracked "
"free-navigation task: " + task_status.detail;
clearPoseTask_(pose_context.navigation_generation);
return status;
}
status.state = toTaskState(task_status.state);
status.progress = task_status.progress;
status.message = task_status.detail;
const auto controller_reported_state = status.state;
const bool controller_state_terminal =
controller_reported_state == AgvTaskState::Completed
|| controller_reported_state == AgvTaskState::Failed
|| controller_reported_state == AgvTaskState::Canceled;
std::uint64_t fault_control_attempt = 0;
const std::string fault = cachedControllerFaultDetail_(
pose_context.controller_fault_sequence_at_start,
controller_reported_state == AgvTaskState::Completed
|| controller_reported_state == AgvTaskState::Failed
? controllerFaultCaptureGraceMs_()
: 0,
&fault_control_attempt);
PoseTaskContext post_fault_context;
if (navigation_generation_.load(std::memory_order_relaxed)
!= observed_navigation_generation
|| !currentPoseTask_(post_fault_context)
|| post_fault_context.navigation_generation
!= pose_context.navigation_generation
|| post_fault_context.task_id != pose_context.task_id) {
continue;
}
const std::string unavailable =
fault_monitoring_unavailable();
if (!fault.empty()) {
std::string attribution;
if (fault_control_attempt != 0
&& fault_control_attempt
!= pose_context.control_attempt_sequence_at_start) {
attribution =
"controller_fault_attribution=ambiguous because the "
"fault was observed after another control command "
"attempt had begun, ";
}
status.state = AgvTaskState::Failed;
status.message =
"SEER Robokit reported a new controller fault while the tracked "
"free-navigation task had controller_task_state="
+ std::to_string(task_status.state) + ": "
+ task_status.detail + ", " + attribution + fault;
if (!unavailable.empty()) {
status.message +=
", controller_fault_monitoring_unavailable="
+ unavailable;
}
} else if (!unavailable.empty()) {
if (controller_reported_state == AgvTaskState::Failed
|| controller_reported_state == AgvTaskState::Canceled) {
status.message +=
", controller_fault_monitoring_unavailable="
+ unavailable;
} else {
status.state = AgvTaskState::Failed;
status.message =
"SEER Robokit tracked free-navigation state is unsafe to accept "
"because controller fault monitoring became unavailable: "
+ unavailable
+ "; query the controller and cancel or stop before "
"another motion command";
return status;
}
}
if (status.state == AgvTaskState::Completed) {
std::string pose_detail;
const bool target_reached =
poseTargetReached_(pose_context, pose_detail);
PoseTaskContext post_pose_context;
if (navigation_generation_.load(std::memory_order_relaxed)
!= observed_navigation_generation
|| !currentPoseTask_(post_pose_context)
|| post_pose_context.navigation_generation
!= pose_context.navigation_generation
|| post_pose_context.task_id != pose_context.task_id) {
continue;
}
std::uint64_t post_pose_fault_control_attempt = 0;
const std::string post_pose_fault =
cachedControllerFaultDetail_(
pose_context.controller_fault_sequence_at_start,
0,
&post_pose_fault_control_attempt);
if (!post_pose_fault.empty()) {
status.state = AgvTaskState::Failed;
std::string attribution;
if (post_pose_fault_control_attempt != 0
&& post_pose_fault_control_attempt
!= pose_context
.control_attempt_sequence_at_start) {
attribution =
"controller_fault_attribution=ambiguous because the "
"fault was observed after another control command "
"attempt had begun, ";
}
status.message =
"SEER Robokit reported the tracked free-navigation task "
"Completed, but a new controller fault was observed during "
"target verification: " + task_status.detail + ", "
+ attribution + post_pose_fault;
} else if (const std::string post_pose_unavailable =
fault_monitoring_unavailable();
!post_pose_unavailable.empty()) {
status.state = AgvTaskState::Failed;
status.message =
"SEER Robokit tracked free-navigation completion is unsafe to "
"accept because controller fault monitoring became "
"unavailable: " + post_pose_unavailable
+ "; query the controller and cancel or stop before "
"another motion command";
return status;
} else if (!target_reached) {
status.state = AgvTaskState::Failed;
status.message =
"SEER Robokit reported the tracked free-navigation task "
"Completed, but the requested target was not reached: "
+ task_status.detail + ", " + pose_detail;
} else {
status.message += ", target_verified: " + pose_detail;
}
}
if (controller_state_terminal) {
clearPoseTask_(pose_context.navigation_generation);
}
return status;
}
PoseTaskContext changed_context;
if (currentPoseTask_(changed_context)) {
status.state = AgvTaskState::Waiting;
status.type = AgvTaskType::NavigateToPose;
status.message =
"SEER Robokit free-navigation task changed while its status was being "
"queried; query navigation status again";
return status;
}
Json::Value payload(Json::objectValue);
jsonMember(payload, "simple") = false;
Json::Value response;
const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response);
if (!result.ok()) {
status.state = AgvTaskState::Failed;
status.message = missing_pose_task_detail.empty()
? result.message
: missing_pose_task_detail + "; 1020 status query failed: "
+ result.message;
return status;
}
const auto controller_result = resultFromResponse_(response);
if (!controller_result.ok()) {
status.state = AgvTaskState::Failed;
status.message = missing_pose_task_detail.empty()
? controller_result.message
: missing_pose_task_detail + "; 1020 status query failed: "
+ controller_result.message;
return status;
}
status.state = toTaskState(jsonGet(response, "task_status", 0).asInt());
status.type = toTaskType(jsonGet(response, "task_type", 0).asInt());
status.message = jsonGet(response, "move_status_info", jsonGet(response, "err_msg", "")).asString();
if (!missing_pose_task_detail.empty()) {
status.message = missing_pose_task_detail
+ "; fallback_1020_status=" + std::to_string(
jsonGet(response, "task_status", 0).asInt())
+ ", fallback_1020_type=" + std::to_string(
jsonGet(response, "task_type", 0).asInt())
+ (status.message.empty() ? std::string{} : ", " + status.message);
}
if (const auto* task_status_package = jsonFind(response, "task_status_package")) {
status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble();
}
return status;
}
AgvResult SeerRobokitAgv::configurePush_()
{
if (config_.state_push_included_fields_size() > 0 && config_.state_push_excluded_fields_size() > 0) {
return AgvResult::failure(
AgvErrorCode::InvalidArgument,
"SEER Robokit push included_fields and excluded_fields cannot both be set");
}
Json::Value payload(Json::objectValue);
if (config_.state_push_interval_ms() > 0) {
jsonMember(payload, "interval") = config_.state_push_interval_ms();
}
appendStringArray(payload, "included_fields", config_.state_push_included_fields());
appendStringArray(payload, "excluded_fields", config_.state_push_excluded_fields());
if (payload.empty()) {
return AgvResult::success();
}
const std::string payload_text = toJsonString_(payload);
const auto frame = buildFrame_(kRobotPushConfigReq, payload_text);
std::lock_guard<std::mutex> lock(mutex_);
if (sock_push_ < 0) {
return AgvResult::failure(AgvErrorCode::NotConnected, "SEER Robokit push socket not connected");
}
if (::send(sock_push_, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast<ssize_t>(frame.size())) {
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit send push config failed: " + systemError());
}
while (true) {
std::uint16_t command = 0;
std::string response_payload;
const auto result = receiveFrame_(sock_push_, command, response_payload);
if (!result.ok()) {
return result;
}
Json::Value response;
std::string error;
if (!response_payload.empty() && !parseJson_(response_payload, response, error)) {
return AgvResult::failure(AgvErrorCode::CommandFailed, error);
}
if (command == kRobotPushConfigRes) {
return resultFromResponse_(response);
}
if (command == kRobotPush && response.isObject()) {
updateCachedRuntimeState_(response);
}
}
}
void SeerRobokitAgv::startPushThread_()
{
if (!state_push_enabled_) {
return;
}
if (push_running_.exchange(true)) {
return;
}
if (sock_push_ < 0) {
push_running_ = false;
return;
}
push_thread_ = std::thread(&SeerRobokitAgv::pushLoop_, this);
}
void SeerRobokitAgv::stopPushThread_()
{
const bool was_running = push_running_.exchange(false);
if (was_running) {
int sock = -1;
{
std::lock_guard<std::mutex> lock(mutex_);
sock = sock_push_;
}
if (sock >= 0) {
::shutdown(sock, SHUT_RDWR);
}
}
if (push_thread_.joinable()) {
push_thread_.join();
}
invalidateControllerFaultState_();
}
void SeerRobokitAgv::pushLoop_()
{
while (push_running_) {
int sock = -1;
{
std::lock_guard<std::mutex> lock(mutex_);
sock = sock_push_;
}
if (sock < 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
continue;
}
std::uint16_t command = 0;
std::string payload;
const auto result = receiveFrame_(sock, command, payload);
if (!push_running_) {
break;
}
if (!result.ok()) {
if (result.code != AgvErrorCode::Timeout) {
invalidateControllerFaultState_();
std::lock_guard<std::mutex> lock(mutex_);
last_error_ = result.message;
closeSocket_(sock_push_);
}
continue;
}
if (command != kRobotPush || payload.empty()) {
continue;
}
Json::Value parsed;
std::string error;
if (!parseJson_(payload, parsed, error)) {
invalidateControllerFaultState_();
std::lock_guard<std::mutex> lock(mutex_);
last_error_ = error;
continue;
}
updateCachedRuntimeState_(parsed);
}
}
void SeerRobokitAgv::invalidateControllerFaultState_()
{
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
controller_fault_channel_epoch_.fetch_add(
1,
std::memory_order_relaxed);
controller_fault_state_observed_ = false;
controller_fault_state_observed_at_ = {};
active_controller_fault_detail_.clear();
runtime_state_cv_.notify_all();
}
void SeerRobokitAgv::updateCachedRuntimeState_(const Json::Value& payload)
{
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
auto state = cached_runtime_state_valid_ ? cached_runtime_state_ : AgvRuntimeState{};
state.timestamp = nowSeconds();
state.connected = true;
if (jsonHas(payload, "x")) state.pose.x = jsonGet(payload, "x", state.pose.x).asDouble();
if (jsonHas(payload, "y")) state.pose.y = jsonGet(payload, "y", state.pose.y).asDouble();
if (jsonHas(payload, "angle")) state.pose.theta = jsonGet(payload, "angle", state.pose.theta).asDouble();
if (jsonHas(payload, "vx")) state.velocity.vx = jsonGet(payload, "vx", state.velocity.vx).asDouble();
if (jsonHas(payload, "vy")) state.velocity.vy = jsonGet(payload, "vy", state.velocity.vy).asDouble();
if (jsonHas(payload, "w")) state.velocity.wz = jsonGet(payload, "w", state.velocity.wz).asDouble();
if (jsonHas(payload, "battery_level")) {
state.battery.percentage = jsonGet(payload, "battery_level", state.battery.percentage).asDouble();
}
if (jsonHas(payload, "battery_temp")) {
state.battery.temperature = jsonGet(payload, "battery_temp", state.battery.temperature).asDouble();
}
if (jsonHas(payload, "charging")) {
state.battery.charging = jsonGet(payload, "charging", state.battery.charging).asBool();
}
if (jsonHas(payload, "voltage")) {
state.battery.voltage = jsonGet(payload, "voltage", state.battery.voltage).asDouble();
}
if (jsonHas(payload, "current")) {
state.battery.current = jsonGet(payload, "current", state.battery.current).asDouble();
}
if (jsonHas(payload, "current_map")) {
state.current_map = jsonGet(payload, "current_map", state.current_map).asString();
}
if (jsonHas(payload, "current_station")) {
state.current_station = jsonGet(payload, "current_station", state.current_station).asString();
}
if (jsonHas(payload, "confidence")) {
state.localized = jsonGet(payload, "confidence", 0.0).asDouble() > 0.0;
}
if (jsonHas(payload, "emergency")) {
state.emergency_stopped = jsonGet(payload, "emergency", state.emergency_stopped).asBool();
}
state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4;
const bool has_fatals = jsonHas(payload, "fatals");
const bool has_errors = jsonHas(payload, "errors");
const bool has_fault_fields = has_fatals || has_errors;
if (has_fault_fields) {
const auto* fatals = jsonFind(payload, "fatals");
const auto* errors = jsonFind(payload, "errors");
const bool valid_fatals = !has_fatals
|| (fatals && fatals->isArray());
const bool valid_errors = !has_errors
|| (errors && errors->isArray());
const bool complete_fault_state =
has_fatals && has_errors && valid_fatals && valid_errors;
if (complete_fault_state) {
controller_fault_state_observed_ = true;
controller_fault_state_observed_at_ =
std::chrono::steady_clock::now();
} else {
controller_fault_state_observed_ = false;
controller_fault_state_observed_at_ = {};
}
const bool reported_fault =
hasFaultArray(payload, "fatals")
|| hasFaultArray(payload, "errors");
const bool invalid_or_incomplete_fault_state =
!complete_fault_state && !reported_fault;
state.fault = reported_fault
|| invalid_or_incomplete_fault_state;
if (state.fault) {
std::ostringstream detail;
detail << (reported_fault
? "SEER Robokit controller fault"
: "SEER Robokit controller fault state is incomplete or malformed");
if (fatals
&& (!fatals->isArray()
|| !fatals->empty()
|| !complete_fault_state)) {
detail << ": fatals="
<< (fatals->isNull()
? std::string("null")
: jsonValueToString(*fatals));
}
if (errors
&& (!errors->isArray()
|| !errors->empty()
|| !complete_fault_state)) {
detail << ": errors="
<< (errors->isNull()
? std::string("null")
: jsonValueToString(*errors));
}
state.last_error = detail.str();
if (state.last_error != active_controller_fault_detail_) {
active_controller_fault_detail_ = state.last_error;
++controller_fault_sequence_;
last_controller_fault_timestamp_ = state.timestamp;
last_controller_fault_detail_ = state.last_error;
last_controller_fault_control_attempt_ =
control_attempt_sequence_.load(
std::memory_order_acquire);
}
} else {
state.last_error.clear();
active_controller_fault_detail_.clear();
}
}
if (state.emergency_stopped) {
state.mode = AgvMode::EmergencyStop;
} else if (state.fault) {
state.mode = AgvMode::Fault;
} else if (state.battery.charging) {
state.mode = AgvMode::Charging;
} else if (state.moving) {
state.mode = AgvMode::Auto;
} else {
state.mode = AgvMode::Idle;
}
cached_runtime_state_ = state;
cached_runtime_state_valid_ = true;
runtime_state_cv_.notify_all();
}
} // namespace cmvr::device

362
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp
View File

@ -1,362 +0,0 @@
#include "seer_robokit_agv.h"
#include "seer_robokit_protocol.h"
#include "seer_robokit_utils.h"
#include <algorithm>
#include <arpa/inet.h>
#include <cstddef>
#include <cstdint>
#include <memory>
#include <mutex>
#include <string>
#include <sys/socket.h>
#include <sys/time.h>
#include <unistd.h>
#include <utility>
#include <vector>
namespace cmvr::device {
using namespace seer_robokit::protocol;
using namespace seer_robokit::detail;
AgvResult SeerRobokitAgv::connectSocket_(int& sock, const int port)
{
sock = ::socket(AF_INET, SOCK_STREAM, 0);
if (sock < 0) {
last_error_ = "create socket failed: " + systemError();
return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_);
}
sockaddr_in address{};
address.sin_family = AF_INET;
address.sin_port = htons(static_cast<std::uint16_t>(port));
if (::inet_pton(AF_INET, ip_.c_str(), &address.sin_addr) <= 0) {
closeSocket_(sock);
last_error_ = "invalid SEER Robokit ip: " + ip_;
return AgvResult::failure(AgvErrorCode::InvalidArgument, last_error_);
}
if (::connect(sock, reinterpret_cast<sockaddr*>(&address), sizeof(address)) < 0) {
closeSocket_(sock);
last_error_ = "connect SEER Robokit port " + std::to_string(port) + " failed: " + systemError();
return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_);
}
timeval timeout{};
timeout.tv_sec = recv_timeout_ms_ / 1000;
timeout.tv_usec = (recv_timeout_ms_ % 1000) * 1000;
::setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
return AgvResult::success();
}
AgvResult SeerRobokitAgv::ensureOtherSocket_()
{
std::lock_guard<std::mutex> lock(mutex_);
if (sock_other_ >= 0) {
return AgvResult::success();
}
return connectSocket_(sock_other_, ports_.other);
}
void SeerRobokitAgv::closeSocket_(int& sock) const
{
if (sock >= 0) {
::close(sock);
sock = -1;
}
}
bool SeerRobokitAgv::connected_() const
{
return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0;
}
AgvResult SeerRobokitAgv::sendCommand_(
const int sock,
const std::uint16_t command,
const Json::Value& payload,
Json::Value* response,
CommandTransmissionState* transmission_state) const
{
std::string response_payload;
auto result = sendCommandRaw_(
sock,
command,
payload,
&response_payload,
transmission_state);
if (!result.ok()) {
return result;
}
if (!response) {
return AgvResult::success();
}
Json::Value parsed;
std::string error;
if (!parseJson_(response_payload, parsed, error)) {
const std::string json_text = extractJson_(response_payload);
if (json_text.empty() || !parseJson_(json_text, parsed, error)) {
return AgvResult::failure(AgvErrorCode::CommandFailed, error);
}
}
*response = std::move(parsed);
return AgvResult::success();
}
AgvResult SeerRobokitAgv::sendCommandRaw_(
const int sock,
const std::uint16_t command,
const Json::Value& payload,
std::string* response_payload,
CommandTransmissionState* transmission_state) const
{
if (transmission_state) {
*transmission_state = CommandTransmissionState::NotSent;
}
const auto exchange = [&]() {
const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload);
const auto frame = buildFrame_(command, payload_text);
const auto sent = ::send(
sock,
frame.data(),
frame.size(),
MSG_NOSIGNAL);
if (sent > 0 && transmission_state) {
*transmission_state = CommandTransmissionState::PossiblySent;
}
if (sent != static_cast<ssize_t>(frame.size())) {
return AgvResult::failure(
AgvErrorCode::CommandFailed,
"SEER Robokit send command failed: " + systemError());
}
std::uint16_t response_command = 0;
std::string payload_text_response;
const auto result = receiveFrame_(sock, response_command, payload_text_response);
if (!result.ok()) {
return result;
}
const auto expected_response_command = static_cast<std::uint16_t>(
command + 10000U);
if (response_command != expected_response_command) {
return AgvResult::failure(
AgvErrorCode::CommandFailed,
"SEER Robokit response command mismatch: expected="
+ std::to_string(expected_response_command)
+ ", actual=" + std::to_string(response_command));
}
if (response_payload) {
*response_payload = std::move(payload_text_response);
}
return AgvResult::success();
};
const auto close_matching_socket_locked = [this, sock]() {
if (sock == sock_status_) {
closeSocket_(sock_status_);
} else if (sock == sock_control_) {
closeSocket_(sock_control_);
} else if (sock == sock_navigation_) {
closeSocket_(sock_navigation_);
} else if (sock == sock_config_) {
closeSocket_(sock_config_);
} else if (sock == sock_other_) {
closeSocket_(sock_other_);
}
};
const auto mark_channel_desynchronized = [](AgvResult result) {
std::string detail = result.message.empty()
? "unknown transport or frame error"
: result.message;
detail +=
"; SEER Robokit channel closed because the response stream may be "
"desynchronized; reconnect before sending another command";
return AgvResult::failure(result.code, detail);
};
bool is_status_socket = false;
{
std::lock_guard<std::mutex> lock(mutex_);
if (sock < 0) {
return AgvResult::failure(
AgvErrorCode::NotConnected,
"SEER Robokit socket not connected");
}
is_status_socket = sock == sock_status_;
}
if (is_status_socket) {
// A slow 1110 status response must never hold the lifecycle/global I/O
// mutex needed by cancelNavigation() or emergencyStop(). The dedicated
// status lock still serializes requests on port 19204. connect_() and
// disconnect_() take this lock before changing the descriptor.
std::lock_guard<std::mutex> status_lock(status_io_mutex_);
{
std::lock_guard<std::mutex> lock(mutex_);
if (sock < 0 || sock != sock_status_) {
return AgvResult::failure(
AgvErrorCode::NotConnected,
"SEER Robokit status socket is no longer connected");
}
}
auto result = exchange();
if (!result.ok()) {
std::lock_guard<std::mutex> lock(mutex_);
close_matching_socket_locked();
return mark_channel_desynchronized(std::move(result));
}
return result;
}
std::lock_guard<std::mutex> lock(mutex_);
if (sock < 0
|| (sock != sock_control_
&& sock != sock_navigation_
&& sock != sock_config_
&& sock != sock_other_)) {
return AgvResult::failure(
AgvErrorCode::NotConnected,
"SEER Robokit socket is no longer connected");
}
auto result = exchange();
if (!result.ok()) {
close_matching_socket_locked();
return mark_channel_desynchronized(std::move(result));
}
return result;
}
AgvResult SeerRobokitAgv::sendCommandNoResponse_(
const int sock,
const std::uint16_t command,
const Json::Value& payload) const
{
return sendCommand_(sock, command, payload, nullptr);
}
std::vector<std::uint8_t> SeerRobokitAgv::buildFrame_(
const std::uint16_t command,
const std::string& payload)
{
std::vector<std::uint8_t> frame(16 + payload.size(), 0);
frame[0] = 0x5A;
frame[1] = 0x01;
frame[2] = 0x00;
frame[3] = 0x01;
const auto length = static_cast<std::uint32_t>(payload.size());
frame[4] = static_cast<std::uint8_t>((length >> 24U) & 0xFFU);
frame[5] = static_cast<std::uint8_t>((length >> 16U) & 0xFFU);
frame[6] = static_cast<std::uint8_t>((length >> 8U) & 0xFFU);
frame[7] = static_cast<std::uint8_t>(length & 0xFFU);
frame[8] = static_cast<std::uint8_t>((command >> 8U) & 0xFFU);
frame[9] = static_cast<std::uint8_t>(command & 0xFFU);
std::copy(payload.begin(), payload.end(), frame.begin() + 16);
return frame;
}
std::string SeerRobokitAgv::toJsonString_(const Json::Value& value)
{
Json::StreamWriterBuilder builder;
builder["indentation"] = "";
return Json::writeString(builder, value);
}
bool SeerRobokitAgv::parseJson_(const std::string& input, Json::Value& output, std::string& error)
{
Json::CharReaderBuilder builder;
std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
return reader->parse(input.data(), input.data() + input.size(), &output, &error);
}
std::string SeerRobokitAgv::extractJson_(const std::string& raw)
{
const auto begin = raw.find('{');
const auto end = raw.rfind('}');
if (begin == std::string::npos || end == std::string::npos || end < begin) {
return {};
}
return raw.substr(begin, end - begin + 1);
}
AgvResult SeerRobokitAgv::receiveFrame_(const int sock, std::uint16_t& command, std::string& payload)
{
const auto recv_exact = [](const int fd, std::uint8_t* data, const std::size_t size) -> AgvResult {
std::size_t offset = 0;
while (offset < size) {
const ssize_t count = ::recv(fd, data + offset, size - offset, 0);
if (count > 0) {
offset += static_cast<std::size_t>(count);
continue;
}
if (count == 0) {
return AgvResult::failure(AgvErrorCode::NotConnected, "SEER Robokit socket closed");
}
if (errno == EINTR) {
continue;
}
if (errno == EAGAIN || errno == EWOULDBLOCK) {
return AgvResult::failure(AgvErrorCode::Timeout, "SEER Robokit receive timeout");
}
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit receive failed: " + systemError());
}
return AgvResult::success();
};
std::uint8_t header[16]{};
auto result = recv_exact(sock, header, sizeof(header));
if (!result.ok()) {
return result;
}
if (header[0] != 0x5A) {
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit frame header is invalid");
}
const auto length = (static_cast<std::uint32_t>(header[4]) << 24U)
| (static_cast<std::uint32_t>(header[5]) << 16U)
| (static_cast<std::uint32_t>(header[6]) << 8U)
| static_cast<std::uint32_t>(header[7]);
command = static_cast<std::uint16_t>((static_cast<std::uint16_t>(header[8]) << 8U) | header[9]);
payload.clear();
if (length == 0) {
return AgvResult::success();
}
if (length > kMaxFramePayloadBytes) {
return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit frame payload is too large");
}
std::vector<std::uint8_t> buffer(length);
result = recv_exact(sock, buffer.data(), buffer.size());
if (!result.ok()) {
return result;
}
payload.assign(reinterpret_cast<const char*>(buffer.data()), buffer.size());
return AgvResult::success();
}
AgvResult SeerRobokitAgv::resultFromResponse_(const Json::Value& response)
{
if (!hasNumericControllerRetCode(response)) {
return AgvResult::failure(
AgvErrorCode::CommandFailed,
"SEER Robokit controller response is missing a numeric ret_code");
}
const auto* ret_code_value = jsonFind(response, "ret_code");
const bool success = ret_code_value->isUInt() || ret_code_value->isUInt64()
? ret_code_value->asUInt64() == 0
: ret_code_value->asInt64() == 0;
const std::string ret_code = jsonValueToString(*ret_code_value);
const std::string message = jsonGet(response, "err_msg", "").asString();
if (success) {
return AgvResult::success();
}
std::string detail = "SEER Robokit command failed: ret_code=" + ret_code;
if (!message.empty()) {
detail += ", err_msg=" + message;
}
return AgvResult::failure(AgvErrorCode::CommandFailed, detail);
}
} // namespace cmvr::device

5541
cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp
View File

File diff suppressed because it is too large Load Diff

12
cmvr-es/devices/agv/src1100/CMakeLists.txt Normal file
View File

@ -0,0 +1,12 @@
add_library(src1100_agv SHARED src/src1100_agv.cpp)
target_include_directories(src1100_agv PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(src1100_agv
PUBLIC
cmvr_es::proto
jsoncpp
)
add_library(cmvr_es::device::src1100_agv ALIAS src1100_agv)
install(TARGETS src1100_agv LIBRARY DESTINATION lib)

179
cmvr-es/devices/agv/src1100/include/src1100_agv.h Normal file
View File

@ -0,0 +1,179 @@
#ifndef CMVR_ES_SRC1100_AGV_H
#define CMVR_ES_SRC1100_AGV_H
#include <atomic>
#include <condition_variable>
#include <cstdint>
#include <deque>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include <json/json.h>
#include "cmvr/config/agv_config/agv_config.pb.h"
#include "devices/agv/abstract_agv.h"
namespace cmvr::device {
class Src1100Agv final : public AbstractAGV {
public:
explicit Src1100Agv(const config::Src1100AgvConfig& cfg);
~Src1100Agv() override;
std::string typeName() const override { return "Src1100Agv"; }
bool init() override;
bool start() override;
bool stop() override;
bool update() override;
AgvRuntimeState runtimeState() const override;
AgvNavigationStatus navigationStatus() const override;
AgvResult emergencyStop() override;
AgvResult clearFault() override;
AgvResult navigateToPose(
const math::Pose2d& pose,
const AgvMotionOptions& options = {},
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
AgvResult navigateToStation(
const std::string& station_id,
const AgvMotionOptions& options = {},
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
AgvResult followPath(const std::vector<AgvPathSegment>& path) override;
AgvResult pauseNavigation() override;
AgvResult resumeNavigation() override;
AgvResult cancelNavigation() override;
AgvResult setVelocity(const AgvVelocity& velocity) override;
AgvResult listMaps(std::vector<std::string>& maps) const override;
AgvResult listStations(std::vector<AgvStation>& stations) const override;
AgvResult switchMap(const std::string& map_name) override;
AgvResult uploadMap(const std::string& map_name, const std::string& content) override;
AgvResult downloadMap(const std::string& map_name, std::string& content) const override;
AgvResult startMapping(const AgvMappingOptions& options = {}) override;
AgvResult getMappingData(int start_index, AgvMappingData& data) const override;
AgvResult getUnifiedMapUpdate(
std::uint64_t after_sequence,
const AgvMapStreamOptions& options,
AgvUnifiedMapUpdate& update) const override;
AgvResult stopMapping() override;
private:
struct Ports {
int status{19204};
int control{19205};
int navigation{19206};
int config{19207};
int other{19210};
int push{19301};
};
AgvResult connect_();
AgvResult disconnect_();
AgvResult connectSocket_(int& sock, int port);
AgvResult ensureOtherSocket_();
void closeSocket_(int& sock) const;
bool connected_() const;
AgvResult sendCommand_(int sock,
std::uint16_t command,
const Json::Value& payload,
Json::Value* response) const;
AgvResult sendCommandRaw_(int sock,
std::uint16_t command,
const Json::Value& payload,
std::string* response_payload) const;
AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const;
AgvResult configurePush_();
void startPushThread_();
void stopPushThread_();
void pushLoop_();
AgvRuntimeState queryRuntimeState_() const;
void updateCachedRuntimeState_(const Json::Value& payload);
void startMapUpdateThread_();
void stopMapUpdateThread_();
void mapUpdateLoop_();
AgvResult refreshMapCacheOnce_(const AgvMapStreamOptions& options) const;
AgvResult parseMapFileToUpdates_(
const std::string& file_name,
const std::string& content,
const AgvMapStreamOptions& options,
std::vector<AgvUnifiedMapUpdate>& updates) const;
AgvResult parseSrc1100MapArchive_(
const std::string& file_name,
const std::string& content,
const AgvMapStreamOptions& options,
std::vector<AgvUnifiedMapUpdate>& updates) const;
AgvResult parseSrc1100Map2D_(
const std::string& file_name,
const std::string& content,
const AgvMapStreamOptions& options,
AgvUnifiedMapUpdate& update) const;
AgvResult parseSrc1100Map3D_(
const std::string& file_name,
const std::string& content,
const AgvMapStreamOptions& options,
AgvUnifiedMapUpdate& update) const;
void cacheMapUpdates_(std::vector<AgvUnifiedMapUpdate> updates) const;
bool findCachedMapUpdate_(
std::uint64_t after_sequence,
const AgvMapStreamOptions& options,
AgvUnifiedMapUpdate& update) const;
bool mapUpdateMatches_(
const AgvUnifiedMapUpdate& update,
const AgvMapStreamOptions& options) const;
static std::vector<std::uint8_t> buildFrame_(std::uint16_t command, const std::string& payload);
static std::string toJsonString_(const Json::Value& value);
static bool parseJson_(const std::string& input, Json::Value& output, std::string& error);
static std::string extractJson_(const std::string& raw);
static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload);
static int optionalInt_(const AgvAdapterParams& params, const std::string& key, int fallback);
static double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback);
static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options);
static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
static AgvResult resultFromResponse_(const Json::Value& response);
config::Src1100AgvConfig config_;
std::string ip_;
int recv_timeout_ms_{1000};
Ports ports_;
bool state_push_enabled_{false};
bool map_update_enabled_{false};
int map_update_interval_ms_{1000};
std::size_t map_update_history_size_{8};
mutable std::mutex mutex_;
int sock_status_{-1};
int sock_control_{-1};
int sock_navigation_{-1};
int sock_config_{-1};
int sock_other_{-1};
int sock_push_{-1};
std::string last_error_;
std::atomic<bool> push_running_{false};
std::thread push_thread_;
mutable std::mutex runtime_state_mutex_;
AgvRuntimeState cached_runtime_state_;
bool cached_runtime_state_valid_{false};
mutable std::atomic<bool> map_update_running_{false};
mutable std::thread map_update_thread_;
mutable std::mutex map_update_mutex_;
mutable std::condition_variable map_update_cv_;
mutable std::deque<AgvUnifiedMapUpdate> cached_map_updates_;
mutable std::uint64_t map_sequence_{0};
mutable int next_mapping_index_{0};
mutable std::size_t last_map_content_hash_{0};
mutable std::string map_session_id_;
};
} // namespace cmvr::device
#endif // CMVR_ES_SRC1100_AGV_H

1898
cmvr-es/devices/agv/src1100/src/src1100_agv.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

2
cmvr-es/devices/arm/CMakeLists.txt
View File

@ -1,7 +1,6 @@
add_subdirectory(motor_robot_arm)
add_subdirectory(aubo_arm)
add_subdirectory(huayan_arm)
add_subdirectory(ume_robot_arm)
add_library(robot_arm INTERFACE)
@ -12,7 +11,6 @@ target_link_libraries(robot_arm
cmvr_es::device::motor_robot_arm
cmvr_es::device::aubo_arm
cmvr_es::device::huayan_arm
cmvr_es::device::ume_robot_arm
cmvr_es::proto
)

89
cmvr-es/devices/arm/aubo_arm/CMakeLists.txt
View File

@ -2,8 +2,6 @@ add_library(aubo_arm SHARED
aubo_arm.cpp
)
find_package(Threads REQUIRED)
target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1)
@ -73,94 +71,7 @@ target_link_libraries(aubo_arm
cmvr_es::proto
PRIVATE
glog
jsoncpp
Threads::Threads
)
add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm)
install(TARGETS aubo_arm LIBRARY DESTINATION lib)
if(BUILD_TESTING)
enable_testing()
add_executable(aubo_arm_motion_result_test
tests/aubo_arm_motion_result_test.cpp
)
target_include_directories(aubo_arm_motion_result_test
PRIVATE
${CMAKE_SOURCE_DIR}/cmvr-es
)
add_test(
NAME aubo_arm_motion_result_test
COMMAND aubo_arm_motion_result_test
)
set_tests_properties(aubo_arm_motion_result_test PROPERTIES TIMEOUT 10)
add_executable(aubo_motion_state_test
tests/aubo_motion_state_test.cpp
)
target_include_directories(aubo_motion_state_test
PRIVATE
${CMAKE_SOURCE_DIR}/cmvr-es
)
add_test(
NAME aubo_motion_state_test
COMMAND aubo_motion_state_test
)
set_tests_properties(aubo_motion_state_test PROPERTIES TIMEOUT 10)
add_executable(aubo_safety_state_test
tests/aubo_safety_state_test.cpp
)
target_include_directories(aubo_safety_state_test
PRIVATE
${CMAKE_SOURCE_DIR}/cmvr-es
)
add_test(
NAME aubo_safety_state_test
COMMAND aubo_safety_state_test
)
set_tests_properties(aubo_safety_state_test PROPERTIES TIMEOUT 10)
add_executable(aubo_arm_json_command_test
tests/aubo_arm_json_command_test.cpp
)
target_link_libraries(aubo_arm_json_command_test
PRIVATE
cmvr_es::device::aubo_arm
cmvr_es::proto
)
add_test(
NAME aubo_arm_json_command_test
COMMAND aubo_arm_json_command_test
)
set_tests_properties(aubo_arm_json_command_test PROPERTIES
TIMEOUT 10
ENVIRONMENT "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
)
if(UNIX AND NOT APPLE)
# The imported AUBO target still contributes its vendor directory to
# direct consumers' build-tree RUNPATH. Put the system runtime first
# for this test; installed artifacts exclude the vendor libstdc++.
execute_process(
COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so.6
OUTPUT_VARIABLE AUBO_TEST_SYSTEM_LIBSTDCXX
OUTPUT_STRIP_TRAILING_WHITESPACE
)
if(EXISTS "${AUBO_TEST_SYSTEM_LIBSTDCXX}")
get_filename_component(
AUBO_TEST_SYSTEM_LIBSTDCXX_REAL
"${AUBO_TEST_SYSTEM_LIBSTDCXX}"
REALPATH
)
get_filename_component(
AUBO_TEST_SYSTEM_LIBSTDCXX_DIR
"${AUBO_TEST_SYSTEM_LIBSTDCXX_REAL}"
DIRECTORY
)
set_property(
TARGET aubo_arm_json_command_test
PROPERTY BUILD_RPATH "${AUBO_TEST_SYSTEM_LIBSTDCXX_DIR}"
)
endif()
endif()
endif()

153
cmvr-es/devices/arm/aubo_arm/README.md
View File

@ -1,153 +0,0 @@
# AUBO RobotArm 与控制柜 IO
`AuboArm` 是 AUBO SDK v0.27.1 的 `RobotArm` 后端。控制柜 Standard 数字 IO
通过设备通用的 `executeJsonCommand` 接口访问，远程调用复用
`cmvr.api.ArmService/ExecuteJsonCommand`，不经过 `SystemService` 或
`MotorService`。该 RPC 只路由到 `RobotArm`，不会把 JSON 命令转发给其他设备类型。
旧的 `cmvr.api.SystemService/ExecuteJsonCommand` 不再注册，调用方必须更新服务路径；
请求和响应消息结构保持不变。
返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。
## 代码与配置
- 实现：[`aubo_arm.h`](aubo_arm.h)、[`aubo_arm.cpp`](aubo_arm.cpp)
- 测试：[`tests/aubo_arm_json_command_test.cpp`](tests/aubo_arm_json_command_test.cpp)
- 设备配置：[`../../../config/devices/arm/aubo_arm.pb.txt`](../../../config/devices/arm/aubo_arm.pb.txt)
- DeviceManager 配置：
[`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt)
- ArmService 实现：
[`../../../service/grpc/server/src/grpc_arm_service.cpp`](../../../service/grpc/server/src/grpc_arm_service.cpp)
- Proto：[`../../../../protos/cmvr/api/arm_service.proto`](../../../../protos/cmvr/api/arm_service.proto)
仓库配置使用 SDK RPC 端口 `30004`。现场部署必须填写真实控制器地址和凭据，
不要把生产密码提交到默认配置。
## 控制柜 Standard 数字 IO
当前支持：
| `operation` | 说明 | 必填字段 |
| --- | --- | --- |
| `get_di` | 读取控制柜数字输入 | `index` |
| `get_do` | 读取控制柜数字输出及其 runstate | `index` |
| `set_do` | 设置控制柜数字输出 | `index`、`value` |
JSON 命令：
```json
{"command":"cabinet_io","operation":"get_di","index":0}
{"command":"cabinet_io","operation":"get_do","index":0}
{"command":"cabinet_io","operation":"set_do","index":0,"value":true}
```
`index` 从 `0` 开始，运行时根据控制器返回的 IO 数量检查范围。
`set_do.value` 必须是 JSON 布尔值 `true` 或 `false`，不接受 `0/1` 或字符串。
`set_do` 成功响应中的 `requested_value` 只表示 SDK 已接受请求；确认实际输出时
必须再调用 `get_do`。
读取成功响应示例：
```json
{
"success": true,
"command": "cabinet_io",
"operation": "get_di",
"index": 0,
"count": 16,
"value": false
}
```
## 通过 gRPC 调用
默认 gRPC 端口为 `50052`。读取 DI0：
```shell
grpcurl -plaintext \
-d '{
"header":{"deviceId":"aubo_arm"},
"requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"get_di\",\"index\":0}"
}' \
127.0.0.1:50052 \
cmvr.api.ArmService/ExecuteJsonCommand
```
设置 DO0 为高电平：
```shell
grpcurl -plaintext \
-d '{
"header":{"deviceId":"aubo_arm"},
"requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"set_do\",\"index\":0,\"value\":true}"
}' \
127.0.0.1:50052 \
cmvr.api.ArmService/ExecuteJsonCommand
```
使用源码默认配置时：
1. 在 `cmvr-es/config/devices/arm/aubo_arm.pb.txt` 填写正确地址和登录信息；
2. 在 `cmvr-es/config/manager/device_manager.pb.txt` 将 `aubo_arm.enable`
改为 `true`；
3. 重新安装配置并启动安装产物。
```shell
cmake --install build
./output/bin/cmvr_es
```
`output/bin/cmvr_es` 默认读取 `output/bin/config/`。使用 `--config` 时，应修改
对应外部配置根。设备未启用或初始化失败时，gRPC 返回
`Device not found: aubo_arm`。
## 安全与语义边界
- 后端使用独立 SDK RPC 会话持续读取控制器的 `SafetyModeType`、
`RobotModeType` 和硬件急停来源；首次有效样本前、监控断线或样本过期时，
所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝；
- 硬件急停会立即使当前运动 generation 失效，并在急停输入有效期间保持锁存。
检测到硬件急停输入消失且控制器重新报告 `Normal`/`ReducedMode` 后，后端应
自动执行 `poweron()` 和 `startup()`，恢复到 `Running` 后再完成安全确认并开放新的
gRPC 控制指令；防护停机和 Safety Fault/Violation 仍保持显式恢复语义；
- `emergencyStop()` 使用独立的 `SoftwareEmergencyStop` 锁存。即使软件急停在真实
硬件急停有效期间触发，后续硬件采样也不能覆盖该锁存，释放硬件急停开关不会
自动清除软件急停；它只能通过显式安全恢复流程解除；
- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。硬件急停
自动恢复先上电到 `Idle`，在刹车释放前清理 runtime、servo 和轨迹队列，再执行
`startup()`；到达 `Running` 后还会再次确认 `ExecId == -1`、普通队列和轨迹队列
均为空、运行时已停止且机械臂稳定，全部成立后才能解除锁存；
- 当前 AUBO 配置通过 `auto_power_on_after_hardware_estop_release: true` 显式启用自动
上电。自动确认失败时继续保持 fail-closed，并允许通过 `torqueOn`/`clearFault`/
`unlockProtectiveStop` 显式重试；本轮释放期间收到 `stopMotion()` 或 `torqueOff()`
会取消自动上电，显式停止始终优先；
- 恢复流程只调用 `poweron()` 和 `startup()`，不会调用 `resume`、`arbitraryResume`、
`startMove`，也不会重新提交急停前的目标、速度、servo 指令或程序；
- AUBO SDK 未在本地文档中保证急停期间 `clearPath` 的可用性，也未说明释放
急停开关后的控制器恢复时序。因此自动恢复必须在释放后再次清队列并完成上述
安全确认；无法确认时不得解除锁存。“释放开关后零位移”的最终保证仍需真机
验证及控制器侧安全配置配合；
- 只访问控制柜 Standard 数字 IO，不访问工具端 IO、可配置 IO 或安全 IO；
- `set_do` 不修改输出 runstate；
- 只有 `StandardOutputRunState::None` 的通道允许写入，否则返回
`output_managed_by_runstate`；
- 普通访问不会调用会重置全部输出配置的
`setDigitalOutputRunstateDefault()`；
- 模拟量 IO 涉及 domain、单位和量程，当前 JSON 接口不开放；
- gRPC/JSON 返回成功不代表目标 IO 具备功能安全等级；
- 真实写测试前应确认通道用途、负载、电气隔离、默认电平和控制器程序所有权。
## 测试
```bash
cmake --build build --target \
aubo_safety_state_test \
aubo_motion_state_test \
aubo_arm_json_command_test -j4
ctest --test-dir build \
-R 'aubo_(safety_state|motion_state|arm_json_command)_test' \
--output-on-failure
```
该测试覆盖 JSON 校验和无硬件错误路径，不代表已在真实 AUBO 控制柜完成 DI/DO
读取、写入或 runstate 拒绝验证。

3215
cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp
View File

File diff suppressed because it is too large Load Diff

44
cmvr-es/devices/arm/aubo_arm/aubo_arm.h
View File

@ -2,8 +2,6 @@
#define CMVR_ES_AUBO_ARM_H
#include <atomic>
#include <cstdint>
#include <functional>
#include <memory>
#include <mutex>
#include <optional>
@ -23,8 +21,6 @@ public:
std::string typeName() const override { return "AuboARM"; }
bool init() override;
bool stop() override;
bool executeJsonCommand(const std::string& request_json,
std::string& response_json) override;
RobotModel getRobotModel() const override { return model_; }
std::size_t getDof() const override { return model_.dof; }
@ -32,17 +28,14 @@ public:
JointGroupState getJointState() const override;
CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
RobotMode getRobotMode() const override;
SafetyMode getSafetyMode() const override;
ControlMode getControlMode() const override;
bool supportsActionQueueMotion() const noexcept override { return true; }
SafetyMode getSafetyMode() const override { return SafetyMode::Normal; }
ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
Result torqueOn() override;
Result torqueOn(
const std::function<bool()>& cancellation_requested) override;
Result torqueOff() override;
Result calibrateZeroQ(const std::string& joint_name) override;
Result emergencyStop() override;
Result protectiveStop() override;
Result protectiveStop() override { return emergencyStop(); }
Result recoverProtectiveStop(
const JointTrajectory&,
const MotionOptions&) override
@ -53,9 +46,9 @@ public:
}
Result setSpeedScaling(double scaling) override;
double getSpeedScaling() const override { return speed_scaling_; }
bool isProtectiveStopped() const override;
bool isEmergencyStopped() const override;
bool isFault() const override;
bool isProtectiveStopped() const override { return false; }
bool isEmergencyStopped() const override { return emergency_stopped_; }
bool isFault() const override { return false; }
Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override;
@ -79,8 +72,8 @@ public:
Result powerOff() override { return torqueOff(); }
Result brakeRelease() override { return torqueOn(); }
Result shutdown() override;
Result clearFault() override;
Result unlockProtectiveStop() override;
Result clearFault() override { return Result::success(); }
Result unlockProtectiveStop() override { return Result::success(); }
Result loadProgram(const std::string& program_name) override;
Result playProgram() override;
Result pauseProgram() override;
@ -93,27 +86,16 @@ public:
CartesianPose fk(const std::string& base_link, const std::string& ee_link) override;
CartesianPose fk(bool is_tcp = true) override;
CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; }
bool busy() const override;
bool busy() const override { return busy_.load(); }
private:
enum class MotionStopKind {
Automatic,
Joint,
Linear,
};
Result unsupported_(const std::string& name) const;
bool validDof_(std::size_t size, std::string& error) const;
Result ensureConnected_(const std::string& context) const;
Result ensureMotionReady_(const std::string& context,
std::uint64_t& safety_epoch) const;
Result completeSafetyRecovery_(const std::string& context,
std::uint64_t expected_safety_epoch);
Result unlockProtectiveStop_(
std::optional<std::uint64_t> expected_safety_epoch);
Result stopMotion_(MotionStopKind kind, double acceleration);
#if defined(CMVR_HAS_AUBO_SDK)
struct SdkState;
#endif
private:
config::RobotArmConfig cfg_;
@ -128,10 +110,12 @@ private:
std::atomic<bool> connected_{false};
std::atomic<bool> busy_{false};
std::atomic<bool> servo_mode_{false};
std::atomic<bool> emergency_stopped_{false};
bool emergency_stopped_{false};
mutable std::mutex mutex_;
#if defined(CMVR_HAS_AUBO_SDK)
std::unique_ptr<SdkState> sdk_;
#endif
};
} // namespace cmvr::device

45
cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h
View File

@ -1,45 +0,0 @@
#ifndef CMVR_ES_AUBO_MOTION_RESULT_H
#define CMVR_ES_AUBO_MOTION_RESULT_H
namespace cmvr::device::aubo_internal {
enum class MotionCommandOutcome {
CompletedWithoutMotion,
CompletedAfterMotion,
Cancelled,
SubmitFailed,
CompletionFailed,
};
enum class MotionWaitResult {
Completed,
Cancelled,
Failed,
};
template <typename WaitForCompletion>
MotionCommandOutcome resolveMotionCommand(
const int return_code,
const int success_code,
const int request_ignore_code,
WaitForCompletion&& wait_for_completion)
{
if (return_code == request_ignore_code) {
return MotionCommandOutcome::CompletedWithoutMotion;
}
if (return_code != success_code) {
return MotionCommandOutcome::SubmitFailed;
}
const auto wait_result = wait_for_completion();
if (wait_result == MotionWaitResult::Cancelled) {
return MotionCommandOutcome::Cancelled;
}
if (wait_result != MotionWaitResult::Completed) {
return MotionCommandOutcome::CompletionFailed;
}
return MotionCommandOutcome::CompletedAfterMotion;
}
} // namespace cmvr::device::aubo_internal
#endif // CMVR_ES_AUBO_MOTION_RESULT_H

369
cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h
View File

@ -1,369 +0,0 @@
#ifndef CMVR_ES_AUBO_MOTION_STATE_H
#define CMVR_ES_AUBO_MOTION_STATE_H
#include <algorithm>
#include <chrono>
#include <condition_variable>
#include <cstdint>
#include <memory>
#include <mutex>
namespace cmvr::device::aubo_internal {
enum class MotionKind {
None,
Joint,
Linear,
};
struct MotionToken {
std::uint64_t generation{0};
MotionKind kind{MotionKind::None};
bool valid() const noexcept
{
return generation != 0 && kind != MotionKind::None;
}
};
enum class MotionStartStatus {
Started,
Invalid,
Busy,
Stopping,
Blocked,
};
struct MotionStartResult {
MotionStartStatus status{MotionStartStatus::Busy};
MotionToken token;
bool started() const noexcept
{
return status == MotionStartStatus::Started;
}
};
enum class MotionFinishMode {
RestorePrevious,
Clear,
Retain,
};
enum class StopStartStatus {
Started,
AlreadyStopping,
};
enum class StopWaitStatus {
Completed,
Failed,
Timeout,
};
class StopCompletion final {
public:
StopWaitStatus waitFor(const std::chrono::milliseconds timeout)
{
std::unique_lock lock(mutex_);
if (!cv_.wait_for(lock, timeout, [this]() { return completed_; })) {
return StopWaitStatus::Timeout;
}
return succeeded_
? StopWaitStatus::Completed
: StopWaitStatus::Failed;
}
private:
friend class MotionState;
void finish(const bool succeeded)
{
{
std::lock_guard lock(mutex_);
succeeded_ = succeeded;
completed_ = true;
}
cv_.notify_all();
}
std::mutex mutex_;
std::condition_variable cv_;
bool completed_{false};
bool succeeded_{false};
};
struct StopRequest {
StopStartStatus status{StopStartStatus::AlreadyStopping};
MotionKind kind{MotionKind::None};
MotionToken active_token;
bool tracked_motion{false};
std::shared_ptr<StopCompletion> completion;
bool started() const noexcept
{
return status == StopStartStatus::Started;
}
};
struct SafetyCancelResult {
MotionKind kind{MotionKind::None};
MotionToken active_token;
bool tracked_motion{false};
};
// Tracks one direct AUBO motion owner. MoveJ/MoveL submissions are serialized
// through the vendor call. Speed calls release the outer mutex before their
// potentially blocking SDK call, so the generation cancellation below also
// closes the stop-vs-speed-submission race.
class MotionState final {
public:
MotionStartResult begin(
const MotionKind kind,
const bool replace_retained_same_kind = false)
{
std::lock_guard lock(mutex_);
if (kind == MotionKind::None) {
return {MotionStartStatus::Invalid, {}};
}
if (stop_in_progress_) {
return {MotionStartStatus::Stopping, {}};
}
if (blocked_) {
return {MotionStartStatus::Blocked, {}};
}
if (owner_active_) {
return {MotionStartStatus::Busy, {}};
}
if (last_kind_ != MotionKind::None &&
(!replace_retained_same_kind || last_kind_ != kind)) {
return {MotionStartStatus::Busy, {}};
}
MotionToken token{++next_generation_, kind};
owner_active_ = true;
active_token_ = token;
previous_kind_ = last_kind_;
return {MotionStartStatus::Started, token};
}
void finish(
const MotionToken& token,
const MotionFinishMode mode = MotionFinishMode::RestorePrevious)
{
std::lock_guard lock(mutex_);
if (!owner_active_ ||
active_token_.generation != token.generation) {
return;
}
owner_active_ = false;
active_token_ = {};
if (!stop_in_progress_ && !blocked_) {
if (mode == MotionFinishMode::Retain) {
last_kind_ = token.kind;
} else if (mode == MotionFinishMode::Clear) {
last_kind_ = MotionKind::None;
} else {
last_kind_ = previous_kind_;
}
}
previous_kind_ = MotionKind::None;
owner_finished_cv_.notify_all();
}
void failMotion(const MotionToken& token)
{
std::lock_guard lock(mutex_);
if (!owner_active_ ||
active_token_.generation != token.generation) {
return;
}
owner_active_ = false;
active_token_ = {};
last_kind_ = token.kind;
previous_kind_ = MotionKind::None;
blocked_ = true;
owner_finished_cv_.notify_all();
}
StopRequest beginStop(
const MotionKind requested_kind = MotionKind::None)
{
std::lock_guard lock(mutex_);
if (stop_in_progress_) {
return {
StopStartStatus::AlreadyStopping,
MotionKind::None,
{},
false,
active_stop_completion_};
}
auto completion = std::make_shared<StopCompletion>();
stop_in_progress_ = true;
active_stop_completion_ = completion;
const MotionToken active = owner_active_
? active_token_
: MotionToken{};
// A successful speedJoint/speedLine call may keep the controller in
// velocity mode after the SDK function returns, even when the target
// velocity is zero and the robot currently reports steady. Retain that
// motion kind until a typed stop has been acknowledged.
const bool tracked_motion =
active.valid() || last_kind_ != MotionKind::None;
if (active.valid()) {
cancelled_generation_ = std::max(
cancelled_generation_, active.generation);
}
MotionKind kind = MotionKind::None;
if (active.valid()) {
kind = active.kind;
} else if (last_kind_ != MotionKind::None) {
kind = last_kind_;
} else if (requested_kind != MotionKind::None) {
kind = requested_kind;
} else {
kind = last_kind_;
}
if (kind != MotionKind::None) {
last_kind_ = kind;
}
return {
StopStartStatus::Started,
kind,
active,
tracked_motion,
completion};
}
SafetyCancelResult cancelActiveForSafety()
{
std::lock_guard lock(mutex_);
const MotionToken active = owner_active_
? active_token_
: MotionToken{};
if (active.valid()) {
cancelled_generation_ = std::max(
cancelled_generation_, active.generation);
}
const MotionKind kind = active.valid()
? active.kind
: last_kind_;
if (kind != MotionKind::None) {
last_kind_ = kind;
}
// This block is intentionally independent of stop_in_progress_. The
// monitor may observe the safety event while a software Stop owns the
// stop transaction; either way no new motion may enter.
blocked_ = true;
owner_finished_cv_.notify_all();
return {kind, active, active.valid() || kind != MotionKind::None};
}
bool cancelled(const MotionToken& token) const
{
std::lock_guard lock(mutex_);
return token.valid() &&
token.generation <= cancelled_generation_;
}
bool waitForOwnerExit(
const MotionToken& token,
const std::chrono::milliseconds timeout)
{
if (!token.valid()) {
return true;
}
std::unique_lock lock(mutex_);
return owner_finished_cv_.wait_for(
lock,
timeout,
[this, &token]() {
return !owner_active_ ||
active_token_.generation != token.generation;
});
}
bool ownerActive(const MotionToken& token) const
{
if (!token.valid()) {
return false;
}
std::lock_guard lock(mutex_);
return owner_active_ &&
active_token_.generation == token.generation;
}
StopWaitStatus waitForStopCompletion(
const StopRequest& request,
const std::chrono::milliseconds timeout) const
{
if (!request.completion) {
return StopWaitStatus::Failed;
}
return request.completion->waitFor(timeout);
}
bool completeStop()
{
std::shared_ptr<StopCompletion> completion;
{
std::lock_guard lock(mutex_);
if (owner_active_) {
return false;
}
stop_in_progress_ = false;
blocked_ = false;
active_token_ = {};
last_kind_ = MotionKind::None;
previous_kind_ = MotionKind::None;
completion = std::move(active_stop_completion_);
owner_finished_cv_.notify_all();
}
if (completion) {
completion->finish(true);
}
return true;
}
void failStop()
{
std::shared_ptr<StopCompletion> completion;
{
std::lock_guard lock(mutex_);
stop_in_progress_ = false;
blocked_ = true;
completion = std::move(active_stop_completion_);
owner_finished_cv_.notify_all();
}
if (completion) {
completion->finish(false);
}
}
bool busy() const
{
std::lock_guard lock(mutex_);
return owner_active_ || stop_in_progress_ || blocked_ ||
last_kind_ != MotionKind::None;
}
private:
mutable std::mutex mutex_;
std::condition_variable owner_finished_cv_;
std::uint64_t next_generation_{0};
std::uint64_t cancelled_generation_{0};
MotionToken active_token_;
MotionKind last_kind_{MotionKind::None};
MotionKind previous_kind_{MotionKind::None};
std::shared_ptr<StopCompletion> active_stop_completion_;
bool owner_active_{false};
bool stop_in_progress_{false};
bool blocked_{false};
};
} // namespace cmvr::device::aubo_internal
#endif // CMVR_ES_AUBO_MOTION_STATE_H

240
cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h
View File

@ -1,240 +0,0 @@
#ifndef CMVR_ES_AUBO_SAFETY_STATE_H
#define CMVR_ES_AUBO_SAFETY_STATE_H
#include <cstdint>
#include <mutex>
#include <optional>
namespace cmvr::device::aubo_internal {
// This is deliberately richer than RobotArm::SafetyMode. Recovery and
// Violation have no lossless public mapping, but both must remain fail-closed.
enum class SafetyCondition {
Unknown,
Normal,
Reduced,
Recovery,
Violation,
ProtectiveStop,
SafeguardStop,
SystemEmergencyStop,
RobotEmergencyStop,
SoftwareEmergencyStop,
Fault,
};
inline bool isMotionSafe(const SafetyCondition condition) noexcept
{
return condition == SafetyCondition::Normal ||
condition == SafetyCondition::Reduced;
}
inline SafetyCondition effectiveSafetyCondition(
const SafetyCondition reported_condition,
const int robot_emergency_stop_source) noexcept
{
if (robot_emergency_stop_source < 0) {
return SafetyCondition::Unknown;
}
if (robot_emergency_stop_source != 0) {
return SafetyCondition::RobotEmergencyStop;
}
return reported_condition;
}
inline bool needsProtectiveUnlock(
const SafetyCondition condition) noexcept
{
return condition == SafetyCondition::ProtectiveStop ||
condition == SafetyCondition::Violation;
}
inline bool needsInterfaceBoardRestart(
const SafetyCondition condition) noexcept
{
return condition == SafetyCondition::SystemEmergencyStop ||
condition == SafetyCondition::RobotEmergencyStop ||
condition == SafetyCondition::Fault;
}
struct SafetyPermit {
std::uint64_t epoch{0};
bool valid() const noexcept { return epoch != 0; }
};
struct RecoveryToken {
std::uint64_t epoch{0};
bool valid() const noexcept { return epoch != 0; }
};
struct SafetySnapshot {
SafetyCondition observed{SafetyCondition::Unknown};
SafetyCondition latched_reason{SafetyCondition::Unknown};
std::uint64_t epoch{0};
bool latched{false};
bool recovery_in_progress{false};
bool software_emergency_stop_latched{false};
};
inline bool shouldAutoRecoverHardwareEmergencyStop(
const SafetySnapshot& snapshot,
const bool hardware_emergency_stop_was_observed,
const int current_emergency_stop_source,
const bool auto_power_on_enabled,
const bool automatic_recovery_suppressed) noexcept
{
return auto_power_on_enabled && !automatic_recovery_suppressed &&
hardware_emergency_stop_was_observed && snapshot.latched &&
!snapshot.recovery_in_progress &&
!snapshot.software_emergency_stop_latched &&
snapshot.latched_reason == SafetyCondition::RobotEmergencyStop &&
isMotionSafe(snapshot.observed) &&
current_emergency_stop_source == 0;
}
// Hardware safety is an event, not a level. Once an unsafe state has been
// observed, returning to Normal only changes the observed level. A separate,
// explicit recovery must prove that the old controller operation has been
// cancelled before new motion permits can be issued.
class SafetyState final {
public:
SafetyState() = default;
void observe(const SafetyCondition condition)
{
std::lock_guard lock(mutex_);
const bool changed = observed_ != condition;
observed_ = condition;
if (condition == SafetyCondition::SoftwareEmergencyStop) {
software_emergency_stop_latched_ = true;
}
if (isMotionSafe(condition)) {
return;
}
if (!latched_ || recovery_in_progress_ || changed) {
++epoch_;
}
latched_ = true;
recovery_in_progress_ = false;
// A physical E-stop sample can continue arriving after a software
// E-stop request. Keep the software stop independently latched so a
// later physical-input release can never clear it automatically.
latched_reason_ = software_emergency_stop_latched_
? SafetyCondition::SoftwareEmergencyStop
: condition;
}
std::optional<SafetyPermit> tryPermit() const
{
std::lock_guard lock(mutex_);
if (latched_ || !isMotionSafe(observed_)) {
return std::nullopt;
}
return SafetyPermit{epoch_};
}
bool validate(const SafetyPermit permit) const
{
std::lock_guard lock(mutex_);
return permit.valid() && permit.epoch == epoch_ && !latched_ &&
isMotionSafe(observed_);
}
std::optional<RecoveryToken> beginRecovery(
const std::uint64_t expected_epoch)
{
std::lock_guard lock(mutex_);
if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ ||
recovery_in_progress_ ||
!isMotionSafe(observed_)) {
return std::nullopt;
}
recovery_in_progress_ = true;
return RecoveryToken{epoch_};
}
bool completeRecovery(
const RecoveryToken token,
const bool robot_running,
const bool controller_idle,
const bool cancellation_confirmed)
{
std::lock_guard lock(mutex_);
if (!token.valid() || token.epoch != epoch_ || !latched_ ||
!recovery_in_progress_ || !isMotionSafe(observed_) ||
!robot_running || !controller_idle ||
!cancellation_confirmed) {
return false;
}
latched_ = false;
recovery_in_progress_ = false;
latched_reason_ = SafetyCondition::Unknown;
software_emergency_stop_latched_ = false;
++epoch_;
return true;
}
// The caller may clear the physical E-stop latch only after it has powered
// the controller, released the brakes, and then re-confirmed an empty,
// steady controller in Running mode. This never authorizes replaying the
// old target, runtime program, or servo session.
bool completeHardwareEmergencyStopRecovery(
const RecoveryToken token,
const bool robot_running,
const bool controller_idle,
const bool cancellation_confirmed)
{
std::lock_guard lock(mutex_);
if (!token.valid() || token.epoch != epoch_ || !latched_ ||
!recovery_in_progress_ ||
software_emergency_stop_latched_ ||
latched_reason_ != SafetyCondition::RobotEmergencyStop ||
!isMotionSafe(observed_) || !robot_running || !controller_idle ||
!cancellation_confirmed) {
return false;
}
latched_ = false;
recovery_in_progress_ = false;
latched_reason_ = SafetyCondition::Unknown;
++epoch_;
return true;
}
void failRecovery(const RecoveryToken token)
{
std::lock_guard lock(mutex_);
if (token.valid() && token.epoch == epoch_) {
recovery_in_progress_ = false;
}
}
SafetySnapshot snapshot() const
{
std::lock_guard lock(mutex_);
return {
observed_,
latched_reason_,
epoch_,
latched_,
recovery_in_progress_,
software_emergency_stop_latched_};
}
private:
mutable std::mutex mutex_;
SafetyCondition observed_{SafetyCondition::Unknown};
SafetyCondition latched_reason_{SafetyCondition::Unknown};
std::uint64_t epoch_{1};
bool latched_{false};
bool recovery_in_progress_{false};
bool software_emergency_stop_latched_{false};
};
} // namespace cmvr::device::aubo_internal
#endif // CMVR_ES_AUBO_SAFETY_STATE_H

50
cmvr-es/devices/arm/aubo_arm/aubo_torque_on_result.h
View File

@ -1,50 +0,0 @@
#ifndef CMVR_ES_AUBO_TORQUE_ON_RESULT_H
#define CMVR_ES_AUBO_TORQUE_ON_RESULT_H
#include <optional>
#include <string>
#include <utility>
#include "common/types/arm/arm_types.h"
namespace cmvr::device::aubo_internal {
inline Result preservePrimaryTorqueOnFailure(
Result primary_failure,
const std::optional<Result>& cancellation_outcome)
{
if (!cancellation_outcome.has_value() ||
cancellation_outcome->message.empty()) {
return primary_failure;
}
if (!primary_failure.message.empty()) {
primary_failure.message += "; ";
}
primary_failure.message +=
"cancellation handling: " + cancellation_outcome->message;
return primary_failure;
}
template <typename Mutation, typename FailureResult,
typename CancellationOutcome>
std::optional<Result> runTorqueOnControllerMutation(
const int success_code,
Mutation&& mutation,
FailureResult&& failure_result,
CancellationOutcome&& cancellation_outcome)
{
const int return_code = std::forward<Mutation>(mutation)();
if (return_code != success_code) {
auto primary_failure =
std::forward<FailureResult>(failure_result)(return_code);
return preservePrimaryTorqueOnFailure(
std::move(primary_failure),
std::forward<CancellationOutcome>(cancellation_outcome)());
}
return std::forward<CancellationOutcome>(cancellation_outcome)();
}
} // namespace cmvr::device::aubo_internal
#endif // CMVR_ES_AUBO_TORQUE_ON_RESULT_H

84
cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_json_command_test.cpp
View File

@ -1,84 +0,0 @@
#include "devices/arm/aubo_arm/aubo_arm.h"
#include <iostream>
#include <string>
namespace {
#define CHECK_TRUE(condition) \
do { \
if (!(condition)) { \
std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \
<< #condition << std::endl; \
return 1; \
} \
} while (false)
bool contains(const std::string& value, const std::string& expected)
{
return value.find(expected) != std::string::npos;
}
cmvr::config::RobotArmConfig makeConfig()
{
cmvr::config::RobotArmConfig config;
config.set_id("aubo_arm_json_test");
auto* vendor = config.mutable_vendor();
vendor->set_brand(cmvr::config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM);
vendor->set_model("AuboTest");
vendor->set_dof(6);
return config;
}
} // namespace
int main()
{
cmvr::device::AuboArm arm(makeConfig());
cmvr::device::AbstractDevice* device = &arm;
std::string response;
CHECK_TRUE(!device->executeJsonCommand("{", response));
CHECK_TRUE(contains(response, R"("error_code":"invalid_json")"));
CHECK_TRUE(!device->executeJsonCommand("[]", response));
CHECK_TRUE(contains(response, R"("error_code":"invalid_json")"));
CHECK_TRUE(!device->executeJsonCommand(
R"({"command":"ptz","operation":"get_di","index":0})", response));
CHECK_TRUE(contains(response, R"("error_code":"unsupported_command")"));
CHECK_TRUE(!device->executeJsonCommand(
R"({"command":"cabinet_io","operation":"get_ai","index":0})", response));
CHECK_TRUE(contains(response, R"("error_code":"invalid_operation")"));
CHECK_TRUE(!device->executeJsonCommand(
R"({"command":"cabinet_io","operation":"get_di","index":-1})", response));
CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")"));
CHECK_TRUE(!device->executeJsonCommand(
R"({"command":"cabinet_io","operation":"set_do","index":0,"value":1})",
response));
CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")"));
CHECK_TRUE(!device->executeJsonCommand(
R"({"command":"cabinet_io","operation":"set_do","index":0})", response));
CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")"));
CHECK_TRUE(!device->executeJsonCommand(
R"({"command":"cabinet_io","operation":"get_di","index":0})", response));
CHECK_TRUE(contains(response, R"("error_code":"not_connected")"));
CHECK_TRUE(!device->executeJsonCommand(
R"({"command":"cabinet_io","operation":"get_do","index":0})", response));
CHECK_TRUE(contains(response, R"("operation":"get_do")"));
CHECK_TRUE(contains(response, R"("error_code":"not_connected")"));
CHECK_TRUE(!device->executeJsonCommand(
R"({"command":"cabinet_io","operation":"set_do","index":0,"value":true})",
response));
CHECK_TRUE(contains(response, R"("operation":"set_do")"));
CHECK_TRUE(contains(response, R"("error_code":"not_connected")"));
return 0;
}

158
cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp
View File

@ -1,158 +0,0 @@
#include "devices/arm/aubo_arm/aubo_motion_result.h"
#include "devices/arm/aubo_arm/aubo_torque_on_result.h"
#include <iostream>
#include <optional>
#include <string>
#include <vector>
namespace {
#define CHECK_TRUE(condition) \
do { \
if (!(condition)) { \
std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \
<< #condition << std::endl; \
return 1; \
} \
} while (false)
} // namespace
int main()
{
using cmvr::device::aubo_internal::MotionCommandOutcome;
using cmvr::device::aubo_internal::MotionWaitResult;
using cmvr::device::aubo_internal::preservePrimaryTorqueOnFailure;
using cmvr::device::aubo_internal::resolveMotionCommand;
using cmvr::device::aubo_internal::runTorqueOnControllerMutation;
constexpr int success_code = 0;
constexpr int request_ignore_code = 13;
int wait_calls = 0;
const auto wait_succeeded = [&wait_calls]() {
++wait_calls;
return MotionWaitResult::Completed;
};
CHECK_TRUE(resolveMotionCommand(
success_code,
success_code,
request_ignore_code,
wait_succeeded) == MotionCommandOutcome::CompletedAfterMotion);
CHECK_TRUE(wait_calls == 1);
wait_calls = 0;
CHECK_TRUE(resolveMotionCommand(
request_ignore_code,
success_code,
request_ignore_code,
wait_succeeded) == MotionCommandOutcome::CompletedWithoutMotion);
CHECK_TRUE(wait_calls == 0);
const int submit_failures[] = {1, 2, 3, -request_ignore_code};
for (const int return_code : submit_failures) {
wait_calls = 0;
CHECK_TRUE(resolveMotionCommand(
return_code,
success_code,
request_ignore_code,
wait_succeeded) == MotionCommandOutcome::SubmitFailed);
CHECK_TRUE(wait_calls == 0);
}
wait_calls = 0;
const auto wait_failed = [&wait_calls]() {
++wait_calls;
return MotionWaitResult::Failed;
};
CHECK_TRUE(resolveMotionCommand(
success_code,
success_code,
request_ignore_code,
wait_failed) == MotionCommandOutcome::CompletionFailed);
CHECK_TRUE(wait_calls == 1);
wait_calls = 0;
const auto wait_cancelled = [&wait_calls]() {
++wait_calls;
return MotionWaitResult::Cancelled;
};
CHECK_TRUE(resolveMotionCommand(
success_code,
success_code,
request_ignore_code,
wait_cancelled) == MotionCommandOutcome::Cancelled);
CHECK_TRUE(wait_calls == 1);
using cmvr::device::ArmErrorCode;
using cmvr::device::Result;
std::vector<std::string> mutation_trace;
const auto failed_mutation = runTorqueOnControllerMutation(
success_code,
[&mutation_trace] {
mutation_trace.push_back("mutation");
return 42;
},
[&mutation_trace](const int return_code) {
mutation_trace.push_back("primary-failure");
return Result::failure(
ArmErrorCode::CommandFailed,
"vendor failure ret=" + std::to_string(return_code));
},
[&mutation_trace]() -> std::optional<Result> {
mutation_trace.push_back("cancellation-cleanup");
return Result::failure(
ArmErrorCode::CommandRejected,
"controller safety termination confirmed");
});
CHECK_TRUE(failed_mutation.has_value());
CHECK_TRUE(failed_mutation->code == ArmErrorCode::CommandFailed);
CHECK_TRUE(
failed_mutation->message.find("vendor failure ret=42") !=
std::string::npos);
CHECK_TRUE(
failed_mutation->message.find(
"controller safety termination confirmed") !=
std::string::npos);
CHECK_TRUE(mutation_trace.size() == 3);
CHECK_TRUE(mutation_trace[0] == "mutation");
CHECK_TRUE(mutation_trace[1] == "primary-failure");
CHECK_TRUE(mutation_trace[2] == "cancellation-cleanup");
const auto cleanup_failed = preservePrimaryTorqueOnFailure(
Result::failure(
ArmErrorCode::CommandFailed,
"vendor exception"),
std::optional<Result>{Result::failure(
ArmErrorCode::CommandFailed,
"safety termination failed")});
CHECK_TRUE(cleanup_failed.code == ArmErrorCode::CommandFailed);
CHECK_TRUE(
cleanup_failed.message.find("vendor exception") !=
std::string::npos);
CHECK_TRUE(
cleanup_failed.message.find("safety termination failed") !=
std::string::npos);
int successful_cancellation_checks = 0;
const auto cancelled_after_success = runTorqueOnControllerMutation(
success_code,
[] { return 0; },
[](const int) {
return Result::failure(
ArmErrorCode::CommandFailed, "must not be used");
},
[&successful_cancellation_checks]() -> std::optional<Result> {
++successful_cancellation_checks;
return Result::failure(
ArmErrorCode::CommandRejected,
"cancelled after successful mutation");
});
CHECK_TRUE(cancelled_after_success.has_value());
CHECK_TRUE(
cancelled_after_success->code == ArmErrorCode::CommandRejected);
CHECK_TRUE(successful_cancellation_checks == 1);
return 0;
}

213
cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp
View File

@ -1,213 +0,0 @@
#include "devices/arm/aubo_arm/aubo_motion_state.h"
#include <atomic>
#include <chrono>
#include <iostream>
#include <thread>
namespace {
#define CHECK_TRUE(condition) \
do { \
if (!(condition)) { \
std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \
<< #condition << std::endl; \
return 1; \
} \
} while (false)
} // namespace
int main()
{
using namespace cmvr::device::aubo_internal;
MotionState state;
CHECK_TRUE(state.begin(MotionKind::None).status ==
MotionStartStatus::Invalid);
const auto joint = state.begin(MotionKind::Joint);
CHECK_TRUE(joint.started());
CHECK_TRUE(state.busy());
CHECK_TRUE(state.begin(MotionKind::Linear).status ==
MotionStartStatus::Busy);
const auto stop_joint = state.beginStop();
CHECK_TRUE(stop_joint.started());
CHECK_TRUE(stop_joint.kind == MotionKind::Joint);
CHECK_TRUE(stop_joint.active_token.generation ==
joint.token.generation);
CHECK_TRUE(stop_joint.tracked_motion);
CHECK_TRUE(state.cancelled(joint.token));
const auto joined_stop_joint = state.beginStop();
CHECK_TRUE(joined_stop_joint.status ==
StopStartStatus::AlreadyStopping);
CHECK_TRUE(
state.waitForStopCompletion(
joined_stop_joint, std::chrono::milliseconds(1)) ==
StopWaitStatus::Timeout);
CHECK_TRUE(state.begin(MotionKind::Linear).status ==
MotionStartStatus::Stopping);
CHECK_TRUE(!state.waitForOwnerExit(
joint.token, std::chrono::milliseconds(1)));
CHECK_TRUE(!state.completeStop());
state.finish(joint.token);
CHECK_TRUE(state.waitForOwnerExit(
joint.token, std::chrono::milliseconds(1)));
CHECK_TRUE(state.completeStop());
CHECK_TRUE(
state.waitForStopCompletion(
joined_stop_joint, std::chrono::milliseconds(1)) ==
StopWaitStatus::Completed);
CHECK_TRUE(!state.busy());
const auto linear = state.begin(MotionKind::Linear);
CHECK_TRUE(linear.started());
CHECK_TRUE(!state.cancelled(linear.token));
// A delayed guard from the cancelled command must not release a newer one.
state.finish(joint.token);
CHECK_TRUE(state.busy());
state.finish(linear.token, MotionFinishMode::Clear);
CHECK_TRUE(!state.busy());
const auto speed_joint = state.begin(MotionKind::Joint);
CHECK_TRUE(speed_joint.started());
state.finish(speed_joint.token, MotionFinishMode::Retain);
CHECK_TRUE(state.busy());
CHECK_TRUE(state.begin(MotionKind::Linear).status ==
MotionStartStatus::Busy);
const auto rejected_speed_update =
state.begin(MotionKind::Joint, true);
CHECK_TRUE(rejected_speed_update.started());
state.finish(
rejected_speed_update.token,
MotionFinishMode::RestorePrevious);
const auto stop_speed = state.beginStop();
CHECK_TRUE(stop_speed.started());
CHECK_TRUE(stop_speed.kind == MotionKind::Joint);
CHECK_TRUE(stop_speed.tracked_motion);
CHECK_TRUE(state.completeStop());
CHECK_TRUE(!state.busy());
const auto idle_stop = state.beginStop();
CHECK_TRUE(idle_stop.kind == MotionKind::None);
CHECK_TRUE(!idle_stop.tracked_motion);
const auto joined_idle_stop = state.beginStop();
CHECK_TRUE(joined_idle_stop.status ==
StopStartStatus::AlreadyStopping);
state.failStop();
CHECK_TRUE(
state.waitForStopCompletion(
joined_idle_stop, std::chrono::milliseconds(1)) ==
StopWaitStatus::Failed);
const auto retry_idle_stop = state.beginStop();
CHECK_TRUE(retry_idle_stop.kind == MotionKind::None);
CHECK_TRUE(!retry_idle_stop.tracked_motion);
CHECK_TRUE(state.completeStop());
const auto mismatched_stop_motion = state.begin(MotionKind::Linear);
CHECK_TRUE(mismatched_stop_motion.started());
const auto mismatched_stop = state.beginStop(MotionKind::Joint);
CHECK_TRUE(mismatched_stop.kind == MotionKind::Linear);
state.finish(mismatched_stop_motion.token);
CHECK_TRUE(state.completeStop());
const auto uncertain_motion = state.begin(MotionKind::Joint);
CHECK_TRUE(uncertain_motion.started());
state.failMotion(uncertain_motion.token);
CHECK_TRUE(state.busy());
CHECK_TRUE(state.begin(MotionKind::Linear).status ==
MotionStartStatus::Blocked);
const auto stop_uncertain = state.beginStop();
CHECK_TRUE(stop_uncertain.kind == MotionKind::Joint);
CHECK_TRUE(stop_uncertain.tracked_motion);
CHECK_TRUE(state.completeStop());
const auto failed_stop_motion = state.begin(MotionKind::Linear);
CHECK_TRUE(failed_stop_motion.started());
const auto failed_stop = state.beginStop();
CHECK_TRUE(failed_stop.kind == MotionKind::Linear);
state.failStop();
CHECK_TRUE(state.busy());
CHECK_TRUE(state.begin(MotionKind::Joint).status ==
MotionStartStatus::Blocked);
state.finish(failed_stop_motion.token);
const auto retry = state.beginStop();
CHECK_TRUE(retry.started());
CHECK_TRUE(retry.kind == MotionKind::Linear);
CHECK_TRUE(state.completeStop());
const auto recovered = state.begin(MotionKind::Joint);
CHECK_TRUE(recovered.started());
state.finish(recovered.token, MotionFinishMode::Clear);
const auto safety_motion = state.begin(MotionKind::Linear);
CHECK_TRUE(safety_motion.started());
const auto safety_cancel = state.cancelActiveForSafety();
CHECK_TRUE(safety_cancel.kind == MotionKind::Linear);
CHECK_TRUE(safety_cancel.tracked_motion);
CHECK_TRUE(state.cancelled(safety_motion.token));
CHECK_TRUE(state.begin(MotionKind::Joint).status ==
MotionStartStatus::Blocked);
state.finish(safety_motion.token);
const auto safety_stop = state.beginStop();
CHECK_TRUE(safety_stop.started());
CHECK_TRUE(safety_stop.kind == MotionKind::Linear);
CHECK_TRUE(state.completeStop());
const auto retained_speed = state.begin(MotionKind::Joint);
CHECK_TRUE(retained_speed.started());
state.finish(retained_speed.token, MotionFinishMode::Retain);
const auto retained_cancel = state.cancelActiveForSafety();
CHECK_TRUE(retained_cancel.kind == MotionKind::Joint);
CHECK_TRUE(retained_cancel.tracked_motion);
CHECK_TRUE(!retained_cancel.active_token.valid());
CHECK_TRUE(state.begin(MotionKind::Linear).status ==
MotionStartStatus::Blocked);
const auto retained_stop = state.beginStop();
CHECK_TRUE(retained_stop.started());
CHECK_TRUE(retained_stop.kind == MotionKind::Joint);
CHECK_TRUE(retained_stop.tracked_motion);
CHECK_TRUE(state.completeStop());
// A waiter keeps the completion for the stop it joined even when another
// stop starts and finishes before the waiter is scheduled again.
const auto concurrent_first_stop = state.beginStop();
CHECK_TRUE(concurrent_first_stop.started());
const auto concurrent_first_join = state.beginStop();
CHECK_TRUE(concurrent_first_join.status ==
StopStartStatus::AlreadyStopping);
std::atomic<bool> waiter_entered{false};
std::atomic<StopWaitStatus> first_wait_result{
StopWaitStatus::Timeout};
std::thread first_waiter([&]() {
waiter_entered.store(true, std::memory_order_release);
first_wait_result.store(
state.waitForStopCompletion(
concurrent_first_join,
std::chrono::milliseconds(500)),
std::memory_order_release);
});
while (!waiter_entered.load(std::memory_order_acquire)) {
std::this_thread::yield();
}
const bool concurrent_first_completed = state.completeStop();
const auto concurrent_second_stop = state.beginStop();
const auto concurrent_second_join = state.beginStop();
state.failStop();
first_waiter.join();
CHECK_TRUE(concurrent_first_completed);
CHECK_TRUE(concurrent_second_stop.started());
CHECK_TRUE(concurrent_second_join.status ==
StopStartStatus::AlreadyStopping);
CHECK_TRUE(first_wait_result.load(std::memory_order_acquire) ==
StopWaitStatus::Completed);
CHECK_TRUE(
state.waitForStopCompletion(
concurrent_second_join, std::chrono::milliseconds(1)) ==
StopWaitStatus::Failed);
return 0;
}

136
cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp
View File

@ -1,136 +0,0 @@
#include "devices/arm/aubo_arm/aubo_safety_state.h"
#include <iostream>
namespace {
#define CHECK_TRUE(condition) \
do { \
if (!(condition)) { \
std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \
<< #condition << std::endl; \
return 1; \
} \
} while (false)
} // namespace
int main()
{
using namespace cmvr::device::aubo_internal;
SafetyState state;
CHECK_TRUE(!state.tryPermit().has_value());
state.observe(SafetyCondition::Normal);
const auto initial_permit = state.tryPermit();
CHECK_TRUE(initial_permit.has_value());
CHECK_TRUE(state.validate(*initial_permit));
CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, 1) ==
SafetyCondition::RobotEmergencyStop);
CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, -1) ==
SafetyCondition::Unknown);
CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Reduced, 0) ==
SafetyCondition::Reduced);
CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::ProtectiveStop));
CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::Violation));
CHECK_TRUE(!needsProtectiveUnlock(SafetyCondition::SafeguardStop));
CHECK_TRUE(needsInterfaceBoardRestart(
SafetyCondition::RobotEmergencyStop));
CHECK_TRUE(needsInterfaceBoardRestart(
SafetyCondition::SystemEmergencyStop));
CHECK_TRUE(needsInterfaceBoardRestart(SafetyCondition::Fault));
CHECK_TRUE(!needsInterfaceBoardRestart(SafetyCondition::Recovery));
state.observe(SafetyCondition::RobotEmergencyStop);
CHECK_TRUE(!state.validate(*initial_permit));
CHECK_TRUE(state.snapshot().latched);
CHECK_TRUE(!state.beginRecovery(state.snapshot().epoch).has_value());
// The observed level alone does not unlock motion. The monitor must first
// prove the old controller operation is fully quiescent.
state.observe(SafetyCondition::Normal);
CHECK_TRUE(state.snapshot().latched);
CHECK_TRUE(!state.tryPermit().has_value());
CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop(
state.snapshot(), false, 0, true, false));
CHECK_TRUE(shouldAutoRecoverHardwareEmergencyStop(
state.snapshot(), true, 0, true, false));
CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop(
state.snapshot(), true, 0, false, false));
CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop(
state.snapshot(), true, 0, true, true));
const auto recovery = state.beginRecovery(state.snapshot().epoch);
CHECK_TRUE(recovery.has_value());
CHECK_TRUE(!state.completeRecovery(*recovery, true, true, false));
state.failRecovery(*recovery);
const auto retry = state.beginRecovery(state.snapshot().epoch);
CHECK_TRUE(retry.has_value());
// Automatic release is not committed at Idle/PowerOn. The controller
// must have completed startup and reached Running first.
CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery(
*retry, false, true, true));
CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery(
*retry, true, false, true));
CHECK_TRUE(state.completeHardwareEmergencyStopRecovery(
*retry, true, true, true));
const auto recovered_permit = state.tryPermit();
CHECK_TRUE(recovered_permit.has_value());
CHECK_TRUE(state.validate(*recovered_permit));
// Releasing a real E-stop must never clear a software-triggered stop that
// was latched while the hardware input was active.
state.observe(SafetyCondition::RobotEmergencyStop);
state.observe(SafetyCondition::SoftwareEmergencyStop);
// The hardware monitor continues publishing the physical E-stop level
// until the switch is released. It must not overwrite the software latch.
state.observe(SafetyCondition::RobotEmergencyStop);
state.observe(SafetyCondition::Normal);
CHECK_TRUE(!shouldAutoRecoverHardwareEmergencyStop(
state.snapshot(), true, 0, true, false));
CHECK_TRUE(state.snapshot().software_emergency_stop_latched);
CHECK_TRUE(state.snapshot().latched_reason ==
SafetyCondition::SoftwareEmergencyStop);
const auto software_recovery = state.beginRecovery(
state.snapshot().epoch);
CHECK_TRUE(software_recovery.has_value());
CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery(
*software_recovery, true, true, true));
state.failRecovery(*software_recovery);
const auto explicit_software_recovery = state.beginRecovery(
state.snapshot().epoch);
CHECK_TRUE(explicit_software_recovery.has_value());
CHECK_TRUE(state.completeRecovery(
*explicit_software_recovery, true, true, true));
CHECK_TRUE(!state.snapshot().software_emergency_stop_latched);
// A new safety event invalidates an in-flight recovery token.
state.observe(SafetyCondition::ProtectiveStop);
state.observe(SafetyCondition::Reduced);
const auto stale_recovery = state.beginRecovery(
state.snapshot().epoch);
CHECK_TRUE(stale_recovery.has_value());
CHECK_TRUE(!state.completeHardwareEmergencyStopRecovery(
*stale_recovery, true, true, true));
state.failRecovery(*stale_recovery);
const auto explicit_recovery = state.beginRecovery(
state.snapshot().epoch);
CHECK_TRUE(explicit_recovery.has_value());
state.observe(SafetyCondition::SafeguardStop);
state.observe(SafetyCondition::Normal);
CHECK_TRUE(!state.completeRecovery(
*explicit_recovery, true, true, true));
CHECK_TRUE(state.snapshot().latched);
// An old API call must not begin recovery for a newer safety event.
const auto stale_epoch = state.snapshot().epoch;
state.observe(SafetyCondition::RobotEmergencyStop);
state.observe(SafetyCondition::Normal);
CHECK_TRUE(!state.beginRecovery(stale_epoch).has_value());
CHECK_TRUE(!state.snapshot().recovery_in_progress);
return 0;
}

49
cmvr-es/devices/arm/huayan_arm/CMakeLists.txt
View File

@ -1,7 +1,5 @@
add_library(huayan_arm SHARED huayan_arm.cpp)
find_package(Threads REQUIRED)
set(HUAYAN_ARM_SDK_DIR ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/huayan_arm/v1.0)
target_include_directories(huayan_arm
@ -22,56 +20,9 @@ target_link_libraries(huayan_arm
PRIVATE
HR_Pro
glog
Threads::Threads
)
add_library(cmvr_es::device::huayan_arm ALIAS huayan_arm)
install(TARGETS huayan_arm LIBRARY DESTINATION lib)
install(FILES ${HUAYAN_ARM_SDK_DIR}/lib/libHR_Pro.so DESTINATION lib)
if(BUILD_TESTING)
add_executable(huayan_lifecycle_state_test
tests/huayan_lifecycle_state_test.cpp
)
target_include_directories(huayan_lifecycle_state_test
PRIVATE
${CMAKE_SOURCE_DIR}/cmvr-es
)
target_link_libraries(huayan_lifecycle_state_test
PRIVATE
Threads::Threads
)
add_test(
NAME huayan_lifecycle_state_test
COMMAND huayan_lifecycle_state_test
)
set_tests_properties(huayan_lifecycle_state_test PROPERTIES TIMEOUT 10)
if(UNIX AND NOT APPLE)
add_executable(huayan_arm_sdk_test
tests/huayan_arm_sdk_test.cpp
)
target_include_directories(huayan_arm_sdk_test
PRIVATE
${CMAKE_SOURCE_DIR}/cmvr-es
${HUAYAN_ARM_SDK_DIR}/include
)
target_link_libraries(huayan_arm_sdk_test
PRIVATE
cmvr_es::device::huayan_arm
Threads::Threads
)
# Export the fake HRIF_* definitions so libhuayan_arm resolves its SDK
# calls to the deterministic test controller instead of real hardware.
target_link_options(huayan_arm_sdk_test PRIVATE -Wl,--export-dynamic)
add_test(
NAME huayan_arm_sdk_test
COMMAND huayan_arm_sdk_test
)
set_tests_properties(huayan_arm_sdk_test PROPERTIES
TIMEOUT 20
ENVIRONMENT "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
)
endif()
endif()

2531
cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp
View File

File diff suppressed because it is too large Load Diff

77
cmvr-es/devices/arm/huayan_arm/huayan_arm.h
View File

@ -9,18 +9,12 @@
#define CMVR_ES_HUAYAN_ROBOT_H
#include <atomic>
#include <chrono>
#include <cstdint>
#include <functional>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <thread>
#include <vector>
#include "cmvr/config/arm_config/arm_config.pb.h"
#include "devices/arm/huayan_arm/huayan_lifecycle_state.h"
#include "devices/arm/robot_arm.h"
namespace cmvr::device {
@ -41,14 +35,13 @@ public:
CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
RobotMode getRobotMode() const override;
SafetyMode getSafetyMode() const override;
ControlMode getControlMode() const override;
bool supportsActionQueueMotion() const noexcept override { return true; }
ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
Result torqueOn() override;
Result torqueOff() override;
Result calibrateZeroQ(const std::string& joint_name) override;
Result emergencyStop() override;
Result protectiveStop() override;
Result protectiveStop() override { return emergencyStop(); }
Result recoverProtectiveStop(
const JointTrajectory&,
const MotionOptions&) override
@ -58,7 +51,7 @@ public:
"protective recovery is not implemented for HuayanRobot");
}
Result setSpeedScaling(double scaling) override;
double getSpeedScaling() const override { return speed_scaling_.load(); }
double getSpeedScaling() const override { return speed_scaling_; }
bool isProtectiveStopped() const override;
bool isEmergencyStopped() const override;
bool isFault() const override;
@ -86,7 +79,7 @@ public:
Result brakeRelease() override { return torqueOn(); }
Result shutdown() override;
Result clearFault() override;
Result unlockProtectiveStop() override;
Result unlockProtectiveStop() override { return clearFault(); }
Result loadProgram(const std::string& program_name) override;
Result playProgram() override;
Result pauseProgram() override;
@ -99,7 +92,7 @@ public:
CartesianPose fk(const std::string& base_link, const std::string& ee_link) override;
CartesianPose fk(bool is_tcp = true) override;
CartesianVelocity getSpeedLCommandTwistBase() const override;
bool busy() const override;
bool busy() const override { return busy_.load(); }
private:
struct HrState {
@ -116,69 +109,21 @@ private:
int connected_to_box{0};
int blending_done{0};
int in_pos{0};
int emergency_signal_fault{0};
int emergency_input{0};
int safeguard_signal_fault{0};
int safeguard_input{0};
bool valid{false};
};
struct RuntimeState;
Result ensureConnected_(const std::string& context) const;
Result ensureMotionReady_(
const std::string& context,
const std::shared_ptr<RuntimeState>& runtime,
huayan_internal::SafetyPermit& permit) const;
Result unsupported_(const std::string& name) const;
Result hrResult_(int code, const std::string& context) const;
Result motionStartFailure_(
const std::string& context,
huayan_internal::MotionStartStatus status) const;
bool validDof_(std::size_t size, std::string& error) const;
HrState readHrState_() const;
HrState sampleHrState_(
const std::shared_ptr<RuntimeState>& runtime) const;
void publishHrState_(
const std::shared_ptr<RuntimeState>& runtime,
const HrState& state) const;
std::vector<double> readJointPositionRad_() const;
std::vector<double> readJointVelocityRad_() const;
CartesianPose readTcpPose_() const;
CartesianVelocity readTcpVelocity_() const;
bool readJointPositionSample_(std::vector<double>& values) const;
bool readJointVelocitySample_(std::vector<double>& values) const;
bool readTcpPoseSample_(CartesianPose& pose) const;
std::vector<double> currentJointPositionDeg_() const;
std::string nextCommandId_() const;
Result waitMotionDone_(
const std::string& context,
const std::shared_ptr<RuntimeState>& runtime,
huayan_internal::MotionToken motion_token,
huayan_internal::SafetyPermit safety_permit,
const std::string& command_id,
const std::vector<double>* joint_target,
const CartesianPose* tcp_target,
int timeout_ms,
const std::function<bool()>& cancellation_requested = {}) const;
bool targetReached_(
const std::vector<double>* joint_target,
const CartesianPose* tcp_target) const;
bool controllerIdleStable_(
const std::shared_ptr<RuntimeState>& runtime,
std::chrono::milliseconds timeout) const;
bool terminateController_(
const std::shared_ptr<RuntimeState>& runtime,
std::chrono::milliseconds timeout,
bool stop_program) const;
Result completeSafetyRecovery_(
const std::string& context,
const std::shared_ptr<RuntimeState>& runtime,
std::uint64_t expected_epoch,
bool enable_robot,
bool release_software_guard);
std::shared_ptr<RuntimeState> runtimeSnapshot_() const;
void safetyMonitorLoop_(const std::shared_ptr<RuntimeState>& runtime);
Result waitMotionDone_(const std::string& context, int timeout_ms) const;
private:
config::RobotArmConfig cfg_;
@ -190,16 +135,12 @@ private:
unsigned int robot_id_{0};
std::string tcp_name_{"TCP"};
std::string ucs_name_{"Base"};
std::atomic<double> speed_scaling_{1.0};
double speed_scaling_{1.0};
std::atomic<bool> connected_{false};
mutable std::atomic<bool> servo_mode_{false};
std::atomic<bool> software_emergency_stopped_{false};
std::atomic<bool> software_protective_stopped_{false};
std::atomic<bool> busy_{false};
std::atomic<bool> servo_mode_{false};
mutable std::mutex mutex_;
mutable std::recursive_mutex sdk_mutex_;
mutable std::atomic<unsigned long long> command_seq_{0};
std::shared_ptr<RuntimeState> runtime_;
std::thread safety_monitor_thread_;
};
} // namespace cmvr::device

504
cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h
View File

@ -1,504 +0,0 @@
#ifndef CMVR_ES_HUAYAN_LIFECYCLE_STATE_H
#define CMVR_ES_HUAYAN_LIFECYCLE_STATE_H
#include <algorithm>
#include <chrono>
#include <condition_variable>
#include <cstdint>
#include <mutex>
#include <optional>
namespace cmvr::device::huayan_internal {
enum class MotionKind {
None,
Joint,
Linear,
SpeedJoint,
SpeedLinear,
Servo,
Program,
};
struct MotionToken {
std::uint64_t generation{0};
MotionKind kind{MotionKind::None};
bool valid() const noexcept
{
return generation != 0 && kind != MotionKind::None;
}
};
enum class MotionStartStatus {
Started,
Invalid,
Busy,
Stopping,
Blocked,
};
struct MotionStartResult {
MotionStartStatus status{MotionStartStatus::Busy};
MotionToken token;
bool started() const noexcept
{
return status == MotionStartStatus::Started;
}
};
enum class MotionFinishMode {
RestorePrevious,
Clear,
Retain,
};
enum class StopStartStatus {
Started,
AlreadyStopping,
};
struct StopRequest {
StopStartStatus status{StopStartStatus::AlreadyStopping};
MotionKind kind{MotionKind::None};
MotionToken active_token;
bool tracked_motion{false};
bool started() const noexcept
{
return status == StopStartStatus::Started;
}
};
struct SafetyCancelResult {
MotionKind kind{MotionKind::None};
MotionToken active_token;
bool tracked_motion{false};
};
struct MotionSnapshot {
MotionKind active_kind{MotionKind::None};
MotionKind retained_kind{MotionKind::None};
std::uint64_t active_generation{0};
bool owner_active{false};
bool stop_in_progress{false};
bool blocked{false};
};
// Tracks a single Huayan controller operation owner. Generation tokens make
// completion from an older RPC harmless after Stop or a safety event has
// cancelled it. Servo and program operations may retain their kind after the
// submitting RPC returns; begin(kind, true) supports same-kind updates while
// that retained controller mode remains active.
class MotionState final {
public:
MotionStartResult begin(
const MotionKind kind,
const bool replace_retained_same_kind = false)
{
std::lock_guard lock(mutex_);
if (kind == MotionKind::None) {
return {MotionStartStatus::Invalid, {}};
}
if (stop_in_progress_) {
return {MotionStartStatus::Stopping, {}};
}
if (blocked_) {
return {MotionStartStatus::Blocked, {}};
}
if (owner_active_) {
return {MotionStartStatus::Busy, {}};
}
if (retained_kind_ != MotionKind::None &&
(!replace_retained_same_kind || retained_kind_ != kind)) {
return {MotionStartStatus::Busy, {}};
}
const MotionToken token{++next_generation_, kind};
owner_active_ = true;
active_token_ = token;
previous_kind_ = retained_kind_;
return {MotionStartStatus::Started, token};
}
void finish(
const MotionToken token,
const MotionFinishMode mode = MotionFinishMode::RestorePrevious)
{
std::lock_guard lock(mutex_);
if (!owner_active_ ||
active_token_.generation != token.generation) {
return;
}
owner_active_ = false;
active_token_ = {};
if (!stop_in_progress_ && !blocked_) {
switch (mode) {
case MotionFinishMode::RestorePrevious:
retained_kind_ = previous_kind_;
break;
case MotionFinishMode::Clear:
retained_kind_ = MotionKind::None;
break;
case MotionFinishMode::Retain:
retained_kind_ = token.kind;
break;
}
}
previous_kind_ = MotionKind::None;
owner_finished_cv_.notify_all();
}
void failMotion(const MotionToken token)
{
std::lock_guard lock(mutex_);
if (!owner_active_ ||
active_token_.generation != token.generation) {
return;
}
owner_active_ = false;
active_token_ = {};
retained_kind_ = token.kind;
previous_kind_ = MotionKind::None;
blocked_ = true;
owner_finished_cv_.notify_all();
}
StopRequest beginStop(
const MotionKind requested_kind = MotionKind::None)
{
std::lock_guard lock(mutex_);
if (stop_in_progress_) {
return {};
}
stop_in_progress_ = true;
const MotionToken active = owner_active_
? active_token_
: MotionToken{};
if (active.valid()) {
cancelled_generation_ = std::max(
cancelled_generation_, active.generation);
}
MotionKind kind = MotionKind::None;
if (active.valid()) {
kind = active.kind;
} else if (retained_kind_ != MotionKind::None) {
kind = retained_kind_;
} else {
kind = requested_kind;
}
if (kind != MotionKind::None) {
retained_kind_ = kind;
}
return {
StopStartStatus::Started,
kind,
active,
active.valid() || kind != MotionKind::None};
}
SafetyCancelResult cancelActiveForSafety()
{
std::lock_guard lock(mutex_);
const MotionToken active = owner_active_
? active_token_
: MotionToken{};
if (active.valid()) {
cancelled_generation_ = std::max(
cancelled_generation_, active.generation);
}
const MotionKind kind = active.valid()
? active.kind
: retained_kind_;
if (kind != MotionKind::None) {
retained_kind_ = kind;
}
// A hardware safety transition is independent of a concurrent
// software Stop. New controller operations remain rejected until
// termination is positively confirmed.
blocked_ = true;
owner_finished_cv_.notify_all();
return {kind, active, active.valid() || kind != MotionKind::None};
}
bool cancelled(const MotionToken token) const
{
std::lock_guard lock(mutex_);
return token.valid() &&
token.generation <= cancelled_generation_;
}
bool waitForOwnerExit(
const MotionToken token,
const std::chrono::milliseconds timeout)
{
if (!token.valid()) {
return true;
}
std::unique_lock lock(mutex_);
return owner_finished_cv_.wait_for(
lock,
timeout,
[this, token]() {
return !owner_active_ ||
active_token_.generation != token.generation;
});
}
bool ownerActive(const MotionToken token) const
{
if (!token.valid()) {
return false;
}
std::lock_guard lock(mutex_);
return owner_active_ &&
active_token_.generation == token.generation;
}
bool completeStop()
{
std::lock_guard lock(mutex_);
if (owner_active_) {
return false;
}
stop_in_progress_ = false;
blocked_ = false;
active_token_ = {};
retained_kind_ = MotionKind::None;
previous_kind_ = MotionKind::None;
owner_finished_cv_.notify_all();
return true;
}
void failStop()
{
std::lock_guard lock(mutex_);
stop_in_progress_ = false;
blocked_ = true;
owner_finished_cv_.notify_all();
}
MotionSnapshot snapshot() const
{
std::lock_guard lock(mutex_);
return {
owner_active_ ? active_token_.kind : MotionKind::None,
retained_kind_,
owner_active_ ? active_token_.generation : 0,
owner_active_,
stop_in_progress_,
blocked_};
}
bool busy() const
{
const auto state = snapshot();
return state.owner_active || state.stop_in_progress ||
state.blocked ||
state.retained_kind != MotionKind::None;
}
private:
mutable std::mutex mutex_;
std::condition_variable owner_finished_cv_;
std::uint64_t next_generation_{0};
std::uint64_t cancelled_generation_{0};
MotionToken active_token_;
MotionKind retained_kind_{MotionKind::None};
MotionKind previous_kind_{MotionKind::None};
bool owner_active_{false};
bool stop_in_progress_{false};
bool blocked_{false};
};
enum class SafetyCondition {
Unknown,
Normal,
EmergencyStop,
SafeguardStop,
RobotFault,
EmergencySignalFault,
SafeguardSignalFault,
SoftwareEmergencyStop,
SoftwareProtectiveStop,
};
struct RawSafetyState {
bool valid{false};
int emergency_signal_fault{0};
int emergency_stop{0};
int safeguard_signal_fault{0};
int safeguard_stop{0};
int robot_fault{0};
bool software_emergency_stop{false};
bool software_protective_stop{false};
};
inline SafetyCondition classifySafetyCondition(
const RawSafetyState& state) noexcept
{
if (!state.valid) {
return SafetyCondition::Unknown;
}
if (state.emergency_signal_fault != 0) {
return SafetyCondition::EmergencySignalFault;
}
if (state.safeguard_signal_fault != 0) {
return SafetyCondition::SafeguardSignalFault;
}
if (state.emergency_stop != 0) {
return SafetyCondition::EmergencyStop;
}
if (state.safeguard_stop != 0) {
return SafetyCondition::SafeguardStop;
}
if (state.robot_fault != 0) {
return SafetyCondition::RobotFault;
}
if (state.software_emergency_stop) {
return SafetyCondition::SoftwareEmergencyStop;
}
if (state.software_protective_stop) {
return SafetyCondition::SoftwareProtectiveStop;
}
return SafetyCondition::Normal;
}
inline bool isMotionSafe(const SafetyCondition condition) noexcept
{
return condition == SafetyCondition::Normal;
}
struct SafetyPermit {
std::uint64_t epoch{0};
bool valid() const noexcept { return epoch != 0; }
};
struct RecoveryToken {
std::uint64_t epoch{0};
bool valid() const noexcept { return epoch != 0; }
};
struct SafetySnapshot {
SafetyCondition observed{SafetyCondition::Unknown};
SafetyCondition latched_reason{SafetyCondition::Unknown};
std::uint64_t epoch{0};
bool latched{false};
bool recovery_in_progress{false};
};
// Safety inputs are events, not merely levels. Returning to Normal never
// clears a prior unsafe event. Explicit recovery is tied atomically to the
// event epoch, so a second event invalidates an older in-flight recovery.
class SafetyState final {
public:
void observe(const RawSafetyState& raw_state)
{
observe(classifySafetyCondition(raw_state));
}
void observe(const SafetyCondition condition)
{
std::lock_guard lock(mutex_);
const bool changed = observed_ != condition;
observed_ = condition;
if (isMotionSafe(condition)) {
return;
}
if (!latched_ || recovery_in_progress_ || changed) {
++epoch_;
}
latched_ = true;
recovery_in_progress_ = false;
latched_reason_ = condition;
}
std::optional<SafetyPermit> tryPermit() const
{
std::lock_guard lock(mutex_);
if (latched_ || !isMotionSafe(observed_)) {
return std::nullopt;
}
return SafetyPermit{epoch_};
}
bool validate(const SafetyPermit permit) const
{
std::lock_guard lock(mutex_);
return permit.valid() && permit.epoch == epoch_ && !latched_ &&
isMotionSafe(observed_);
}
std::optional<RecoveryToken> beginRecovery(
const std::uint64_t expected_epoch)
{
std::lock_guard lock(mutex_);
if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ ||
recovery_in_progress_ || !isMotionSafe(observed_)) {
return std::nullopt;
}
recovery_in_progress_ = true;
return RecoveryToken{epoch_};
}
bool completeRecovery(
const RecoveryToken token,
const bool robot_ready,
const bool controller_idle,
const bool cancellation_confirmed)
{
std::lock_guard lock(mutex_);
if (!token.valid() || token.epoch != epoch_ || !latched_ ||
!recovery_in_progress_ || !isMotionSafe(observed_) ||
!robot_ready || !controller_idle || !cancellation_confirmed) {
return false;
}
latched_ = false;
recovery_in_progress_ = false;
latched_reason_ = SafetyCondition::Unknown;
++epoch_;
return true;
}
void failRecovery(const RecoveryToken token)
{
std::lock_guard lock(mutex_);
if (token.valid() && token.epoch == epoch_) {
recovery_in_progress_ = false;
}
}
SafetySnapshot snapshot() const
{
std::lock_guard lock(mutex_);
return {
observed_,
latched_reason_,
epoch_,
latched_,
recovery_in_progress_};
}
private:
mutable std::mutex mutex_;
SafetyCondition observed_{SafetyCondition::Unknown};
SafetyCondition latched_reason_{SafetyCondition::Unknown};
std::uint64_t epoch_{1};
bool latched_{false};
bool recovery_in_progress_{false};
};
} // namespace cmvr::device::huayan_internal
#endif // CMVR_ES_HUAYAN_LIFECYCLE_STATE_H

874
cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp
View File

@ -1,874 +0,0 @@
#include "devices/arm/huayan_arm/huayan_arm.h"
#include <algorithm>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <future>
#include <iostream>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include "HR_Pro.h"
namespace {
using Clock = std::chrono::steady_clock;
using namespace std::chrono_literals;
constexpr double kPi = 3.14159265358979323846;
double radToDeg(const double value)
{
return value * 180.0 / kPi;
}
struct FakeSdkState final {
std::mutex mutex;
bool connected{false};
bool enabled{true};
bool electrified{true};
bool robot_error{false};
bool paused{false};
bool emergency_input{false};
bool emergency_signal_fault{false};
bool safeguard_input{false};
bool safeguard_signal_fault{false};
bool software_safeguard{false};
bool motion_active{false};
bool motion_is_joint{true};
bool stop_pending{false};
bool hold_next_motion{false};
bool stale_done_once{false};
Clock::time_point completion_at{};
Clock::time_point stop_complete_at{};
std::array<double, 6> joint_position_deg{};
std::array<double, 6> joint_target_deg{};
std::array<double, 6> tcp_position_hr{};
std::array<double, 6> tcp_target_hr{};
std::string waypoint_id;
bool servo_started{false};
bool program_running{false};
std::string selected_program;
int move_j_calls{0};
int move_l_calls{0};
int speed_j_calls{0};
int speed_l_calls{0};
int group_stop_calls{0};
int group_reset_calls{0};
int start_servo_calls{0};
int stop_script_calls{0};
int idle_velocity_reads_after_stop{0};
bool count_idle_reads{false};
void refreshLocked()
{
const auto now = Clock::now();
const bool safety_active = emergency_input || safeguard_input ||
software_safeguard;
if (stop_pending && now >= stop_complete_at) {
stop_pending = false;
motion_active = false;
stale_done_once = false;
count_idle_reads = true;
}
if (motion_active && !stop_pending && !safety_active &&
completion_at != Clock::time_point{} && now >= completion_at) {
motion_active = false;
stale_done_once = false;
if (motion_is_joint) {
joint_position_deg = joint_target_deg;
} else {
tcp_position_hr = tcp_target_hr;
}
}
}
bool movingLocked()
{
refreshLocked();
return motion_active && !emergency_input && !safeguard_input &&
!software_safeguard;
}
bool doneLocked()
{
refreshLocked();
return !motion_active;
}
void startMotionLocked(const bool joint)
{
motion_active = true;
motion_is_joint = joint;
stop_pending = false;
count_idle_reads = false;
stale_done_once = true;
if (hold_next_motion) {
// The fallback deadline keeps a failed test from leaving a worker
// blocked for the production 60 second timeout.
completion_at = Clock::now() + 3s;
hold_next_motion = false;
} else {
completion_at = Clock::now() + 120ms;
}
}
};
FakeSdkState g_sdk;
void resetFakeSdk()
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.connected = false;
g_sdk.enabled = true;
g_sdk.electrified = true;
g_sdk.robot_error = false;
g_sdk.paused = false;
g_sdk.emergency_input = false;
g_sdk.emergency_signal_fault = false;
g_sdk.safeguard_input = false;
g_sdk.safeguard_signal_fault = false;
g_sdk.software_safeguard = false;
g_sdk.motion_active = false;
g_sdk.motion_is_joint = true;
g_sdk.stop_pending = false;
g_sdk.hold_next_motion = false;
g_sdk.stale_done_once = false;
g_sdk.completion_at = {};
g_sdk.stop_complete_at = {};
g_sdk.joint_position_deg = {};
g_sdk.joint_target_deg = {};
g_sdk.tcp_position_hr = {};
g_sdk.tcp_target_hr = {};
g_sdk.waypoint_id.clear();
g_sdk.servo_started = false;
g_sdk.program_running = false;
g_sdk.selected_program.clear();
g_sdk.move_j_calls = 0;
g_sdk.move_l_calls = 0;
g_sdk.speed_j_calls = 0;
g_sdk.speed_l_calls = 0;
g_sdk.group_stop_calls = 0;
g_sdk.group_reset_calls = 0;
g_sdk.start_servo_calls = 0;
g_sdk.stop_script_calls = 0;
g_sdk.idle_velocity_reads_after_stop = 0;
g_sdk.count_idle_reads = false;
}
void holdNextMotion()
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.hold_next_motion = true;
}
void setHardwareEmergencyStop(const bool active)
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.emergency_input = active;
// If the wrapper never sends a real group Stop, releasing the switch makes
// the pending fake waypoint move again. This models the field failure.
}
void setEmergencySignalFault(const bool active)
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.emergency_signal_fault = active;
}
void dropFakeTransport()
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.connected = false;
}
template <typename Predicate>
bool waitUntil(Predicate&& predicate,
const std::chrono::milliseconds timeout = 2s)
{
const auto deadline = Clock::now() + timeout;
while (Clock::now() < deadline) {
if (predicate()) {
return true;
}
std::this_thread::sleep_for(10ms);
}
return predicate();
}
cmvr::config::RobotArmConfig makeConfig()
{
cmvr::config::RobotArmConfig cfg;
cfg.set_id("huayan_fake_sdk");
auto* vendor = cfg.mutable_vendor();
vendor->set_brand(cmvr::config::VENDOR_ROBOT_ARM_BRAND_HUAYAN_ARM);
vendor->set_ip("127.0.0.1");
vendor->set_port(10003);
vendor->set_model("HuayanFake");
vendor->set_dof(6);
vendor->set_base_frame("Base");
vendor->set_tool_frame("TCP");
for (int i = 1; i <= 6; ++i) {
vendor->add_joint_names("joint_" + std::to_string(i));
}
return cfg;
}
int failures = 0;
#define CHECK_TRUE(condition) \
do { \
if (!(condition)) { \
std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \
<< #condition << std::endl; \
++failures; \
} \
} while (false)
} // namespace
// The test executable exports these strong symbols. On ELF platforms they
// interpose the real SDK definitions used by libhuayan_arm, giving the test a
// deterministic controller without opening a network connection.
extern "C" {
int HRIF_Connect(unsigned int, const char*, unsigned short)
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.connected = true;
return 0;
}
int HRIF_DisConnect(unsigned int)
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.connected = false;
g_sdk.motion_active = false;
return 0;
}
bool HRIF_IsConnected(unsigned int)
{
std::lock_guard lock(g_sdk.mutex);
return g_sdk.connected;
}
int HRIF_GetErrorCodeStr(unsigned int, int error_code, std::string& message)
{
message = "fake SDK error " + std::to_string(error_code);
return 0;
}
int HRIF_GrpEnable(unsigned int, unsigned int)
{
std::lock_guard lock(g_sdk.mutex);
if (g_sdk.emergency_input || g_sdk.safeguard_input ||
g_sdk.software_safeguard) {
return 101;
}
g_sdk.enabled = true;
g_sdk.electrified = true;
return 0;
}
int HRIF_GrpDisable(unsigned int, unsigned int)
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.enabled = false;
g_sdk.electrified = false;
return 0;
}
int HRIF_GrpReset(unsigned int, unsigned int)
{
std::lock_guard lock(g_sdk.mutex);
++g_sdk.group_reset_calls;
if (g_sdk.emergency_input || g_sdk.safeguard_input ||
g_sdk.software_safeguard) {
return 102;
}
g_sdk.robot_error = false;
return 0;
}
int HRIF_GrpStop(unsigned int, unsigned int)
{
std::lock_guard lock(g_sdk.mutex);
++g_sdk.group_stop_calls;
g_sdk.idle_velocity_reads_after_stop = 0;
g_sdk.count_idle_reads = false;
if (g_sdk.motion_active) {
g_sdk.stop_pending = true;
g_sdk.stop_complete_at = Clock::now() + 120ms;
g_sdk.completion_at = {};
} else {
g_sdk.stop_pending = false;
g_sdk.count_idle_reads = true;
}
g_sdk.servo_started = false;
return 0;
}
int HRIF_SetOverride(unsigned int, unsigned int, double)
{
return 0;
}
int HRIF_ReadRobotState(unsigned int, unsigned int,
int& moving, int& enabled, int& error,
int& error_code, int& error_axis, int& brake,
int& paused, int& emergency_stop, int& safeguard,
int& electrified, int& connected_to_box,
int& blending_done, int& in_position)
{
std::lock_guard lock(g_sdk.mutex);
if (!g_sdk.connected) {
return 201;
}
moving = g_sdk.movingLocked() ? 1 : 0;
enabled = g_sdk.enabled ? 1 : 0;
error = g_sdk.robot_error ? 1 : 0;
error_code = g_sdk.robot_error ? 9001 : 0;
error_axis = 0;
brake = g_sdk.enabled ? 1 : 0;
paused = g_sdk.paused ? 1 : 0;
emergency_stop = g_sdk.emergency_input ? 1 : 0;
safeguard = (g_sdk.safeguard_input || g_sdk.software_safeguard) ? 1 : 0;
electrified = g_sdk.electrified ? 1 : 0;
connected_to_box = 1;
blending_done = moving == 0 ? 1 : 0;
in_position = g_sdk.doneLocked() ? 1 : 0;
return 0;
}
int HRIF_ReadEmergencyInfo(unsigned int, unsigned int,
int& emergency_signal_fault,
int& emergency_input,
int& safeguard_signal_fault,
int& safeguard_input)
{
std::lock_guard lock(g_sdk.mutex);
if (!g_sdk.connected) {
return 202;
}
emergency_signal_fault = g_sdk.emergency_signal_fault ? 1 : 0;
emergency_input = g_sdk.emergency_input ? 1 : 0;
safeguard_signal_fault = g_sdk.safeguard_signal_fault ? 1 : 0;
safeguard_input =
(g_sdk.safeguard_input || g_sdk.software_safeguard) ? 1 : 0;
return 0;
}
int HRIF_ReadCurWaypointID(unsigned int, unsigned int, std::string& waypoint)
{
std::lock_guard lock(g_sdk.mutex);
waypoint = g_sdk.waypoint_id;
return 0;
}
int HRIF_IsMotionDone(unsigned int, unsigned int, bool& done)
{
std::lock_guard lock(g_sdk.mutex);
if (g_sdk.stale_done_once) {
g_sdk.stale_done_once = false;
done = true;
} else {
done = g_sdk.doneLocked();
}
return 0;
}
int HRIF_ReadActJointPos(unsigned int, unsigned int,
double& j1, double& j2, double& j3,
double& j4, double& j5, double& j6)
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.refreshLocked();
j1 = g_sdk.joint_position_deg[0];
j2 = g_sdk.joint_position_deg[1];
j3 = g_sdk.joint_position_deg[2];
j4 = g_sdk.joint_position_deg[3];
j5 = g_sdk.joint_position_deg[4];
j6 = g_sdk.joint_position_deg[5];
return 0;
}
int HRIF_ReadActJointVel(unsigned int, unsigned int,
double& j1, double& j2, double& j3,
double& j4, double& j5, double& j6)
{
std::lock_guard lock(g_sdk.mutex);
const double velocity = g_sdk.movingLocked() ? 5.0 : 0.0;
j1 = j2 = j3 = j4 = j5 = j6 = velocity;
if (velocity == 0.0 && g_sdk.count_idle_reads) {
++g_sdk.idle_velocity_reads_after_stop;
}
return 0;
}
int HRIF_ReadActTcpPos(unsigned int, unsigned int,
double& x, double& y, double& z,
double& rx, double& ry, double& rz)
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.refreshLocked();
x = g_sdk.tcp_position_hr[0];
y = g_sdk.tcp_position_hr[1];
z = g_sdk.tcp_position_hr[2];
rx = g_sdk.tcp_position_hr[3];
ry = g_sdk.tcp_position_hr[4];
rz = g_sdk.tcp_position_hr[5];
return 0;
}
int HRIF_ReadActTcpVel(unsigned int, unsigned int,
double& x, double& y, double& z,
double& rx, double& ry, double& rz)
{
std::lock_guard lock(g_sdk.mutex);
const double velocity = g_sdk.movingLocked() ? 5.0 : 0.0;
x = y = z = rx = ry = rz = velocity;
return 0;
}
int HRIF_MoveJ(unsigned int, unsigned int,
double, double, double, double, double, double,
double j1, double j2, double j3,
double j4, double j5, double j6,
std::string, std::string, double, double, double,
int, int, int, int, std::string command_id)
{
std::lock_guard lock(g_sdk.mutex);
++g_sdk.move_j_calls;
g_sdk.joint_target_deg = {j1, j2, j3, j4, j5, j6};
g_sdk.waypoint_id = std::move(command_id);
g_sdk.startMotionLocked(true);
return 0;
}
int HRIF_MoveL(unsigned int, unsigned int,
double x, double y, double z,
double rx, double ry, double rz,
double, double, double, double, double, double,
std::string, std::string, double, double, double,
int, int, int, std::string command_id)
{
std::lock_guard lock(g_sdk.mutex);
++g_sdk.move_l_calls;
g_sdk.tcp_target_hr = {x, y, z, rx, ry, rz};
g_sdk.waypoint_id = std::move(command_id);
g_sdk.startMotionLocked(false);
return 0;
}
int HRIF_SpeedJ(unsigned int, unsigned int,
double, double, double, double, double, double,
double, double)
{
std::lock_guard lock(g_sdk.mutex);
++g_sdk.speed_j_calls;
g_sdk.startMotionLocked(true);
return 0;
}
int HRIF_SpeedL(unsigned int, unsigned int,
double, double, double, double, double, double,
double, double, double)
{
std::lock_guard lock(g_sdk.mutex);
++g_sdk.speed_l_calls;
g_sdk.startMotionLocked(false);
return 0;
}
int HRIF_StartServo(unsigned int, unsigned int, double, double)
{
std::lock_guard lock(g_sdk.mutex);
++g_sdk.start_servo_calls;
g_sdk.servo_started = true;
return 0;
}
int HRIF_PushServoJ(unsigned int, unsigned int,
double j1, double j2, double j3,
double j4, double j5, double j6)
{
std::lock_guard lock(g_sdk.mutex);
if (!g_sdk.servo_started) {
return 301;
}
g_sdk.joint_position_deg = {j1, j2, j3, j4, j5, j6};
return 0;
}
int HRIF_PushServoP(unsigned int, unsigned int,
std::vector<double>& coord,
std::vector<double>&,
std::vector<double>&)
{
std::lock_guard lock(g_sdk.mutex);
if (!g_sdk.servo_started || coord.size() < 6) {
return 302;
}
std::copy_n(coord.begin(), 6, g_sdk.tcp_position_hr.begin());
return 0;
}
int HRIF_SwitchScript(unsigned int, unsigned int, std::string script_name)
{
std::lock_guard lock(g_sdk.mutex);
if (script_name.empty()) {
return 401;
}
g_sdk.selected_program = std::move(script_name);
return 0;
}
int HRIF_StartScript(unsigned int)
{
std::lock_guard lock(g_sdk.mutex);
if (g_sdk.selected_program.empty()) {
return 402;
}
g_sdk.program_running = true;
g_sdk.paused = false;
return 0;
}
int HRIF_PauseScript(unsigned int)
{
std::lock_guard lock(g_sdk.mutex);
if (!g_sdk.program_running) {
return 403;
}
g_sdk.paused = true;
return 0;
}
int HRIF_StopScript(unsigned int)
{
std::lock_guard lock(g_sdk.mutex);
++g_sdk.stop_script_calls;
g_sdk.program_running = false;
g_sdk.paused = false;
return 0;
}
int HRIF_EnterSafetyGuard(unsigned int, unsigned int, int flag)
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.software_safeguard = flag != 0;
return 0;
}
int HRIF_ShutdownRobot(unsigned int)
{
std::lock_guard lock(g_sdk.mutex);
g_sdk.connected = false;
g_sdk.enabled = false;
g_sdk.electrified = false;
return 0;
}
} // extern "C"
int main()
{
using namespace cmvr::device;
resetFakeSdk();
HuayanRobot arm(makeConfig());
CHECK_TRUE(arm.connect("127.0.0.1", 10003).ok());
MotionOptions options;
options.velocity = 0.4;
options.acceleration = 0.8;
CHECK_TRUE(arm.supportsActionQueueMotion());
JointPositionCommand joint_a{{0.10, -0.05, 0.08, 0.0, 0.02, -0.03}};
MotionOptions cancelled_options = options;
cancelled_options.cancellation_requested = []() { return true; };
CHECK_TRUE(!arm.moveJ(joint_a, cancelled_options).ok());
CartesianPose cancelled_pose;
cancelled_pose.x = 0.20;
cancelled_pose.z = 0.30;
CHECK_TRUE(!arm.moveL(cancelled_pose, cancelled_options).ok());
{
std::lock_guard lock(g_sdk.mutex);
CHECK_TRUE(g_sdk.move_j_calls == 0);
CHECK_TRUE(g_sdk.move_l_calls == 0);
}
// Once a command has been accepted, caller cancellation returns promptly
// but retains the typed motion barrier until Stop confirms controller idle.
std::atomic<bool> cancel_during_wait{false};
MotionOptions cancellable_options = options;
cancellable_options.cancellation_requested = [&cancel_during_wait]() {
return cancel_during_wait.load();
};
JointPositionCommand cancelled_in_wait{
{0.05, -0.02, 0.04, 0.01, 0.0, -0.01}};
holdNextMotion();
auto cancelled_motion = std::async(std::launch::async, [&]() {
return arm.moveJ(cancelled_in_wait, cancellable_options);
});
CHECK_TRUE(waitUntil([&]() {
std::lock_guard lock(g_sdk.mutex);
return g_sdk.move_j_calls > 0;
}));
cancel_during_wait.store(true);
CHECK_TRUE(cancelled_motion.wait_for(1s) == std::future_status::ready);
if (cancelled_motion.wait_for(0ms) == std::future_status::ready) {
CHECK_TRUE(!cancelled_motion.get().ok());
}
CHECK_TRUE(arm.busy());
CHECK_TRUE(arm.stopMotion().ok());
CHECK_TRUE(!arm.busy());
// The first IsMotionDone read intentionally reports the preceding idle
// state. Completion must be correlated with the command/target. Once the
// target is reached, an identical command is an idempotent no-op.
CHECK_TRUE(arm.moveJ(joint_a, options).ok());
int move_j_after_first = 0;
{
std::lock_guard lock(g_sdk.mutex);
move_j_after_first = g_sdk.move_j_calls;
}
CHECK_TRUE(arm.moveJ(joint_a, options).ok());
{
std::lock_guard lock(g_sdk.mutex);
CHECK_TRUE(g_sdk.move_j_calls == move_j_after_first);
}
CartesianPose pose_a;
pose_a.x = 0.31;
pose_a.y = -0.12;
pose_a.z = 0.42;
pose_a.rx = 0.08;
pose_a.ry = -0.04;
pose_a.rz = 0.12;
CHECK_TRUE(arm.moveL(pose_a, options).ok());
int move_l_after_first = 0;
{
std::lock_guard lock(g_sdk.mutex);
move_l_after_first = g_sdk.move_l_calls;
}
CHECK_TRUE(arm.moveL(pose_a, options).ok());
{
std::lock_guard lock(g_sdk.mutex);
CHECK_TRUE(g_sdk.move_l_calls == move_l_after_first);
}
// Stop must cancel the old owner and wait until the controller reports
// stable idle; clearing the owner immediately after GrpStop would fail the
// elapsed-time and consecutive-idle checks below.
JointPositionCommand joint_b{{0.22, -0.08, 0.14, 0.03, 0.04, -0.01}};
holdNextMotion();
const int before_held_move = move_j_after_first;
auto held_move = std::async(std::launch::async, [&]() {
return arm.moveJ(joint_b, options);
});
CHECK_TRUE(waitUntil([&]() {
std::lock_guard lock(g_sdk.mutex);
return g_sdk.move_j_calls > before_held_move;
}));
const auto stop_started = Clock::now();
CHECK_TRUE(arm.stopMotion().ok());
const auto stop_elapsed = Clock::now() - stop_started;
CHECK_TRUE(stop_elapsed >= 100ms);
CHECK_TRUE(held_move.wait_for(1s) == std::future_status::ready);
if (held_move.wait_for(0ms) == std::future_status::ready) {
CHECK_TRUE(!held_move.get().ok());
}
CHECK_TRUE(!arm.busy());
{
std::lock_guard lock(g_sdk.mutex);
CHECK_TRUE(g_sdk.idle_velocity_reads_after_stop >= 3);
}
CHECK_TRUE(arm.moveJ(joint_b, options).ok());
// A hardware E-stop cancels and terminates the active waypoint. Releasing
// the switch does not clear the software latch or grant a new permit.
JointPositionCommand joint_c{{0.34, -0.02, 0.09, 0.05, -0.02, 0.07}};
holdNextMotion();
int before_estop_move = 0;
int before_estop_stop = 0;
{
std::lock_guard lock(g_sdk.mutex);
before_estop_move = g_sdk.move_j_calls;
before_estop_stop = g_sdk.group_stop_calls;
}
auto estop_move = std::async(std::launch::async, [&]() {
return arm.moveJ(joint_c, options);
});
CHECK_TRUE(waitUntil([&]() {
std::lock_guard lock(g_sdk.mutex);
return g_sdk.move_j_calls > before_estop_move;
}));
setHardwareEmergencyStop(true);
CHECK_TRUE(waitUntil([&]() {
std::lock_guard lock(g_sdk.mutex);
return g_sdk.group_stop_calls > before_estop_stop;
}));
CHECK_TRUE(estop_move.wait_for(2s) == std::future_status::ready);
if (estop_move.wait_for(0ms) == std::future_status::ready) {
CHECK_TRUE(!estop_move.get().ok());
}
setHardwareEmergencyStop(false);
std::this_thread::sleep_for(150ms);
int move_count_while_latched = 0;
{
std::lock_guard lock(g_sdk.mutex);
move_count_while_latched = g_sdk.move_j_calls;
}
const auto rejected_while_latched = arm.moveJ(joint_a, options);
CHECK_TRUE(!rejected_while_latched.ok());
{
std::lock_guard lock(g_sdk.mutex);
CHECK_TRUE(g_sdk.move_j_calls == move_count_while_latched);
}
CHECK_TRUE(arm.clearFault().ok());
CHECK_TRUE(arm.torqueOn().ok());
CHECK_TRUE(arm.moveJ(joint_a, options).ok());
// Speed commands own the controller while waiting. A different motion is
// rejected, and Stop releases ownership only after termination.
holdNextMotion();
JointVelocityCommand speed{{0.1, 0.0, 0.0, 0.0, 0.0, 0.0}};
int speed_calls_before = 0;
{
std::lock_guard lock(g_sdk.mutex);
speed_calls_before = g_sdk.speed_j_calls;
}
auto speed_motion = std::async(std::launch::async, [&]() {
return arm.speedJ(speed, 0.5, 2.0);
});
CHECK_TRUE(waitUntil([&]() {
std::lock_guard lock(g_sdk.mutex);
return g_sdk.speed_j_calls > speed_calls_before;
}));
CHECK_TRUE(!arm.moveL(pose_a, options).ok());
CHECK_TRUE(arm.stopMotion().ok());
CHECK_TRUE(speed_motion.wait_for(1s) == std::future_status::ready);
if (speed_motion.wait_for(0ms) == std::future_status::ready) {
CHECK_TRUE(!speed_motion.get().ok());
}
CHECK_TRUE(!arm.busy());
// SpeedL used to hold the SDK mutex while waiting, which deadlocked its
// own timeout/Stop path. A concurrent Stop must cancel it, settle the
// controller, and allow a genuinely new Move command afterwards.
holdNextMotion();
CartesianVelocity line_speed;
line_speed.vx = 0.05;
int speed_l_calls_before = 0;
{
std::lock_guard lock(g_sdk.mutex);
speed_l_calls_before = g_sdk.speed_l_calls;
}
auto line_speed_motion = std::async(std::launch::async, [&]() {
return arm.speedL(line_speed, 0.5, 2.0, FrameType::Base);
});
CHECK_TRUE(waitUntil([&]() {
std::lock_guard lock(g_sdk.mutex);
return g_sdk.speed_l_calls > speed_l_calls_before;
}));
CHECK_TRUE(arm.stopMotion().ok());
CHECK_TRUE(line_speed_motion.wait_for(1s) == std::future_status::ready);
if (line_speed_motion.wait_for(0ms) == std::future_status::ready) {
CHECK_TRUE(!line_speed_motion.get().ok());
}
CHECK_TRUE(!arm.busy());
CHECK_TRUE(arm.moveJ(joint_b, options).ok());
// A dual-channel emergency input mismatch is a typed emergency latch. It
// remains blocked after the wiring level is healthy and is recovered only
// through the emergency recovery path.
int stops_before_signal_fault = 0;
{
std::lock_guard lock(g_sdk.mutex);
stops_before_signal_fault = g_sdk.group_stop_calls;
}
setEmergencySignalFault(true);
CHECK_TRUE(waitUntil([&]() {
std::lock_guard lock(g_sdk.mutex);
return g_sdk.group_stop_calls > stops_before_signal_fault;
}));
setEmergencySignalFault(false);
std::this_thread::sleep_for(100ms);
CHECK_TRUE(!arm.moveJ(joint_c, options).ok());
CHECK_TRUE(arm.torqueOn().ok());
CHECK_TRUE(arm.moveJ(joint_c, options).ok());
// Power-off holds a terminal barrier through GrpDisable. Motion remains
// denied until an explicit enable confirms the powered state again.
CHECK_TRUE(arm.torqueOff().ok());
CHECK_TRUE(!arm.moveJ(joint_a, options).ok());
CHECK_TRUE(arm.torqueOn().ok());
CHECK_TRUE(arm.moveJ(joint_a, options).ok());
// Servo and program modes retain ownership beyond the start call. Stop of
// a retained program must use StopScript as well as the group stop path.
ServoOptions servo_options;
CHECK_TRUE(arm.startServoMode(servo_options).ok());
CHECK_TRUE(!arm.moveJ(joint_b, options).ok());
CHECK_TRUE(arm.servoJ(joint_b).ok());
CHECK_TRUE(arm.stopServoMode().ok());
CHECK_TRUE(!arm.busy());
CHECK_TRUE(arm.loadProgram("fake_program.script").ok());
CHECK_TRUE(arm.playProgram().ok());
CHECK_TRUE(!arm.moveJ(joint_c, options).ok());
int stop_script_calls_before = 0;
{
std::lock_guard lock(g_sdk.mutex);
stop_script_calls_before = g_sdk.stop_script_calls;
}
CHECK_TRUE(arm.stopMotion().ok());
{
std::lock_guard lock(g_sdk.mutex);
CHECK_TRUE(g_sdk.stop_script_calls > stop_script_calls_before);
}
CHECK_TRUE(!arm.busy());
// Retire an in-flight stale generation after transport loss before
// reconnecting; no old waiter may issue SDK reads into the new session.
holdNextMotion();
int moves_before_transport_loss = 0;
{
std::lock_guard lock(g_sdk.mutex);
moves_before_transport_loss = g_sdk.move_j_calls;
}
auto transport_lost_move = std::async(std::launch::async, [&]() {
return arm.moveJ(joint_c, options);
});
CHECK_TRUE(waitUntil([&]() {
std::lock_guard lock(g_sdk.mutex);
return g_sdk.move_j_calls > moves_before_transport_loss;
}));
dropFakeTransport();
CHECK_TRUE(arm.connect("127.0.0.1", 10003).ok());
CHECK_TRUE(transport_lost_move.wait_for(1s) == std::future_status::ready);
if (transport_lost_move.wait_for(0ms) == std::future_status::ready) {
CHECK_TRUE(!transport_lost_move.get().ok());
}
CHECK_TRUE(arm.moveJ(joint_b, options).ok());
CHECK_TRUE(arm.disconnect().ok());
resetFakeSdk();
HuayanRobot shutdown_arm(makeConfig());
CHECK_TRUE(shutdown_arm.connect("127.0.0.1", 10003).ok());
CHECK_TRUE(shutdown_arm.shutdown().ok());
CHECK_TRUE(!shutdown_arm.isConnected());
return failures == 0 ? 0 : 1;
}

232
cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp
View File

@ -1,232 +0,0 @@
#include "devices/arm/huayan_arm/huayan_lifecycle_state.h"
#include <chrono>
#include <iostream>
namespace {
#define CHECK_TRUE(condition) \
do { \
if (!(condition)) { \
std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \
<< #condition << std::endl; \
return 1; \
} \
} while (false)
} // namespace
int main()
{
using namespace cmvr::device::huayan_internal;
MotionState motion;
CHECK_TRUE(motion.begin(MotionKind::None).status ==
MotionStartStatus::Invalid);
// A completed target does not poison an identical subsequent command,
// while an actually concurrent command is rejected.
const auto first_joint = motion.begin(MotionKind::Joint);
CHECK_TRUE(first_joint.started());
CHECK_TRUE(motion.begin(MotionKind::Joint).status ==
MotionStartStatus::Busy);
CHECK_TRUE(motion.begin(MotionKind::Linear).status ==
MotionStartStatus::Busy);
motion.finish(first_joint.token);
const auto repeated_joint = motion.begin(MotionKind::Joint);
CHECK_TRUE(repeated_joint.started());
CHECK_TRUE(repeated_joint.token.generation >
first_joint.token.generation);
motion.finish(first_joint.token);
CHECK_TRUE(motion.ownerActive(repeated_joint.token));
motion.finish(repeated_joint.token);
CHECK_TRUE(!motion.busy());
// Stop cancels the current generation and cannot complete before its
// owner exits.
const auto linear = motion.begin(MotionKind::Linear);
CHECK_TRUE(linear.started());
const auto stop_linear = motion.beginStop();
CHECK_TRUE(stop_linear.started());
CHECK_TRUE(stop_linear.kind == MotionKind::Linear);
CHECK_TRUE(stop_linear.active_token.generation ==
linear.token.generation);
CHECK_TRUE(stop_linear.tracked_motion);
CHECK_TRUE(motion.cancelled(linear.token));
CHECK_TRUE(motion.beginStop().status ==
StopStartStatus::AlreadyStopping);
CHECK_TRUE(motion.begin(MotionKind::Joint).status ==
MotionStartStatus::Stopping);
CHECK_TRUE(!motion.waitForOwnerExit(
linear.token, std::chrono::milliseconds(1)));
CHECK_TRUE(!motion.completeStop());
motion.finish(linear.token);
CHECK_TRUE(motion.waitForOwnerExit(
linear.token, std::chrono::milliseconds(1)));
CHECK_TRUE(motion.completeStop());
CHECK_TRUE(!motion.busy());
// An uncertain submission/completion remains fail-closed until a
// positively acknowledged Stop clears it.
const auto failed_speed = motion.begin(MotionKind::SpeedLinear);
CHECK_TRUE(failed_speed.started());
motion.failMotion(failed_speed.token);
CHECK_TRUE(motion.snapshot().blocked);
CHECK_TRUE(motion.begin(MotionKind::Joint).status ==
MotionStartStatus::Blocked);
const auto stop_failed_speed = motion.beginStop();
CHECK_TRUE(stop_failed_speed.kind == MotionKind::SpeedLinear);
CHECK_TRUE(stop_failed_speed.tracked_motion);
CHECK_TRUE(motion.completeStop());
const auto failed_stop_motion = motion.begin(MotionKind::Joint);
CHECK_TRUE(failed_stop_motion.started());
const auto failed_stop = motion.beginStop();
CHECK_TRUE(failed_stop.kind == MotionKind::Joint);
motion.failStop();
CHECK_TRUE(motion.snapshot().blocked);
CHECK_TRUE(motion.begin(MotionKind::Linear).status ==
MotionStartStatus::Blocked);
motion.finish(failed_stop_motion.token);
const auto retry_failed_stop = motion.beginStop();
CHECK_TRUE(retry_failed_stop.kind == MotionKind::Joint);
CHECK_TRUE(motion.completeStop());
// Servo and program modes remain owned after their start RPC returns.
const auto servo = motion.begin(MotionKind::Servo);
CHECK_TRUE(servo.started());
motion.finish(servo.token, MotionFinishMode::Retain);
CHECK_TRUE(motion.snapshot().retained_kind == MotionKind::Servo);
CHECK_TRUE(motion.begin(MotionKind::Program).status ==
MotionStartStatus::Busy);
const auto servo_update = motion.begin(MotionKind::Servo, true);
CHECK_TRUE(servo_update.started());
motion.finish(servo_update.token);
CHECK_TRUE(motion.snapshot().retained_kind == MotionKind::Servo);
const auto stop_servo = motion.beginStop();
CHECK_TRUE(stop_servo.kind == MotionKind::Servo);
CHECK_TRUE(stop_servo.tracked_motion);
CHECK_TRUE(motion.completeStop());
const auto program = motion.begin(MotionKind::Program);
CHECK_TRUE(program.started());
motion.finish(program.token, MotionFinishMode::Retain);
const auto cancelled_program = motion.cancelActiveForSafety();
CHECK_TRUE(cancelled_program.kind == MotionKind::Program);
CHECK_TRUE(cancelled_program.tracked_motion);
CHECK_TRUE(!cancelled_program.active_token.valid());
CHECK_TRUE(motion.begin(MotionKind::Joint).status ==
MotionStartStatus::Blocked);
const auto stop_program = motion.beginStop();
CHECK_TRUE(stop_program.kind == MotionKind::Program);
CHECK_TRUE(motion.completeStop());
const auto safety_move = motion.begin(MotionKind::SpeedJoint);
CHECK_TRUE(safety_move.started());
const auto cancelled_move = motion.cancelActiveForSafety();
CHECK_TRUE(cancelled_move.kind == MotionKind::SpeedJoint);
CHECK_TRUE(cancelled_move.active_token.generation ==
safety_move.token.generation);
CHECK_TRUE(motion.cancelled(safety_move.token));
motion.finish(safety_move.token);
const auto stop_safety_move = motion.beginStop();
CHECK_TRUE(stop_safety_move.kind == MotionKind::SpeedJoint);
CHECK_TRUE(motion.completeStop());
RawSafetyState raw;
CHECK_TRUE(classifySafetyCondition(raw) == SafetyCondition::Unknown);
raw.valid = true;
CHECK_TRUE(classifySafetyCondition(raw) == SafetyCondition::Normal);
raw.software_protective_stop = true;
CHECK_TRUE(classifySafetyCondition(raw) ==
SafetyCondition::SoftwareProtectiveStop);
raw.software_emergency_stop = true;
CHECK_TRUE(classifySafetyCondition(raw) ==
SafetyCondition::SoftwareEmergencyStop);
raw.robot_fault = 1;
CHECK_TRUE(classifySafetyCondition(raw) ==
SafetyCondition::RobotFault);
raw.safeguard_stop = 1;
CHECK_TRUE(classifySafetyCondition(raw) ==
SafetyCondition::SafeguardStop);
raw.emergency_stop = 1;
CHECK_TRUE(classifySafetyCondition(raw) ==
SafetyCondition::EmergencyStop);
raw.safeguard_signal_fault = 1;
CHECK_TRUE(classifySafetyCondition(raw) ==
SafetyCondition::SafeguardSignalFault);
raw.emergency_signal_fault = 1;
CHECK_TRUE(classifySafetyCondition(raw) ==
SafetyCondition::EmergencySignalFault);
SafetyState safety;
CHECK_TRUE(!safety.tryPermit().has_value());
safety.observe(SafetyCondition::Normal);
const auto initial_permit = safety.tryPermit();
CHECK_TRUE(initial_permit.has_value());
CHECK_TRUE(safety.validate(*initial_permit));
safety.observe(SafetyCondition::EmergencyStop);
CHECK_TRUE(safety.snapshot().latched);
CHECK_TRUE(!safety.validate(*initial_permit));
CHECK_TRUE(!safety.beginRecovery(safety.snapshot().epoch).has_value());
const auto first_emergency_epoch = safety.snapshot().epoch;
safety.observe(SafetyCondition::EmergencyStop);
safety.observe(SafetyCondition::EmergencyStop);
CHECK_TRUE(safety.snapshot().epoch == first_emergency_epoch);
// Releasing the hardware switch only changes the observed level; it does
// not clear the event latch or issue a new motion permit.
safety.observe(SafetyCondition::Normal);
CHECK_TRUE(safety.snapshot().latched);
CHECK_TRUE(!safety.tryPermit().has_value());
const auto not_ready = safety.beginRecovery(safety.snapshot().epoch);
CHECK_TRUE(not_ready.has_value());
CHECK_TRUE(!safety.completeRecovery(*not_ready, false, true, true));
CHECK_TRUE(safety.snapshot().latched);
safety.failRecovery(*not_ready);
const auto not_idle = safety.beginRecovery(safety.snapshot().epoch);
CHECK_TRUE(not_idle.has_value());
CHECK_TRUE(!safety.completeRecovery(*not_idle, true, false, true));
CHECK_TRUE(safety.snapshot().latched);
safety.failRecovery(*not_idle);
const auto not_cancelled =
safety.beginRecovery(safety.snapshot().epoch);
CHECK_TRUE(not_cancelled.has_value());
CHECK_TRUE(!safety.completeRecovery(*not_cancelled, true, true, false));
CHECK_TRUE(safety.snapshot().latched);
safety.failRecovery(*not_cancelled);
const auto recovery_retry =
safety.beginRecovery(safety.snapshot().epoch);
CHECK_TRUE(recovery_retry.has_value());
CHECK_TRUE(safety.completeRecovery(
*recovery_retry, true, true, true));
const auto recovered_permit = safety.tryPermit();
CHECK_TRUE(recovered_permit.has_value());
CHECK_TRUE(safety.validate(*recovered_permit));
// A second safety event, including the same physical E-stop being pressed
// again, invalidates an older recovery token atomically.
safety.observe(SafetyCondition::EmergencyStop);
safety.observe(SafetyCondition::Normal);
const auto stale_recovery =
safety.beginRecovery(safety.snapshot().epoch);
CHECK_TRUE(stale_recovery.has_value());
safety.observe(SafetyCondition::EmergencyStop);
safety.observe(SafetyCondition::Normal);
CHECK_TRUE(!safety.completeRecovery(
*stale_recovery, true, true, true));
CHECK_TRUE(safety.snapshot().latched);
CHECK_TRUE(!safety.snapshot().recovery_in_progress);
const auto stale_epoch = safety.snapshot().epoch;
safety.observe(SafetyCondition::SafeguardStop);
safety.observe(SafetyCondition::Normal);
CHECK_TRUE(!safety.beginRecovery(stale_epoch).has_value());
return 0;
}

1
cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt
View File

@ -26,7 +26,6 @@ add_executable(motor_robot_arm_mujoco_test
target_link_libraries(motor_robot_arm_mujoco_test
PRIVATE
cmvr_es::device::motor_robot_arm
cmvr_es::algorithms::arm_control
cmvr_es::device::motor_manager
cmvr_es::device::mujoco_motor_driver
cmvr_es::mujoco_viewer

22
cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h
View File

@ -36,13 +36,6 @@ public:
RobotMode getRobotMode() const override { return RobotMode::Idle; }
SafetyMode getSafetyMode() const override;
ControlMode getControlMode() const override { return ControlMode::Position; }
bool supportsTeleopGroupServo() const noexcept override
{
// commandCyclicPosition is currently dispatched one joint at a time.
// A config switch cannot turn that partial-write behavior into the
// atomic/timed group primitive required by network teleoperation.
return false;
}
Result torqueOn() override;
Result torqueOff() override;
@ -103,12 +96,6 @@ public:
bool busy() const override;
private:
enum class TrajectoryExecutionResult {
Completed,
Canceled,
Failed,
};
bool containsJoint_(const std::string& joint_name) const;
bool safetyStopRequested_() const;
std::optional<Result> safetyStopResult_(const std::string& command,
@ -121,10 +108,7 @@ private:
std::vector<double> readJointPosition_() const;
bool configureAlgorithms_();
TrajectoryExecutionResult executeMoveLTrajectory_(
const CartesianJointTrajectory& trajectory,
const std::function<bool()>& cancellation_requested,
std::uint64_t motion_generation);
bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory);
static CartesianVelocityController::Config toCartesianVelocityControllerConfig_(
const config::CartesianVelocityControllerConfig& config);
@ -140,7 +124,6 @@ private:
std::unordered_set<std::string> joint_set_;
std::string motor_system_id_;
std::shared_ptr<MotorManager> motor_manager_{nullptr};
std::uint64_t motor_control_claim_id_{0};
std::shared_ptr<cmvr::IKSolver> ik_solver_{nullptr};
std::shared_ptr<JointMotionPlanner> joint_planner_{nullptr};
@ -148,10 +131,7 @@ private:
std::unique_ptr<CartesianVelocityController> cartesian_velocity_controller_{nullptr};
mutable std::mutex mutex_;
std::atomic<std::uint64_t> motion_generation_{0};
std::atomic<bool> busy_{false};
std::atomic<bool> powered_on_{false};
mutable std::atomic<std::uint64_t> joint_state_sequence_{0};
double speed_scaling_{1.0};
std::atomic<bool> protective_stopped_{false};
std::atomic<bool> emergency_stopped_{false};

282
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
View File

@ -35,36 +35,6 @@ struct AtomicFlagGuard {
~AtomicFlagGuard() { flag.store(false); }
};
bool cancellationRequested(
const std::function<bool()>& cancellation_requested) noexcept
{
if (!cancellation_requested) {
return false;
}
try {
return cancellation_requested();
} catch (...) {
return true;
}
}
const config::JointLimitsConfig* configuredJointLimits(
const config::ArmKinematicsConfig& kinematics)
{
switch (kinematics.algorithm_case()) {
case config::ArmKinematicsConfig::kPinocchioDlsIkSolver:
return &kinematics.pinocchio_dls_ik_solver()
.joint_limit_policy()
.limits();
case config::ArmKinematicsConfig::kPinocchioQpIkSolver:
return &kinematics.pinocchio_qp_ik_solver()
.joint_limit_policy()
.limits();
default:
return nullptr;
}
}
} // namespace
MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg)
@ -103,39 +73,6 @@ MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg)
model_.manufacturer = "cmvr";
model_.dof = static_cast<std::size_t>(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<std::size_t>(dof_));
for (int index = 0; index < dof_; ++index) {
const auto& source = configured_limits->joints(index);
if (source.joint_name() != joint_names_[static_cast<std::size_t>(index)] ||
!std::isfinite(source.q_lb()) ||
!std::isfinite(source.q_ub()) ||
!std::isfinite(source.qd()) ||
source.q_lb() >= source.q_ub() ||
source.qd() <= 0.0) {
valid_limits = false;
break;
}
JointLimit limit;
limit.lower = source.q_lb();
limit.upper = source.q_ub();
limit.max_velocity = source.qd();
limit.max_acceleration = source.qdd();
model_.joint_limits.push_back(limit);
}
if (!valid_limits) {
model_.joint_limits.clear();
CMVR_LOG(ERROR)
<< "[MotorRobotArm] invalid or misordered custom joint limits: "
<< id_;
}
}
}
MotorRobotArm::~MotorRobotArm()
@ -143,9 +80,6 @@ MotorRobotArm::~MotorRobotArm()
if (cartesian_velocity_controller_) {
cartesian_velocity_controller_->shutdown();
}
if (motor_manager_ && motor_control_claim_id_ != 0U) {
motor_manager_->releaseArmJoints(motor_control_claim_id_);
}
}
bool MotorRobotArm::init()
@ -194,16 +128,6 @@ bool MotorRobotArm::init()
CMVR_LOG(ERROR) << "[MotorRobotArm] failed to configure algorithms: " << id_;
return false;
}
if (motor_control_claim_id_ == 0U) {
std::string claim_error;
if (!motor_manager_->claimArmJoints(
id_, joint_names_, motor_control_claim_id_, &claim_error)) {
CMVR_LOG(ERROR)
<< "[MotorRobotArm] failed to claim direct motor control: "
<< id_ << ", detail=" << claim_error;
return false;
}
}
CMVR_LOG(INFO) << "[MotorRobotArm] (init): Arm '" << id_ << "' init success";
return true;
}
@ -219,8 +143,8 @@ ArmState MotorRobotArm::getRobotState() const
const bool emergency_stopped = emergency_stopped_.load();
ArmState state;
state.connected = motor_manager_ != nullptr;
state.powered_on = powered_on_.load(std::memory_order_acquire);
state.brake_released = state.powered_on && !emergency_stopped;
state.powered_on = true;
state.brake_released = !emergency_stopped;
state.moving = busy();
state.protective_stopped = protective_stopped;
state.emergency_stopped = emergency_stopped;
@ -240,35 +164,15 @@ JointGroupState MotorRobotArm::getJointState() const
state.position.reserve(joint_names_.size());
state.velocity.reserve(joint_names_.size());
state.effort.reserve(joint_names_.size());
bool values_valid = true;
for (const auto& joint_name : joint_names_) {
auto motor = getMotor_(joint_name);
if (!motor) {
values_valid = false;
continue;
}
const double position = motor->getQ();
const double velocity = motor->getQd();
values_valid =
values_valid && std::isfinite(position) && std::isfinite(velocity);
state.position.push_back(position);
state.velocity.push_back(velocity);
state.position.push_back(motor->getQ());
state.velocity.push_back(motor->getQd());
state.effort.push_back(0.0);
}
values_valid =
values_valid && state.position.size() == joint_names_.size() &&
state.velocity.size() == joint_names_.size();
state.sequence =
joint_state_sequence_.fetch_add(1, std::memory_order_relaxed) + 1;
state.sample_monotonic_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::steady_clock::now().time_since_epoch())
.count();
state.position_valid = values_valid;
state.velocity_valid = values_valid;
// MotorRobotArm currently has no verified effort feedback path. The zero
// placeholders above must never be advertised as measured torque.
state.effort_valid = false;
return state;
}
@ -314,15 +218,11 @@ Result MotorRobotArm::torqueOn()
}
}
emergency_stopped_.store(false);
powered_on_.store(true, std::memory_order_release);
return Result::success();
}
Result MotorRobotArm::torqueOff()
{
// Until every joint reports a successful disable, the aggregate powered
// state is unknown and therefore must not satisfy a require_powered gate.
powered_on_.store(false, std::memory_order_release);
for (const auto& joint_name : joint_names_) {
auto motor = getMotor_(joint_name);
if (!motor) {
@ -368,7 +268,6 @@ Result MotorRobotArm::calibrateZeroQ(const std::string& joint_name)
Result MotorRobotArm::emergencyStop()
{
motion_generation_.fetch_add(1, std::memory_order_acq_rel);
if (cartesian_velocity_controller_) {
cartesian_velocity_controller_->shutdown();
}
@ -610,8 +509,6 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
if (const auto stopped = safetyStopResult_("moveJ")) {
return *stopped;
}
const auto motion_generation =
motion_generation_.load(std::memory_order_acquire);
std::string error;
if (!validatePositionCommand_(target, error)) {
return Result::failure(ArmErrorCode::InvalidArgument, error);
@ -623,12 +520,7 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_);
}
BusyGuard busy_guard{busy_};
if (cancellationRequested(options.cancellation_requested)) {
return Result::failure(
ArmErrorCode::CommandRejected,
"[MotorRobotArm] moveJ canceled before planning: " + id_);
}
std::lock_guard<std::mutex> lock(mutex_);
JointTrajectory samples;
if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) {
@ -640,36 +532,19 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
std::vector<std::shared_ptr<AbstractMotor>> motors;
motors.reserve(joint_names_.size());
if (cancellationRequested(options.cancellation_requested)) {
return Result::failure(
ArmErrorCode::CommandRejected,
"[MotorRobotArm] moveJ canceled before dispatch: " + id_);
}
{
std::lock_guard<std::mutex> lock(mutex_);
if (motion_generation_.load(std::memory_order_acquire) !=
motion_generation) {
return Result::failure(
ArmErrorCode::CommandRejected,
"[MotorRobotArm] moveJ canceled before dispatch: " + id_);
for (const auto& joint_name : joint_names_) {
auto motor = getMotor_(joint_name);
if (!motor) {
return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
}
for (const auto& joint_name : joint_names_) {
auto motor = getMotor_(joint_name);
if (!motor) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"motor not found for joint: " + joint_name);
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to set cyclic position mode for joint: " +
joint_name);
}
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
return Result::failure(
ArmErrorCode::CommandFailed,
"failed to set cyclic position mode for joint: " +
joint_name);
}
}
motors.push_back(std::move(motor));
}
motors.push_back(std::move(motor));
}
const auto t0 = std::chrono::steady_clock::now();
@ -687,28 +562,10 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
std::copy_n(sample.velocity.begin(),
std::min(sample.velocity.size(), command_velocity.size()),
command_velocity.begin());
if (cancellationRequested(options.cancellation_requested)) {
return Result::failure(
ArmErrorCode::CommandRejected,
"[MotorRobotArm] moveJ canceled during execution: " + id_);
}
{
// The local generation and one complete joint frame are ordered
// against stopMotion(). External cancellation is intentionally
// evaluated before taking the device mutex because it is caller code.
std::lock_guard<std::mutex> lock(mutex_);
if (motion_generation_.load(std::memory_order_acquire) !=
motion_generation) {
return Result::failure(
ArmErrorCode::CommandRejected,
"[MotorRobotArm] moveJ canceled during execution: " + id_);
}
if (!motor_manager_->commandCyclicPositionsAtomic(
motors, sample.position, command_velocity)) {
return Result::failure(
ArmErrorCode::CommandFailed,
"failed to submit atomic cyclic position command");
}
if (!motor_manager_->commandCyclicPositionsAtomic(
motors, sample.position, command_velocity)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to submit atomic cyclic position command");
}
if (k + 1 < samples.size()) {
const double next_t = samples[k + 1].time_s > 0.0
@ -766,7 +623,6 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
Result MotorRobotArm::stopJ(const double acceleration)
{
motion_generation_.fetch_add(1, std::memory_order_acq_rel);
if (safetyStopRequested_()) {
return Result::success();
}
@ -782,8 +638,6 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
if (const auto stopped = safetyStopResult_("moveL")) {
return *stopped;
}
const auto motion_generation =
motion_generation_.load(std::memory_order_acquire);
if (cartesian_velocity_controller_) {
cartesian_velocity_controller_->shutdown();
}
@ -801,12 +655,6 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
}
BusyGuard busy_guard{busy_};
if (cancellationRequested(options.cancellation_requested)) {
return Result::failure(
ArmErrorCode::CommandRejected,
"[MotorRobotArm] moveL canceled before planning: " + id_);
}
std::vector<double> q_start;
std::vector<double> qd_now;
if (!readArmState_(q_start, qd_now)) {
@ -831,25 +679,13 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
<< ", executable_path_m=" << trajectory.executable_path_length;
}
switch (executeMoveLTrajectory_(
trajectory,
options.cancellation_requested,
motion_generation)) {
case TrajectoryExecutionResult::Completed:
return Result::success();
case TrajectoryExecutionResult::Canceled:
return Result::failure(
ArmErrorCode::CommandRejected,
"[MotorRobotArm] moveL canceled during execution: " + id_);
case TrajectoryExecutionResult::Failed:
if (const auto stopped = safetyStopResult_("moveL", true)) {
return *stopped;
}
return Result::failure(
ArmErrorCode::CommandFailed, "moveL execution failed");
if (executeMoveLTrajectory_(trajectory)) {
return Result::success();
}
return Result::failure(ArmErrorCode::CommandFailed,
"moveL execution failed");
if (const auto stopped = safetyStopResult_("moveL", true)) {
return *stopped;
}
return Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
}
Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
@ -879,26 +715,16 @@ Result MotorRobotArm::stopL(const std::optional<double> acceleration)
Result MotorRobotArm::stopMotion()
{
// Revoke position trajectories before stopping the velocity worker. The
// controller fences its old worker and sends zero before joining it.
motion_generation_.fetch_add(1, std::memory_order_acq_rel);
if (cartesian_velocity_controller_) {
cartesian_velocity_controller_->shutdown();
}
JointVelocityCommand zero;
zero.velocity.assign(joint_names_.size(), 0.0);
return speedJ(zero, 0.0, 0.0);
stopL(0.0);
return stopJ(0.0);
}
Result MotorRobotArm::shutdown()
{
motion_generation_.fetch_add(1, std::memory_order_acq_rel);
if (cartesian_velocity_controller_) {
cartesian_velocity_controller_->shutdown();
}
JointVelocityCommand zero;
zero.velocity.assign(joint_names_.size(), 0.0);
return speedJ(zero, 0.0, 0.0);
return stopJ(0.0);
}
Result MotorRobotArm::startServoMode(const ServoOptions& options)
@ -1185,44 +1011,29 @@ bool MotorRobotArm::configureAlgorithms_()
return true;
}
MotorRobotArm::TrajectoryExecutionResult
MotorRobotArm::executeMoveLTrajectory_(
const CartesianJointTrajectory& trajectory,
const std::function<bool()>& cancellation_requested,
const std::uint64_t motion_generation)
bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory)
{
if (trajectory.position.empty() ||
trajectory.velocity.size() != trajectory.position.size() ||
trajectory.time.size() != trajectory.position.size()) {
return TrajectoryExecutionResult::Failed;
return false;
}
if (trajectory.position.size() == 1) {
return cancellationRequested(cancellation_requested) ||
motion_generation_.load(std::memory_order_acquire) !=
motion_generation
? TrajectoryExecutionResult::Canceled
: TrajectoryExecutionResult::Completed;
return true;
}
std::vector<std::shared_ptr<AbstractMotor>> motors;
motors.reserve(joint_names_.size());
if (cancellationRequested(cancellation_requested)) {
return TrajectoryExecutionResult::Canceled;
}
{
std::lock_guard<std::mutex> lock(mutex_);
if (motion_generation_.load(std::memory_order_acquire) !=
motion_generation) {
return TrajectoryExecutionResult::Canceled;
}
for (const auto& joint_name : joint_names_) {
auto motor = getMotor_(joint_name);
if (!motor) {
return TrajectoryExecutionResult::Failed;
return false;
}
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
return TrajectoryExecutionResult::Failed;
return false;
}
}
motors.push_back(std::move(motor));
@ -1232,40 +1043,23 @@ MotorRobotArm::executeMoveLTrajectory_(
auto next_deadline = std::chrono::steady_clock::now();
for (std::size_t i = 1; i < trajectory.position.size(); ++i) {
if (safetyStopRequested_()) {
return TrajectoryExecutionResult::Failed;
return false;
}
const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]);
const auto& position = trajectory.position[i];
const auto& velocity = trajectory.velocity[i];
if (position.size() != motors.size() || velocity.size() != motors.size()) {
return TrajectoryExecutionResult::Failed;
return false;
}
if (cancellationRequested(cancellation_requested)) {
return TrajectoryExecutionResult::Canceled;
}
{
// Keep the local stop decision and the complete joint frame in the
// same critical section as stopMotion()/stopJ().
std::lock_guard<std::mutex> lock(mutex_);
if (motion_generation_.load(std::memory_order_acquire) !=
motion_generation) {
return TrajectoryExecutionResult::Canceled;
}
if (!motor_manager_->commandCyclicPositionsAtomic(
motors, position, velocity)) {
return TrajectoryExecutionResult::Failed;
}
if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) {
return false;
}
next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(dt_segment));
std::this_thread::sleep_until(next_deadline);
}
return cancellationRequested(cancellation_requested) ||
motion_generation_.load(std::memory_order_acquire) !=
motion_generation
? TrajectoryExecutionResult::Canceled
: TrajectoryExecutionResult::Completed;
return true;
}
CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityControllerConfig_(

375
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp
View File

@ -2,17 +2,12 @@
#include <algorithm>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <filesystem>
#include <functional>
#include <future>
#include <iostream>
#include <limits>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <unordered_set>
@ -104,142 +99,6 @@ struct ArmMujocoConfigCase {
const char* config_file;
};
class BlockingCartesianMotionPlanner final : public CartesianMotionPlanner {
public:
bool configureSpeedL(const config::SpeedLPlannerConfig&, std::size_t) override
{
return true;
}
bool configureMoveL(const config::MoveLPlannerConfig&) override
{
return true;
}
bool planMoveL(const CartesianPose&,
const std::vector<double>&,
const std::vector<double>&,
double,
double,
double,
FrameType,
CartesianJointTrajectory&) override
{
return false;
}
bool speedLStep(const CartesianVelocity&,
double,
const std::vector<double>& q_measured,
const std::vector<double>&,
std::vector<double>& qd_command,
FrameType) override
{
std::unique_lock lock(mutex_);
if (step_count_++ == 0) {
first_step_entered_ = true;
condition_.notify_all();
condition_.wait(lock, [&] { return release_first_step_; });
}
qd_command.assign(q_measured.size(), 0.4);
return true;
}
bool updateSpeedLAcceleration(double) override
{
return true;
}
CartesianVelocity getSpeedLCommandTwistBase() const override
{
return {};
}
bool waitForFirstStep(const std::chrono::milliseconds timeout)
{
std::unique_lock lock(mutex_);
return condition_.wait_for(
lock, timeout, [&] { return first_step_entered_; });
}
void releaseFirstStep()
{
{
std::lock_guard lock(mutex_);
release_first_step_ = true;
}
condition_.notify_all();
}
private:
mutable std::mutex mutex_;
std::condition_variable condition_;
std::size_t step_count_{0};
bool first_step_entered_{false};
bool release_first_step_{false};
};
class VelocityCommandRecorder {
public:
Result record(const JointVelocityCommand& velocity, double)
{
{
std::lock_guard lock(mutex_);
commands_.push_back(velocity.velocity);
}
condition_.notify_all();
return Result::success();
}
bool waitForZero(const std::chrono::milliseconds timeout)
{
std::unique_lock lock(mutex_);
return condition_.wait_for(lock, timeout, [&] {
return std::any_of(commands_.begin(), commands_.end(), isZero_);
});
}
bool waitForNonZeroAfter(const std::size_t index,
const std::chrono::milliseconds timeout)
{
std::unique_lock lock(mutex_);
return condition_.wait_for(lock, timeout, [&] {
return index < commands_.size() &&
std::any_of(commands_.begin() + index,
commands_.end(),
[](const auto& command) {
return !isZero_(command);
});
});
}
std::size_t size() const
{
std::lock_guard lock(mutex_);
return commands_.size();
}
bool allZeroFrom(const std::size_t index) const
{
std::lock_guard lock(mutex_);
return index <= commands_.size() &&
std::all_of(commands_.begin() + index,
commands_.end(), isZero_);
}
private:
static bool isZero_(const std::vector<double>& command)
{
return std::all_of(command.begin(), command.end(), [](const double value) {
return std::abs(value) < 1e-12;
});
}
mutable std::mutex mutex_;
std::condition_variable condition_;
std::vector<std::vector<double>> commands_;
};
void PrintTo(const ArmMujocoConfigCase& value, std::ostream* os)
{
*os << value.name << " (" << value.config_file << ")";
@ -455,240 +314,6 @@ TEST_P(MotorRobotArmMujocoTest, MoveL)
EXPECT_LT(outcome.move_l_error, 0.04);
}
TEST_P(MotorRobotArmMujocoTest, StopMotionDoesNotWaitForMoveJCancellationCallback)
{
MotionOptions options;
options.velocity = 0.4;
options.acceleration = 2.0;
std::mutex cancellation_mutex;
std::condition_variable cancellation_condition;
int cancellation_checks = 0;
bool release_dispatch_check = false;
options.cancellation_requested = [&] {
std::unique_lock lock(cancellation_mutex);
++cancellation_checks;
cancellation_condition.notify_all();
if (cancellation_checks == 3) {
cancellation_condition.wait(lock, [&] {
return release_dispatch_check;
});
}
return false;
};
std::vector<double> target(kDof, 0.0);
target[0] = 0.2;
auto motion = std::async(std::launch::async, [&] {
return arm_->moveJ(JointPositionCommand{target}, options);
});
bool cancellation_blocked = false;
{
std::unique_lock lock(cancellation_mutex);
cancellation_blocked = cancellation_condition.wait_for(
lock, std::chrono::seconds(2), [&] {
return cancellation_checks >= 3;
});
}
auto stop = std::async(std::launch::async, [&] {
return arm_->stopMotion();
});
EXPECT_TRUE(cancellation_blocked);
EXPECT_EQ(stop.wait_for(std::chrono::seconds(1)),
std::future_status::ready);
{
std::lock_guard lock(cancellation_mutex);
release_dispatch_check = true;
}
cancellation_condition.notify_all();
const auto motion_result = motion.get();
const auto stop_result = stop.get();
EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected)
<< motion_result.message;
EXPECT_TRUE(stop_result.ok()) << stop_result.message;
}
TEST_P(MotorRobotArmMujocoTest, StopMotionDoesNotWaitForMoveLCancellationCallback)
{
const std::vector<double> initial{
0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0};
MotionOptions joint_options;
joint_options.velocity = 2.8;
joint_options.acceleration = 20.0;
const auto setup = arm_->moveJ(
JointPositionCommand{initial}, joint_options);
ASSERT_TRUE(setup.ok()) << setup.message;
CartesianPose target = arm_->getTcpPose();
target.x += 0.05;
MotionOptions options;
options.velocity = 0.4;
options.acceleration = 5.0;
options.jerk = 20.0;
std::mutex cancellation_mutex;
std::condition_variable cancellation_condition;
int cancellation_checks = 0;
bool release_dispatch_check = false;
options.cancellation_requested = [&] {
std::unique_lock lock(cancellation_mutex);
++cancellation_checks;
cancellation_condition.notify_all();
if (cancellation_checks == 3) {
cancellation_condition.wait(lock, [&] {
return release_dispatch_check;
});
}
return false;
};
auto motion = std::async(std::launch::async, [&] {
return arm_->moveL(target, options, FrameType::Base);
});
bool cancellation_blocked = false;
{
std::unique_lock lock(cancellation_mutex);
cancellation_blocked = cancellation_condition.wait_for(
lock, std::chrono::seconds(2), [&] {
return cancellation_checks >= 3;
});
}
auto stop = std::async(std::launch::async, [&] {
return arm_->stopMotion();
});
EXPECT_TRUE(cancellation_blocked);
EXPECT_EQ(stop.wait_for(std::chrono::seconds(1)),
std::future_status::ready);
{
std::lock_guard lock(cancellation_mutex);
release_dispatch_check = true;
}
cancellation_condition.notify_all();
const auto motion_result = motion.get();
const auto stop_result = stop.get();
EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected)
<< motion_result.message;
EXPECT_TRUE(stop_result.ok()) << stop_result.message;
}
TEST_P(MotorRobotArmMujocoTest, StopMotionCancelsMoveJWithoutExternalCallback)
{
MotionOptions options;
options.velocity = 0.1;
options.acceleration = 0.5;
std::vector<double> target(kDof, 0.0);
target[0] = 0.4;
auto motion = std::async(std::launch::async, [&] {
return arm_->moveJ(JointPositionCommand{target}, options);
});
waitFor([&] { return arm_->busy(); }, std::chrono::seconds(1));
const auto stop_result = arm_->stopMotion();
const auto motion_result = motion.get();
EXPECT_TRUE(stop_result.ok()) << stop_result.message;
EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected)
<< motion_result.message;
EXPECT_FALSE(arm_->busy());
}
TEST_P(MotorRobotArmMujocoTest, StopMotionCancelsMoveLWithoutExternalCallback)
{
const std::vector<double> initial{
0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0.0, 0.0};
MotionOptions joint_options;
joint_options.velocity = 2.8;
joint_options.acceleration = 20.0;
const auto setup = arm_->moveJ(
JointPositionCommand{initial}, joint_options);
ASSERT_TRUE(setup.ok()) << setup.message;
CartesianPose target = arm_->getTcpPose();
target.x += 0.08;
MotionOptions options;
options.velocity = 0.1;
options.acceleration = 1.0;
options.jerk = 5.0;
auto motion = std::async(std::launch::async, [&] {
return arm_->moveL(target, options, FrameType::Base);
});
waitFor([&] { return arm_->busy(); }, std::chrono::seconds(1));
const auto stop_result = arm_->stopMotion();
const auto motion_result = motion.get();
EXPECT_TRUE(stop_result.ok()) << stop_result.message;
EXPECT_EQ(motion_result.code, ArmErrorCode::CommandRejected)
<< motion_result.message;
EXPECT_FALSE(arm_->busy());
}
TEST(CartesianVelocityControllerTest,
ShutdownFencesStaleWriteAndLaterSpeedLRestartsWorker)
{
constexpr std::size_t dof = 2;
auto planner = std::make_shared<BlockingCartesianMotionPlanner>();
VelocityCommandRecorder recorder;
CartesianVelocityController controller(
CartesianVelocityController::Config{},
planner,
dof,
[](std::vector<double>& q, std::vector<double>& qd) {
q.assign(dof, 0.0);
qd.assign(dof, 0.0);
return true;
},
[&](const JointVelocityCommand& command, const double acceleration) {
return recorder.record(command, acceleration);
});
CartesianVelocity velocity;
velocity.vx = 0.1;
const auto first = controller.speedL(
velocity, 0.5, 0.0, FrameType::Base);
ASSERT_TRUE(first.ok()) << first.message;
const bool first_step_entered =
planner->waitForFirstStep(std::chrono::seconds(1));
if (!first_step_entered) {
planner->releaseFirstStep();
controller.shutdown();
FAIL() << "velocity worker did not enter the blocking planner step";
}
auto shutdown = std::async(std::launch::async, [&] {
controller.shutdown();
});
EXPECT_TRUE(recorder.waitForZero(std::chrono::seconds(1)));
EXPECT_EQ(shutdown.wait_for(std::chrono::milliseconds(20)),
std::future_status::timeout);
const auto zero_index = recorder.size();
planner->releaseFirstStep();
EXPECT_EQ(shutdown.wait_for(std::chrono::seconds(1)),
std::future_status::ready);
shutdown.get();
EXPECT_TRUE(recorder.allZeroFrom(zero_index));
EXPECT_FALSE(controller.busy());
const auto restart_index = recorder.size();
const auto restarted = controller.speedL(
velocity, 0.5, 0.0, FrameType::Base);
EXPECT_TRUE(restarted.ok()) << restarted.message;
EXPECT_TRUE(recorder.waitForNonZeroAfter(
restart_index, std::chrono::seconds(1)));
controller.shutdown();
EXPECT_FALSE(controller.busy());
}
TEST_P(MotorRobotArmMujocoTest, SpeedL)
{
MotorRobotArm& arm = *arm_;

59
cmvr-es/devices/arm/robot_arm.h
View File

@ -2,7 +2,6 @@
#define CMVR_ES_ROBOT_ARM_H
#include <cstddef>
#include <functional>
#include <memory>
#include <optional>
#include <string>
@ -29,44 +28,7 @@ public:
virtual SafetyMode getSafetyMode() const = 0;
virtual ControlMode getControlMode() const = 0;
// Queued actions require synchronous completion, cooperative cancellation
// at the final device-submission boundary, and a bounded typed Stop which
// returns success only after controller idle is confirmed. Backends must
// opt in only after all of these semantics have been validated.
virtual bool supportsActionQueueMotion() const noexcept { return false; }
// ArmTeleop requires an explicitly reviewed group-servo implementation.
// Existing and vendor arms remain unavailable until their implementations
// override this capability after timing and partial-write validation.
virtual bool supportsTeleopGroupServo() const noexcept { return false; }
virtual JointEffortSource jointEffortSource() const noexcept
{
return JointEffortSource::Unspecified;
}
virtual Result torqueOn() = 0;
// Long-running startup implementations may cooperatively observe loss of
// their control lease. Backends which have not adopted cancellation retain
// the legacy behavior, while still rejecting an already-cancelled request
// before entering the vendor API.
virtual Result torqueOn(
const std::function<bool()>& cancellation_requested)
{
if (cancellation_requested) {
try {
if (cancellation_requested()) {
return Result::failure(
ArmErrorCode::CommandRejected,
"torqueOn cancelled before execution");
}
} catch (...) {
return Result::failure(
ArmErrorCode::CommandRejected,
"torqueOn cancellation check failed");
}
}
return torqueOn();
}
virtual Result torqueOff() = 0;
virtual Result calibrateZeroQ(const std::string& joint_name) = 0;
@ -114,27 +76,6 @@ public:
FrameType frame = FrameType::Base) = 0;
virtual Result stopServoMode() = 0;
// Torque streaming is optional. Backends which do not provide an atomic
// group torque port retain source compatibility and fail explicitly.
virtual Result startTorqueMode(const TorqueServoOptions&)
{
return Result::failure(
ArmErrorCode::UnsupportedCommand,
"torque servo mode is unsupported by this RobotArm");
}
virtual Result servoTorque(const JointTorqueCommand&)
{
return Result::failure(
ArmErrorCode::UnsupportedCommand,
"torque servo command is unsupported by this RobotArm");
}
virtual Result stopTorqueMode()
{
return Result::failure(
ArmErrorCode::UnsupportedCommand,
"torque servo mode is unsupported by this RobotArm");
}
virtual Result connect(const std::string& ip, int port) = 0;
virtual Result disconnect() = 0;
virtual bool isConnected() const = 0;

4
cmvr-es/devices/arm/robot_arm_factory.h
View File

@ -9,7 +9,6 @@
#include "devices/arm/aubo_arm/aubo_arm.h"
#include "devices/arm/huayan_arm/huayan_arm.h"
#include "devices/arm/motor_robot_arm/include/motor_robot_arm.h"
#include "devices/arm/ume_robot_arm/include/ume_robot_arm.h"
namespace cmvr::device {
@ -37,9 +36,6 @@ public:
return nullptr;
}
case config::RobotArmConfig::kUme:
return std::make_shared<UmeRobotArm>(cfg);
case config::RobotArmConfig::BACKEND_NOT_SET:
default:
{

92
cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt
View File

@ -1,92 +0,0 @@
add_library(ume_robot_arm SHARED
src/damiao_mit_codec.cpp
src/damiao_can_fd_chain.cpp
src/ume_robot_arm.cpp
)
target_include_directories(ume_robot_arm PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)
target_link_libraries(ume_robot_arm
PUBLIC
cmvr_es::device::canbus
cmvr_es::ik_solver
cmvr_es::common
PRIVATE
cmvr_es::proto
cmvr_es::logging
pthread
)
add_library(cmvr_es::device::ume_robot_arm ALIAS ume_robot_arm)
install(TARGETS ume_robot_arm LIBRARY DESTINATION lib)
if(BUILD_TESTING)
add_executable(damiao_mit_codec_test
tests/damiao_mit_codec_test.cpp
)
target_link_libraries(damiao_mit_codec_test
PRIVATE
cmvr_es::device::ume_robot_arm
gtest
gtest_main
pthread
)
add_test(
NAME damiao_mit_codec_test
COMMAND damiao_mit_codec_test
)
set(_ume_robot_arm_test_environment
"LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
)
if(CMVR_TEST_SYSTEM_LIBSTDCXX)
list(APPEND _ume_robot_arm_test_environment
"LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
endif()
set_tests_properties(damiao_mit_codec_test PROPERTIES
TIMEOUT 10
ENVIRONMENT "${_ume_robot_arm_test_environment}"
)
add_executable(damiao_can_fd_chain_test
tests/damiao_can_fd_chain_test.cpp
)
target_link_libraries(damiao_can_fd_chain_test
PRIVATE
cmvr_es::device::ume_robot_arm
gtest
gtest_main
pthread
)
add_test(
NAME damiao_can_fd_chain_test
COMMAND damiao_can_fd_chain_test
)
set_tests_properties(damiao_can_fd_chain_test PROPERTIES
TIMEOUT 10
ENVIRONMENT "${_ume_robot_arm_test_environment}"
)
add_executable(ume_robot_arm_test
tests/ume_robot_arm_test.cpp
)
target_link_libraries(ume_robot_arm_test
PRIVATE
cmvr_es::device::ume_robot_arm
gtest
gtest_main
pthread
)
target_compile_definitions(ume_robot_arm_test PRIVATE
CMVR_UME_ARM_CONFIG_PATH="${PROJECT_SOURCE_DIR}/cmvr-es/config/devices/arm/ume_arms.pb.txt"
)
add_test(
NAME ume_robot_arm_test
COMMAND ume_robot_arm_test
)
set_tests_properties(ume_robot_arm_test PROPERTIES
TIMEOUT 10
ENVIRONMENT "${_ume_robot_arm_test_environment}"
)
endif()

145
cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h
View File

@ -1,145 +0,0 @@
#ifndef CMVR_ES_DAMIAO_CAN_FD_CHAIN_H
#define CMVR_ES_DAMIAO_CAN_FD_CHAIN_H
#include <atomic>
#include <chrono>
#include <cstddef>
#include <cstdint>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
#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<AbstractCanbus> bus,
std::vector<DamiaoJointSpec> 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<DamiaoJointSpec>& 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<CanFrame>& frames,
std::chrono::steady_clock::time_point deadline) noexcept;
bool sendFramesBestEffort_(
const std::vector<CanFrame>& 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<AbstractCanbus> bus_;
std::vector<DamiaoJointSpec> joints_;
DamiaoChainOptions options_;
std::vector<CanFrame> tx_frames_;
std::vector<CanFrame> rx_frames_;
std::vector<DamiaoJointFeedback> feedback_scratch_;
std::vector<bool> feedback_seen_;
mutable std::mutex io_mutex_;
mutable std::mutex status_mutex_;
std::atomic<DamiaoChainState> state_{DamiaoChainState::Closed};
DamiaoChainStatistics statistics_;
std::string last_error_;
};
} // namespace cmvr::device
#endif // CMVR_ES_DAMIAO_CAN_FD_CHAIN_H

135
cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h
View File

@ -1,135 +0,0 @@
#ifndef CMVR_ES_DAMIAO_MIT_CODEC_H
#define CMVR_ES_DAMIAO_MIT_CODEC_H
#include <cstdint>
#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

189
cmvr-es/devices/arm/ume_robot_arm/include/ume_robot_arm.h
View File

@ -1,189 +0,0 @@
#ifndef CMVR_ES_UME_ROBOT_ARM_H
#define CMVR_ES_UME_ROBOT_ARM_H
#include <array>
#include <atomic>
#include <cstddef>
#include <cstdint>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#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<double, kDof> q{};
std::array<double, kDof> dq{};
std::array<double, kDof> tau_measured{};
std::array<std::int64_t, kDof> 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<AbstractCanbus> canbus);
~UmeRobotArm() override;
std::string typeName() const override { return "UmeRobotArm"; }
bool init() override;
bool start() override;
bool stop() override;
DeviceHealthSnapshot healthSnapshot() override;
RobotModel getRobotModel() const override { return model_; }
std::size_t getDof() const override { return UmeArmSample::kDof; }
ArmState getRobotState() const override;
JointGroupState getJointState() const override;
Result readSample(UmeArmSample& sample) const;
CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
RobotMode getRobotMode() const override;
SafetyMode getSafetyMode() const override;
ControlMode getControlMode() const override;
Result torqueOn() override;
Result torqueOff() override;
Result calibrateZeroQ(const std::string& joint_name) override;
Result emergencyStop() override;
Result protectiveStop() override;
Result recoverProtectiveStop(
const JointTrajectory&,
const MotionOptions&) override
{
return Result::failure(
ArmErrorCode::UnsupportedCommand,
"protective recovery is not implemented for UmeRobotArm");
}
Result setSpeedScaling(double scaling) override;
double getSpeedScaling() const override { return 1.0; }
bool isProtectiveStopped() const override
{
return protective_stopped_.load();
}
bool isEmergencyStopped() const override
{
return emergency_stopped_.load();
}
bool isFault() const override { return fault_latched_.load(); }
Result moveJ(const JointPositionCommand& target,
const MotionOptions& options) override;
Result speedJ(const JointVelocityCommand& velocity,
double acceleration,
double duration) override;
Result stopJ(double acceleration) override;
Result moveL(const CartesianPose& target,
const MotionOptions& options,
FrameType frame = FrameType::Base) override;
Result speedL(const CartesianVelocity& velocity,
double acceleration,
double duration,
FrameType frame = FrameType::Base) override;
Result stopL(std::optional<double> 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<double> ik(const std::string& base_link,
const std::string& ee_link,
const CartesianPose& pose) override;
std::shared_ptr<cmvr::IKSolver> 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<AbstractCanbus> canbus_;
std::unique_ptr<DamiaoCanFdChain> chain_;
std::vector<DamiaoJointSpec> joint_specs_;
RobotModel model_;
std::shared_ptr<cmvr::IKSolver> 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<double, UmeArmSample::kDof> latest_torque_command_{};
UmeArmSample latest_sample_;
std::string last_error_;
std::atomic<bool> initialized_{false};
std::atomic<bool> running_{false};
std::atomic<bool> torque_mode_{false};
std::atomic<bool> powered_on_{false};
std::atomic<bool> fault_latched_{false};
std::atomic<bool> protective_stopped_{false};
std::atomic<bool> emergency_stopped_{false};
std::atomic<bool> command_ready_{false};
std::atomic<std::uint64_t> command_sequence_{0};
std::atomic<std::int64_t> command_time_ns_{0};
std::atomic<std::int64_t> loop_period_ns_{1250000};
std::atomic<std::uint32_t> 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

705
cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp
View File

@ -1,705 +0,0 @@
#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h"
#include <algorithm>
#include <cmath>
#include <limits>
#include <unordered_set>
#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<AbstractCanbus> bus,
std::vector<DamiaoJointSpec> 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<std::string> names;
std::unordered_set<std::uint32_t> command_ids;
std::unordered_set<std::uint32_t> feedback_ids;
std::unordered_set<std::uint32_t> 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<std::uint16_t>(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<CanFrame>& frames,
const std::chrono::steady_clock::time_point deadline) noexcept
{
if (frames.empty() ||
frames.size() > static_cast<std::size_t>(
std::numeric_limits<int32_t>::max())) {
return false;
}
int32_t count = static_cast<int32_t>(frames.size());
return bus_->sendUntil(frames, &count, deadline) ==
msgs::ErrorCode::OK &&
count == static_cast<int32_t>(frames.size());
}
bool DamiaoCanFdChain::sendFramesBestEffort_(
const std::vector<CanFrame>& frames) noexcept
{
if (frames.empty() ||
frames.size() > static_cast<std::size_t>(
std::numeric_limits<int32_t>::max())) {
return false;
}
int32_t count = static_cast<int32_t>(frames.size());
return bus_->send(frames, &count) == msgs::ErrorCode::OK &&
count == static_cast<int32_t>(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

254
cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp
View File

@ -1,254 +0,0 @@
#include "arm/ume_robot_arm/include/damiao_mit_codec.h"
#include <algorithm>
#include <cmath>
#include <cstring>
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<std::uint16_t>(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<double>(std::uint32_t{1} << bits);
return (static_cast<double>(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<std::uint8_t>((q >> 8U) & 0xFFU);
frame.data[1] = static_cast<std::uint8_t>(q & 0xFFU);
frame.data[2] = static_cast<std::uint8_t>((dq >> 4U) & 0xFFU);
frame.data[3] = static_cast<std::uint8_t>(
((dq & 0xFU) << 4U) | ((kp >> 8U) & 0xFU));
frame.data[4] = static_cast<std::uint8_t>(kp & 0xFFU);
frame.data[5] = static_cast<std::uint8_t>((kd >> 4U) & 0xFFU);
frame.data[6] = static_cast<std::uint8_t>(
((kd & 0xFU) << 4U) | ((tau >> 8U) & 0xFU));
frame.data[7] = static_cast<std::uint8_t>(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<std::uint16_t>(
(static_cast<std::uint16_t>(frame.data[1]) << 8U) |
frame.data[2]);
const std::uint16_t dq =
static_cast<std::uint16_t>(
(static_cast<std::uint16_t>(frame.data[3]) << 4U) |
(frame.data[4] >> 4U));
const std::uint16_t tau =
static_cast<std::uint16_t>(
((static_cast<std::uint16_t>(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

1035
cmvr-es/devices/arm/ume_robot_arm/src/ume_robot_arm.cpp
View File

File diff suppressed because it is too large Load Diff

407
cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp
View File

@ -1,407 +0,0 @@
#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h"
#include <chrono>
#include <deque>
#include <memory>
#include <string>
#include <thread>
#include <utility>
#include <vector>
#include <gtest/gtest.h>
#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<CanFrame>& frames,
int32_t* frame_num) override
{
if (!started || !frame_num ||
*frame_num != static_cast<int32_t>(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<CanFrame>* 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<CanFrame> 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<std::vector<CanFrame>> sent_batches;
std::deque<CanFrame> replies;
std::deque<std::vector<CanFrame>> 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<std::uint8_t>((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<std::vector<CanFrame>>& 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<FakeCanbus>();
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<FakeCanbus>();
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<FakeCanbus>();
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<FakeCanbus>();
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<FakeCanbus>();
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<FakeCanbus>();
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<FakeCanbus>();
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<FakeCanbus>();
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<FakeCanbus>();
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<FakeCanbus>();
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<FakeCanbus>();
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

150
cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp
View File

@ -1,150 +0,0 @@
#include "arm/ume_robot_arm/include/damiao_mit_codec.h"
#include <array>
#include <cmath>
#include <limits>
#include <gtest/gtest.h>
namespace cmvr::device {
namespace {
void expectPayload(const CanFrame& frame,
const std::array<std::uint8_t, 8>& 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<double>::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

338
cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp
View File

@ -1,338 +0,0 @@
#include "arm/ume_robot_arm/include/ume_robot_arm.h"
#include <chrono>
#include <cstdint>
#include <deque>
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include <gtest/gtest.h>
#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<CanFrame>& frames,
int32_t* frame_num) override
{
std::lock_guard lock(mutex);
if (!started || !frame_num ||
*frame_num != static_cast<int32_t>(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::nanoseconds>(
std::chrono::steady_clock::now().time_since_epoch())
.count();
reply.data[0] = static_cast<std::uint8_t>(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<CanFrame>* 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<CanFrame> replies;
std::vector<std::vector<CanFrame>> 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<bool()>& 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<LoopbackDamiaoBus>();
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<LoopbackDamiaoBus>();
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<LoopbackDamiaoBus>();
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<LoopbackDamiaoBus>();
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<LoopbackDamiaoBus>();
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<LoopbackDamiaoBus>();
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<LoopbackDamiaoBus>();
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

139
cmvr-es/devices/biohead/abstract_biohead.h
View File

@ -1,11 +1,6 @@
#ifndef ABSTRACT_BIOHEAD_H
#define ABSTRACT_BIOHEAD_H
#pragma once
#include <cstdint>
#include <mutex>
#include <utility>
#include "../abstract_device.h"
namespace cmvr::device {
@ -63,8 +58,6 @@ namespace cmvr::device {
// 抽象头部类
class AbstractBiohead : public AbstractDevice {
public:
using OperationalToken = std::uint64_t;
AbstractBiohead() = default;
~AbstractBiohead() override = default;
@ -83,140 +76,20 @@ namespace cmvr::device {
virtual void expressionSadness() {};
virtual void expressionYawn() {};
// Capture under the process-wide StopAll admission gate. Commands
// from an older generation are rejected after operational stop.
OperationalToken beginOperationalActivity() const noexcept
{
std::lock_guard lock(operational_mutex_);
return operational_generation_;
}
virtual bool setExpressionPoseIfCurrent(
OperationalToken token,
FacialExpressionState& expression_state,
double vel = 0.5,
double acc = 0.1)
{
return runIfOperationalActivityCurrent_(token, [&] {
setExpressionPose(expression_state, vel, acc);
});
}
virtual bool streamFacialPoseIfCurrent(
OperationalToken token,
FacialExpressionState& expression_state,
double vel,
double acc)
{
return runIfOperationalActivityCurrent_(token, [&] {
streamFacialPose(expression_state, vel, acc);
});
}
virtual bool speakStartIfCurrent(OperationalToken token)
{
return runIfOperationalActivityCurrent_(token, [&] {
speakstart();
});
}
virtual bool expressionHappyIfCurrent(OperationalToken token)
{
return runIfOperationalActivityCurrent_(token, [&] {
expressionHappy();
});
}
virtual bool expressionSurprisedIfCurrent(OperationalToken token)
{
return runIfOperationalActivityCurrent_(token, [&] {
expressionSurprised();
});
}
virtual bool expressionTiredIfCurrent(OperationalToken token)
{
return runIfOperationalActivityCurrent_(token, [&] {
expressionTired();
});
}
virtual bool expressionAngryIfCurrent(OperationalToken token)
{
return runIfOperationalActivityCurrent_(token, [&] {
expressionAngry();
});
}
virtual bool expressionSadnessIfCurrent(OperationalToken token)
{
return runIfOperationalActivityCurrent_(token, [&] {
expressionSadness();
});
}
virtual bool expressionYawnIfCurrent(OperationalToken token)
{
return runIfOperationalActivityCurrent_(token, [&] {
expressionYawn();
});
}
// Stops expression motion and speaking without closing the device.
// True confirms that old activity was fenced and the hold completed.
virtual bool stopOperationalActivity()
{
invalidateOperationalActivities_();
speakstop();
(void)runOperationalStop_([&] { eStop(); });
return false;
}
FacialExpressionState expression_state_;
protected:
template <typename Operation>
bool runIfOperationalActivityCurrent_(
const OperationalToken token,
Operation&& operation)
{
std::lock_guard lock(operational_mutex_);
if (token == 0U || token != operational_generation_) {
return false;
}
std::forward<Operation>(operation)();
return true;
}
std::atomic<bool> emergency_stop_requested = false;
template <typename Operation>
bool runOperationalStop_(Operation&& operation)
{
std::lock_guard lock(operational_mutex_);
std::forward<Operation>(operation)();
return true;
}
bool operationalActivityCurrent_(
const OperationalToken token) const noexcept
{
std::lock_guard lock(operational_mutex_);
return token != 0U && token == operational_generation_;
}
void invalidateOperationalActivities_() noexcept
{
std::lock_guard lock(operational_mutex_);
++operational_generation_;
if (operational_generation_ == 0U) {
++operational_generation_;
}
}
private:
mutable std::mutex operational_mutex_;
OperationalToken operational_generation_{1U};
};
} // namespace cmvr::device
#endif // ABSTRACT_BIOHEAD_H

45
cmvr-es/devices/biohead/biohead_esp32/include/biohead_esp32.h
View File

@ -4,12 +4,10 @@
#include "../../abstract_biohead.h"
#include "../../../../hardware/include/esp32_serial_port.h"
#include "cmvr/config/biohead_config/biohead_config.pb.h"
#include <atomic>
#include <vector>
#include <string>
#include <memory>
#include <mutex>
#include <condition_variable>
namespace cmvr::device {
@ -21,7 +19,7 @@ namespace cmvr::device {
class BioHeadRobot : public AbstractBiohead {
public:
explicit BioHeadRobot(const config::BioHeadRobotConfig &config);
~BioHeadRobot() override;
~BioHeadRobot() override = default;
std::string typeName() const override { return "BioHeadRobot"; }
bool init() override;
@ -32,25 +30,7 @@ namespace cmvr::device {
void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) override;
void speakstart() override;
void speakstop() override;
bool stopOperationalActivity() override;
bool setExpressionPoseIfCurrent(
OperationalToken token,
FacialExpressionState& expression_state,
double vel = 0.5,
double acc = 0.1) override;
bool streamFacialPoseIfCurrent(
OperationalToken token,
FacialExpressionState& expression_state,
double vel,
double acc) override;
bool speakStartIfCurrent(OperationalToken token) override;
bool expressionHappyIfCurrent(OperationalToken token) override;
bool expressionSurprisedIfCurrent(OperationalToken token) override;
bool expressionTiredIfCurrent(OperationalToken token) override;
bool expressionAngryIfCurrent(OperationalToken token) override;
bool expressionSadnessIfCurrent(OperationalToken token) override;
bool expressionYawnIfCurrent(OperationalToken token) override;
void speakthread();
void expressionHappy()override;
void expressionSurprised()override;
@ -63,23 +43,11 @@ namespace cmvr::device {
private:
// 内部方法
void parseConfig(const config::BioHeadRobotConfig &config);
bool sendServoCommands(
const std::vector<double>& targets,
uint16_t duration_ms,
bool force = false);
bool sendRawIfCurrent(
OperationalToken token,
const std::vector<uint8_t>& raw_data);
void sendServoCommands( const std::vector<double>& targets, uint16_t duration_ms);
uint16_t angleToRaw(double angle);
double normalizeToAngle(double normalized, size_t index);
bool sendExpression(
OperationalToken token,
const std::vector<double>& device_64_angles,
const std::vector<double>& device_65_angles,
int step_ms);
bool startSpeaking(OperationalToken token);
void speakthread(OperationalToken token);
void sendExpression(const std::vector<double>& device_64_angles, const std::vector<double>& device_65_angles, int step_ms);
@ -101,9 +69,8 @@ namespace cmvr::device {
std::shared_ptr<std::thread> speak_thread_;
std::atomic<bool> speak_running_{false};
std::mutex speak_mutex_;
std::mutex expression_wait_mutex_;
std::condition_variable expression_wait_cv_;
};

232
cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp
View File

@ -21,11 +21,6 @@ BioHeadRobot::BioHeadRobot(const config::BioHeadRobotConfig &config) {
}
BioHeadRobot::~BioHeadRobot()
{
speakstop();
}
bool BioHeadRobot::init() {
@ -117,36 +112,13 @@ double BioHeadRobot::normalizeToAngle(double normalized, size_t index) {
}
void BioHeadRobot::getState(RobotState &state) {
std::lock_guard lock(stateMutex_);
state.error = false;
state.joint_positions = current_joints_;
}
void BioHeadRobot::eStop() {
CMVR_LOG(WARNING) << "[BioHeadRobot] Emergency stop: hold current joint positions.";
(void)sendServoCommands(last_joints_, 0, true);
}
bool BioHeadRobot::setExpressionPoseIfCurrent(
const OperationalToken token,
FacialExpressionState& expression_state,
const double vel,
const double acc)
{
return runIfOperationalActivityCurrent_(token, [&] {
setExpressionPose(expression_state, vel, acc);
});
}
bool BioHeadRobot::streamFacialPoseIfCurrent(
const OperationalToken token,
FacialExpressionState& expression_state,
const double vel,
const double acc)
{
return runIfOperationalActivityCurrent_(token, [&] {
streamFacialPose(expression_state, vel, acc);
});
sendServoCommands(current_joints_, 100); // 快速下发当前角度
}
void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, double vel, double acc) {
@ -188,10 +160,8 @@ void BioHeadRobot::setExpressionPose(FacialExpressionState& expression_state, do
for (size_t i = 0; i < joints.size(); ++i) {
CMVR_LOG(INFO) << "Joint[" << i << "] = " << joints[i]; // 打印每个关节的角度
}
const uint16_t duration = vel > 0.0
? static_cast<uint16_t>(1000.0 / vel)
: 0U;
(void)sendServoCommands(joints, duration);
uint16_t duration = static_cast<uint16_t>(1000.0 / vel);
sendServoCommands(joints, duration);
}
@ -238,30 +208,17 @@ void BioHeadRobot::streamFacialPose(FacialExpressionState& expression_state, dou
CMVR_LOG(INFO) << "嘴角3=： " << ": " << joints[15];
CMVR_LOG(INFO) << "嘴角4=： " << ": " << joints[16];
const uint16_t duration = vel > 0.0
? static_cast<uint16_t>(1000.0 / vel)
: 0U;
(void)sendServoCommands(joints, duration);
uint16_t duration = static_cast<uint16_t>(1000.0 / vel);
sendServoCommands(joints, duration);
}
void BioHeadRobot::speakstart() {
(void)speakStartIfCurrent(beginOperationalActivity());
}
bool BioHeadRobot::speakStartIfCurrent(const OperationalToken token)
{
return startSpeaking(token);
}
bool BioHeadRobot::startSpeaking(const OperationalToken token)
{
std::lock_guard lock(speak_mutex_);
if (speak_running_.load()) {
CMVR_LOG(INFO) << "[BioHeadRobot] speak thread already running.";
return operationalActivityCurrent_(token);
return;
}
// 检查 channels 中是否有 65:8 和 65:9
@ -272,9 +229,12 @@ bool BioHeadRobot::startSpeaking(const OperationalToken token)
}
if (!found8 || !found9) {
CMVR_LOG(ERROR) << "[BioHeadRobot] Required servo channels not found (addr 65 ch 8/9). speakstart aborted.";
return false;
return;
}
// 启动线程
speak_running_.store(true);
// 清理旧线程（若有）
if (speak_thread_ && speak_thread_->joinable()) {
try {
@ -285,24 +245,19 @@ bool BioHeadRobot::startSpeaking(const OperationalToken token)
speak_thread_.reset();
}
bool started = false;
const bool current = runIfOperationalActivityCurrent_(token, [&] {
speak_running_.store(true, std::memory_order_release);
speak_thread_ = std::make_shared<std::thread>(
&BioHeadRobot::speakthread, this, token);
started = true;
});
if (!current || !started) {
speak_running_.store(false, std::memory_order_release);
return false;
}
speak_thread_ = std::make_shared<std::thread>(&BioHeadRobot::speakthread, this);
CMVR_LOG(INFO) << "[BioHeadRobot] speak thread started.";
return true;
}
void BioHeadRobot::speakstop() {
std::lock_guard lock(speak_mutex_);
speak_running_.store(false, std::memory_order_release);
{
if (!speak_running_.load()) {
CMVR_LOG(INFO) << "[BioHeadRobot] speak thread not running.";
return;
}
speak_running_.store(false);
}
// 唤醒线程（如果在 wait 中）
// join 并清理线程对象
if (speak_thread_) {
@ -320,20 +275,7 @@ void BioHeadRobot::speakstop() {
CMVR_LOG(INFO) << "[BioHeadRobot] speak thread stopped.";
}
bool BioHeadRobot::stopOperationalActivity()
{
invalidateOperationalActivities_();
expression_wait_cv_.notify_all();
speakstop();
bool hold_confirmed = false;
(void)runOperationalStop_([&] {
hold_confirmed = sendServoCommands(last_joints_, 0, true);
});
return hold_confirmed;
}
void BioHeadRobot::speakthread(const OperationalToken token) {
void BioHeadRobot::speakthread() {
CMVR_LOG(INFO) << "[BioHeadRobot] speakthread running.";
// 固定参数
@ -371,11 +313,7 @@ void BioHeadRobot::speakthread(const OperationalToken token) {
}
// 以当前角度为基准
std::vector<double> base;
{
std::lock_guard lock(stateMutex_);
base = current_joints_;
}
std::vector<double> base = current_joints_;
if (base.size() != channels_.size()) {
base.resize(channels_.size(), 90.0);
}
@ -408,8 +346,7 @@ void BioHeadRobot::speakthread(const OperationalToken token) {
double current_random_factor = 0.0;
const double random_update_interval = 0.2; // 每0.2秒更新一次随机扰动
while (speak_running_.load(std::memory_order_acquire) &&
operationalActivityCurrent_(token)) {
while (speak_running_.load()) {
auto now = std::chrono::steady_clock::now();
double t = std::chrono::duration_cast<std::chrono::duration<double>>(now - start).count();
@ -515,9 +452,7 @@ void BioHeadRobot::speakthread(const OperationalToken token) {
}
// 下发
if (!sendRawIfCurrent(token, raw_data)) {
break;
}
serial_->sendRawServoData(raw_data);
// 控制循环频率
std::this_thread::sleep_for(std::chrono::milliseconds(step_ms));
@ -548,23 +483,12 @@ void BioHeadRobot::speakthread(const OperationalToken token) {
}
}
if (sendRawIfCurrent(token, restore_data)) {
CMVR_LOG(INFO)
<< "[BioHeadRobot] speakthread exiting and restored base pose.";
} else {
CMVR_LOG(INFO)
<< "[BioHeadRobot] speakthread stopped without a stale restore.";
}
speak_running_.store(false, std::memory_order_release);
serial_->sendRawServoData(restore_data);
CMVR_LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose.";
}
bool BioHeadRobot::sendExpression(
const OperationalToken token,
const std::vector<double>& device_64_angles,
const std::vector<double>& device_65_angles,
const int step_ms)
{
void BioHeadRobot::sendExpression(const std::vector<double>& device_64_angles, const std::vector<double>& device_65_angles, int step_ms) {
std::vector<uint8_t> raw_data;
// 处理设备64角度
@ -587,20 +511,8 @@ bool BioHeadRobot::sendExpression(
raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
}
if (!sendRawIfCurrent(token, raw_data)) {
return false;
}
{
std::unique_lock lock(expression_wait_mutex_);
if (expression_wait_cv_.wait_for(
lock,
std::chrono::seconds(5),
[this, token] {
return !operationalActivityCurrent_(token);
})) {
return false;
}
}
serial_->sendRawServoData(raw_data);
std::this_thread::sleep_for(std::chrono::seconds(5));
// 恢复到原始角度
// 设备64角度（10通道）
@ -630,78 +542,50 @@ bool BioHeadRobot::sendExpression(
raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节
}
return sendRawIfCurrent(token, raw_data_neutral);
serial_->sendRawServoData(raw_data_neutral);
}
//高兴
void BioHeadRobot::expressionHappy() {
(void)expressionHappyIfCurrent(beginOperationalActivity());
}
bool BioHeadRobot::expressionHappyIfCurrent(const OperationalToken token) {
const std::vector<double> device_64_angles = {90, 90, 90, 90, 80, 125, 100, 60, 90, 90};
const std::vector<double> device_65_angles = {100, 80, 125, 135, 100, 105, 110, 90, 90, 90};
return sendExpression(token, device_64_angles, device_65_angles, 0);
sendExpression(device_64_angles, device_65_angles, 0);
}
//惊讶
void BioHeadRobot::expressionSurprised() {
(void)expressionSurprisedIfCurrent(beginOperationalActivity());
}
bool BioHeadRobot::expressionSurprisedIfCurrent(const OperationalToken token) {
const std::vector<double> device_64_angles = {90, 100, 100, 70, 20, 140, 130, 50, 90, 90};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 70, 110};
return sendExpression(token, device_64_angles, device_65_angles, 0);
sendExpression(device_64_angles, device_65_angles, 0);
}
//睡觉
void BioHeadRobot::expressionTired() {
(void)expressionTiredIfCurrent(beginOperationalActivity());
}
bool BioHeadRobot::expressionTiredIfCurrent(const OperationalToken token) {
const std::vector<double> device_64_angles = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 105, 110, 90, 85, 95};
return sendExpression(token, device_64_angles, device_65_angles, 0);
sendExpression(device_64_angles, device_65_angles, 0);
}
//愤怒
void BioHeadRobot::expressionAngry() {
(void)expressionAngryIfCurrent(beginOperationalActivity());
}
bool BioHeadRobot::expressionAngryIfCurrent(const OperationalToken token) {
const std::vector<double> device_64_angles = {90, 70, 90, 110, 70, 125, 110, 80, 70, 90};
const std::vector<double> device_65_angles = {100, 80, 130, 130, 70, 55, 50, 125, 90, 90};
return sendExpression(token, device_64_angles, device_65_angles, 0);
sendExpression(device_64_angles, device_65_angles, 0);
}
//悲伤
void BioHeadRobot::expressionSadness() {
(void)expressionSadnessIfCurrent(beginOperationalActivity());
}
bool BioHeadRobot::expressionSadnessIfCurrent(const OperationalToken token) {
const std::vector<double> device_64_angles = {90, 70, 90, 110, 70, 125, 110, 80, 90, 90};
const std::vector<double> device_65_angles = {100, 80, 130, 130, 70, 55, 50, 125, 90, 90};
return sendExpression(token, device_64_angles, device_65_angles, 0);
sendExpression(device_64_angles, device_65_angles, 0);
}
//打哈欠
void BioHeadRobot::expressionYawn() {
(void)expressionYawnIfCurrent(beginOperationalActivity());
}
bool BioHeadRobot::expressionYawnIfCurrent(const OperationalToken token) {
const std::vector<double> device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
return sendExpression(token, device_64_angles, device_65_angles, 0);
sendExpression(device_64_angles, device_65_angles, 0);
}
bool BioHeadRobot::sendServoCommands(
const std::vector<double>& targets,
const uint16_t duration_ms,
const bool force)
{
if (!serial_ || targets.size() != channels_.size() ||
targets.size() != min_angles_.size() ||
targets.size() != max_angles_.size() ||
targets.size() != last_joints_.size()) {
return false;
}
void BioHeadRobot::sendServoCommands(const std::vector<double>& targets, uint16_t duration_ms) {
std::vector<uint8_t> addrs, chs;
std::vector<uint16_t> raws;
@ -714,14 +598,17 @@ bool BioHeadRobot::sendServoCommands(
continue;
}
// 更新 last_joints_，只有当角度变化较大时才更新
last_joints_[i] = tgt;
// 准备打包数据
addrs.push_back(channels_[i].addr);
chs.push_back(channels_[i].channel);
raws.push_back(angleToRaw(tgt));
}
// 2. 如果没有任何通道需要更新，就直接返回
if (raws.empty() && !force) {
return true;
if (raws.empty()) {
return;
}
std::vector<uint8_t> raw_data;
// 原始格式处理
@ -753,41 +640,7 @@ bool BioHeadRobot::sendServoCommands(
const bool sent = serial_->sendRawServoData(raw_data);
if (sent) {
for (std::size_t i = 0; i < targets.size(); ++i) {
last_joints_[i] =
std::clamp(targets[i], min_angles_[i], max_angles_[i]);
}
std::lock_guard lock(stateMutex_);
current_joints_ = last_joints_;
}
return sent;
}
bool BioHeadRobot::sendRawIfCurrent(
const OperationalToken token,
const std::vector<uint8_t>& raw_data)
{
bool sent = false;
const bool current = runIfOperationalActivityCurrent_(token, [&] {
sent = serial_ && serial_->sendRawServoData(raw_data);
if (!sent || raw_data.size() % 5U != 0U) {
return;
}
for (std::size_t offset = 0; offset < raw_data.size(); offset += 5U) {
for (std::size_t index = 0; index < channels_.size(); ++index) {
if (channels_[index].addr == raw_data[offset] &&
channels_[index].channel == raw_data[offset + 1U]) {
last_joints_[index] = raw_data[offset + 2U];
break;
}
}
}
std::lock_guard lock(stateMutex_);
current_joints_ = last_joints_;
});
return current && sent;
serial_->sendRawServoData(raw_data);
}
@ -798,3 +651,4 @@ uint16_t BioHeadRobot::angleToRaw(double angle) {
} // namespace cmvr::device

6
cmvr-es/devices/camera/abstract_camera.h
View File

@ -131,12 +131,6 @@ namespace cmvr::device {
virtual bool startStreaming() {return true;}
virtual void stopStreaming() {}
virtual bool startOperationalActivity() { return start(); }
// Stops activity started by CameraService::StartCamera without
// tearing down the device lifecycle. Implementations must return true
// only after the camera is quiescent and a later start() can resume it
// without another init(). Unsupported backends fail closed.
virtual bool stopOperationalActivity() { return false; }
virtual bool controlPtz(PtzCommand command, bool stop, int speed) {
(void)command;
(void)stop;

3
cmvr-es/devices/camera/hikvision_camera/include/hikvision_camera.h
View File

@ -36,7 +36,6 @@ public:
bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
bool startStreaming() override;
void stopStreaming() override;
bool stopOperationalActivity() override;
bool controlPtz(PtzCommand command, bool stop, int speed) override;
bool executeJsonCommand(const std::string& request_json, std::string& response_json) override;
bool requestKeyFrame() override;
@ -57,7 +56,7 @@ private:
void releaseSdk_();
bool login_();
bool startPreview_();
bool stopPreview_();
void stopPreview_();
bool requestKeyFrame_();
void stopRecordingUnlocked_();
void fillIntrinsics_(Rs2Intrinsics& intrinsics) const;

28
cmvr-es/devices/camera/hikvision_camera/src/hikvision_camera.cpp
View File

@ -323,7 +323,7 @@ bool HikvisionCamera::start()
if (state_.is_opened) {
return true;
}
if (user_id_ < 0 && !login_()) {
if (!login_()) {
return false;
}
if (!startPreview_()) {
@ -348,7 +348,7 @@ bool HikvisionCamera::stop()
state_.is_streaming = false;
stream_count_ = 0;
resetStreamState_();
(void)stopPreview_();
stopPreview_();
if (user_id_ >= 0) {
NET_DVR_Logout(user_id_);
user_id_ = -1;
@ -544,23 +544,6 @@ void HikvisionCamera::stopStreaming()
}
}
bool HikvisionCamera::stopOperationalActivity()
{
std::lock_guard lock(ctrl_mtx_);
clear_error_();
if (stream_count_ != 0 || state_.is_streaming || state_.is_recording) {
return false;
}
if (!stopPreview_()) {
setError_(sdkError_("NET_DVR_StopRealPlay"));
return false;
}
state_.is_opened = false;
return real_handle_ < 0 && !state_.is_streaming &&
!state_.is_recording;
}
bool HikvisionCamera::controlPtz(PtzCommand command, bool stop, int speed)
{
std::lock_guard lock(ctrl_mtx_);
@ -934,7 +917,7 @@ bool HikvisionCamera::startPreview_()
return true;
}
bool HikvisionCamera::stopPreview_()
void HikvisionCamera::stopPreview_()
{
const int preview_handle = real_handle_;
{
@ -947,12 +930,9 @@ bool HikvisionCamera::stopPreview_()
awaiting_key_frame_ = false;
}
if (preview_handle >= 0) {
if (!NET_DVR_StopRealPlay(preview_handle)) {
return false;
}
NET_DVR_StopRealPlay(preview_handle);
real_handle_ = -1;
}
return true;
}
void HikvisionCamera::fillIntrinsics_(Rs2Intrinsics& intrinsics) const

21
cmvr-es/devices/camera/hikvision_camera/tests/hikvision_camera_callback_test.cpp
View File

@ -269,30 +269,11 @@ bool testCallbackPublicationLifecycle()
CHECK_TRUE(frame.sequence == 0);
CHECK_TRUE(frame.codec_config_generation == 3);
camera.stopStreaming();
CHECK_TRUE(camera.stopOperationalActivity());
CHECK_TRUE(g_stop_callback_count.load() == 1);
cmvr::device::CameraState stopped_state{};
camera.getState(stopped_state);
CHECK_TRUE(stopped_state.is_initialized);
CHECK_TRUE(!stopped_state.is_opened);
CHECK_TRUE(!stopped_state.is_streaming);
// Operational stop keeps the SDK/login lifecycle reusable. start() only
// recreates the preview pipeline and can stream again without init().
CHECK_TRUE(camera.startOperationalActivity());
CHECK_TRUE(camera.startStreaming());
emitIFrame();
CHECK_TRUE(camera.waitEncodedFrame(
frame, cursor, std::chrono::milliseconds(50)));
CHECK_TRUE(frame.stream_epoch == 3);
camera.stopStreaming();
// The fake StopRealPlay invokes the SDK callback synchronously. stop()
// owns ctrl_mtx_ here, proving the callback neither takes that mutex nor
// publishes after the preview handle has been invalidated.
CHECK_TRUE(camera.stop());
CHECK_TRUE(g_stop_callback_count.load() == 2);
CHECK_TRUE(g_stop_callback_count.load() == 1);
CHECK_TRUE(!camera.waitEncodedFrame(
frame, cursor, std::chrono::milliseconds(10)));
return true;

Some files were not shown because too many files have changed in this diff Show More