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

fix(huayan): harden motion and safety lifecycle

Browse Source
This commit is contained in:
xtkuang 2026-08-07 15:11:26 +08:00
parent 3b87f681cf
commit 9095fbf68c
6 changed files with 3840 additions and 357 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,5 +1,7 @@
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
@ -20,9 +22,56 @@ 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)
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()

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

File diff suppressed because it is too large Load Diff

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

@ -9,12 +9,17 @@
#define CMVR_ES_HUAYAN_ROBOT_H
#include <atomic>
#include <chrono>
#include <cstdint>
#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 {
@ -35,15 +40,15 @@ public:
CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
RobotMode getRobotMode() const override;
SafetyMode getSafetyMode() const override;
ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
ControlMode getControlMode() const override;
Result torqueOn() override;
Result torqueOff() override;
Result calibrateZeroQ(const std::string& joint_name) override;
Result emergencyStop() override;
Result protectiveStop() override { return emergencyStop(); }
Result protectiveStop() override;
Result setSpeedScaling(double scaling) override;
double getSpeedScaling() const override { return speed_scaling_; }
double getSpeedScaling() const override { return speed_scaling_.load(); }
bool isProtectiveStopped() const override;
bool isEmergencyStopped() const override;
bool isFault() const override;
@ -71,7 +76,7 @@ public:
Result brakeRelease() override { return torqueOn(); }
Result shutdown() override;
Result clearFault() override;
Result unlockProtectiveStop() override { return clearFault(); }
Result unlockProtectiveStop() override;
Result loadProgram(const std::string& program_name) override;
Result playProgram() override;
Result pauseProgram() override;
@ -84,7 +89,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 { return busy_.load(); }
bool busy() const override;
private:
struct HrState {
@ -101,21 +106,68 @@ 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, int timeout_ms) 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;
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);
private:
config::RobotArmConfig cfg_;
@ -127,12 +179,16 @@ private:
unsigned int robot_id_{0};
std::string tcp_name_{"TCP"};
std::string ucs_name_{"Base"};
double speed_scaling_{1.0};
std::atomic<double> speed_scaling_{1.0};
std::atomic<bool> connected_{false};
std::atomic<bool> busy_{false};
std::atomic<bool> servo_mode_{false};
mutable std::atomic<bool> servo_mode_{false};
std::atomic<bool> software_emergency_stopped_{false};
std::atomic<bool> software_protective_stopped_{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
@ -140,4 +196,4 @@ private:
#endif // CMVR_ES_HUAYAN_ROBOT_H
#endif //CMVR_ES_HUAYAN_ARM_H
#endif //CMVR_ES_HUAYAN_ARM_H

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

@ -0,0 +1,504 @@
#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

834
cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp Normal file
View File

@ -0,0 +1,834 @@
#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;
// 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.
JointPositionCommand joint_a{{0.10, -0.05, 0.08, 0.0, 0.02, -0.03}};
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 Normal file
View File

@ -0,0 +1,232 @@
#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;
}