Skip to content

L
linkerhand-cpp-sdk
Commit 8275ee2e authored by 唐永康's avatar 唐永康
Browse files
Options

feat: add RM75 O6 trajectory replay workflow

parent b145d527
Hide whitespace changes
Inline Side-by-side
Showing with 18130 additions and 1208 deletions
CMakeLists.txt
View file @ 8275ee2e
...@@ -50,7 +50,7 @@ message(STATUS "Found LinkerHand headers: ${LINKER_HAND_INCLUDE_DIR}") ...@@ -50,7 +50,7 @@ message(STATUS "Found LinkerHand headers: ${LINKER_HAND_INCLUDE_DIR}")
unset(REALMAN_ARM_LIB CACHE) unset(REALMAN_ARM_LIB CACHE)
unset(REALMAN_ARM_LIB_CANDIDATE CACHE) unset(REALMAN_ARM_LIB_CANDIDATE CACHE)
set(REALMAN_RM_API2_ROOT "" CACHE PATH "Path to RealMan RM_API2 C++ SDK root, for example /home/mashiro/RM_API2/C++") set(REALMAN_RM_API2_ROOT "" CACHE PATH "Path to the RealMan RM_API2 C++ SDK root")
set(REALMAN_ARM_ROOT "") set(REALMAN_ARM_ROOT "")
set(REALMAN_ARM_INCLUDE_DIR "") set(REALMAN_ARM_INCLUDE_DIR "")
set(REALMAN_ARM_LIB_DIR "") set(REALMAN_ARM_LIB_DIR "")
...@@ -61,7 +61,6 @@ if(REALMAN_RM_API2_ROOT) ...@@ -61,7 +61,6 @@ if(REALMAN_RM_API2_ROOT)
list(APPEND REALMAN_ARM_CANDIDATE_ROOTS ${REALMAN_RM_API2_ROOT}) list(APPEND REALMAN_ARM_CANDIDATE_ROOTS ${REALMAN_RM_API2_ROOT})
endif() endif()
list(APPEND REALMAN_ARM_CANDIDATE_ROOTS list(APPEND REALMAN_ARM_CANDIDATE_ROOTS
/home/mashiro/RM_API2/C++
${CMAKE_CURRENT_SOURCE_DIR}/third_party/Robotic_Arm ${CMAKE_CURRENT_SOURCE_DIR}/third_party/Robotic_Arm
) )
list(REMOVE_DUPLICATES REALMAN_ARM_CANDIDATE_ROOTS) list(REMOVE_DUPLICATES REALMAN_ARM_CANDIDATE_ROOTS)
...@@ -149,7 +148,7 @@ if(REALMAN_ARM_INCLUDE_DIR) ...@@ -149,7 +148,7 @@ if(REALMAN_ARM_INCLUDE_DIR)
endif() endif()
option(REALMAN_BUILD_HARDWARE_TESTS "Build independent RealMan hardware module test executables" ON) option(REALMAN_BUILD_HARDWARE_TESTS "Build independent RealMan hardware module test executables" ON)
option(REALMAN_REGISTER_HARDWARE_TESTS "Register independent RealMan hardware module tests with CTest" ON) option(REALMAN_REGISTER_HARDWARE_TESTS "Register manual RealMan hardware programs with CTest" OFF)
if(REALMAN_REGISTER_HARDWARE_TESTS) if(REALMAN_REGISTER_HARDWARE_TESTS)
enable_testing() enable_testing()
......
CMakePresets.json
View file @ 8275ee2e
...@@ -17,8 +17,8 @@ ...@@ -17,8 +17,8 @@
"CMAKE_EXPORT_COMPILE_COMMANDS": "ON", "CMAKE_EXPORT_COMPILE_COMMANDS": "ON",
"BUILD_TESTING": "OFF", "BUILD_TESTING": "OFF",
"REALMAN_BUILD_HARDWARE_TESTS": "ON", "REALMAN_BUILD_HARDWARE_TESTS": "ON",
"REALMAN_REGISTER_HARDWARE_TESTS": "ON", "REALMAN_REGISTER_HARDWARE_TESTS": "OFF",
"REALMAN_RM_API2_ROOT": "/home/mashiro/RM_API2/C++" "REALMAN_RM_API2_ROOT": "$env{REALMAN_RM_API2_ROOT}"
} }
}, },
{ {
...@@ -32,8 +32,8 @@ ...@@ -32,8 +32,8 @@
"CMAKE_EXPORT_COMPILE_COMMANDS": "ON", "CMAKE_EXPORT_COMPILE_COMMANDS": "ON",
"BUILD_TESTING": "OFF", "BUILD_TESTING": "OFF",
"REALMAN_BUILD_HARDWARE_TESTS": "ON", "REALMAN_BUILD_HARDWARE_TESTS": "ON",
"REALMAN_REGISTER_HARDWARE_TESTS": "ON", "REALMAN_REGISTER_HARDWARE_TESTS": "OFF",
"REALMAN_RM_API2_ROOT": "/home/mashiro/RM_API2/C++" "REALMAN_RM_API2_ROOT": "$env{REALMAN_RM_API2_ROOT}"
} }
} }
], ],
...@@ -102,27 +102,6 @@ ...@@ -102,27 +102,6 @@
] ]
}, },
{ {
"name": "realman-hw-run-readonly-tests-debug",
"configurePreset": "clion-debug",
"targets": [
"run_realman_hardware_readonly_tests"
]
},
{
"name": "rm75-acceptance-run-readonly-debug",
"configurePreset": "clion-debug",
"targets": [
"run_rm75_acceptance_readonly_tests"
]
},
{
"name": "realman-hw-run-tests-debug",
"configurePreset": "clion-debug",
"targets": [
"run_realman_hardware_tests"
]
},
{
"name": "realman-hw-force-retreat-debug", "name": "realman-hw-force-retreat-debug",
"configurePreset": "clion-debug", "configurePreset": "clion-debug",
"targets": [ "targets": [
...@@ -137,13 +116,6 @@ ...@@ -137,13 +116,6 @@
] ]
}, },
{ {
"name": "realman-hw-run-manual-tests-debug",
"configurePreset": "clion-debug",
"targets": [
"run_realman_hardware_manual_tests"
]
},
{
"name": "realman-demo-release", "name": "realman-demo-release",
"configurePreset": "clion-release", "configurePreset": "clion-release",
"targets": [ "targets": [
...@@ -207,27 +179,6 @@ ...@@ -207,27 +179,6 @@
] ]
}, },
{ {
"name": "realman-hw-run-readonly-tests-release",
"configurePreset": "clion-release",
"targets": [
"run_realman_hardware_readonly_tests"
]
},
{
"name": "rm75-acceptance-run-readonly-release",
"configurePreset": "clion-release",
"targets": [
"run_rm75_acceptance_readonly_tests"
]
},
{
"name": "realman-hw-run-tests-release",
"configurePreset": "clion-release",
"targets": [
"run_realman_hardware_tests"
]
},
{
"name": "realman-hw-force-retreat-release", "name": "realman-hw-force-retreat-release",
"configurePreset": "clion-release", "configurePreset": "clion-release",
"targets": [ "targets": [
...@@ -240,13 +191,6 @@ ...@@ -240,13 +191,6 @@
"targets": [ "targets": [
"realman_hw_test_force_retreat_hold" "realman_hw_test_force_retreat_hold"
] ]
},
{
"name": "realman-hw-run-manual-tests-release",
"configurePreset": "clion-release",
"targets": [
"run_realman_hardware_manual_tests"
]
} }
] ]
} }
RM/CMakeLists.txt
View file @ 8275ee2e
cmake_minimum_required(VERSION 3.16) cmake_minimum_required(VERSION 3.16)
# RM 是独立机械臂子工程，和仓库根目录的 LinkerHand 灵巧手工程分开构建。 # RM 是独立睿尔曼 RM75 机械臂子工程，和仓库根目录的 LinkerHand O6 灵巧手工程分开构建。
# 这样后续更换机械臂品牌时，不会影响灵巧手 SDK 的 CMake 目标。 # 当前支持边界只覆盖“睿尔曼机械臂 + 灵心巧手 O6”目标程序，不做多机械臂品牌泛化。
project(RMJsonControl project(RMJsonControl
VERSION 0.1.0 VERSION 0.1.0
LANGUAGES CXX LANGUAGES CXX
DESCRIPTION "Standalone JSON-command RealMan robotic arm control project" DESCRIPTION "RM75 control, drag-teach capture, validation, and raw trajectory storage"
) )
set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF) set(CMAKE_CXX_EXTENSIONS OFF)
option(RM_BUILD_TESTING "Build RM JSON command parser tests" ON) option(RM_BUILD_TESTING "Build RM unit and no-hardware integration tests" ON)
if(MSVC)
add_compile_options(/W4 /permissive-)
elseif(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(OpenSSL REQUIRED COMPONENTS Crypto)
find_package(Threads REQUIRED)
# core 库只包含 JSON 解析和命令执行流程，不链接任何真实硬件 SDK。 # core 库只包含 JSON 解析和命令执行流程，不链接任何真实硬件 SDK。
# 这样 parser/runner 的单元测试可以在没有机械臂的机器上运行。 # 这样 parser/runner 的单元测试可以在没有机械臂的机器上运行。
...@@ -31,6 +40,12 @@ target_compile_features(rm_json_core PUBLIC cxx_std_17) ...@@ -31,6 +40,12 @@ target_compile_features(rm_json_core PUBLIC cxx_std_17)
add_library(rm_trajectory_core add_library(rm_trajectory_core
src/modules/trajectory/trajectory_file_store.cpp src/modules/trajectory/trajectory_file_store.cpp
src/modules/trajectory/sha256_file_checksum.cpp
src/modules/trajectory/raw_trajectory_store.cpp
src/modules/trajectory/drag_trajectory_validator.cpp
src/modules/trajectory/vendor_drag_trajectory_parser.cpp
src/modules/trajectory/reversed_vendor_trajectory_store.cpp
src/modules/trajectory/trajectory_playback_report_store.cpp
src/core/drag_teach_recorder.cpp src/core/drag_teach_recorder.cpp
) )
...@@ -40,31 +55,122 @@ target_include_directories(rm_trajectory_core ...@@ -40,31 +55,122 @@ target_include_directories(rm_trajectory_core
) )
target_compile_features(rm_trajectory_core PUBLIC cxx_std_17) target_compile_features(rm_trajectory_core PUBLIC cxx_std_17)
target_link_libraries(rm_trajectory_core
PUBLIC
OpenSSL::Crypto
)
target_compile_definitions(rm_trajectory_core target_compile_definitions(rm_trajectory_core
PRIVATE
RM_RUNTIME_DATA_ROOT="${CMAKE_CURRENT_BINARY_DIR}/runtime_data"
)
add_library(rm_drag_teach_core
src/core/hardware_execution_gate.cpp
src/core/drag_teach_service.cpp
)
target_include_directories(rm_drag_teach_core
PUBLIC PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/include
)
target_compile_features(rm_drag_teach_core PUBLIC cxx_std_17)
target_link_libraries(rm_drag_teach_core
PUBLIC
rm_trajectory_core
Threads::Threads
)
add_library(rm_linker_hand_adapter
${CMAKE_CURRENT_LIST_DIR}/../src/linker_hand/linker_hand_adapter.cpp
)
target_include_directories(rm_linker_hand_adapter
PUBLIC
${CMAKE_CURRENT_LIST_DIR}/../include
${CMAKE_CURRENT_SOURCE_DIR}/include
)
target_compile_features(rm_linker_hand_adapter PUBLIC cxx_std_17)
# RM_API2 的 Modbus byte 数组编解码保持为无硬件纯转换库，使边界值测试无需连接 SDK 或设备。
add_library(rm_realman_modbus_codec
${CMAKE_CURRENT_LIST_DIR}/../src/realman/modbus_register_codec.cpp
)
target_include_directories(rm_realman_modbus_codec
PUBLIC
${CMAKE_CURRENT_LIST_DIR}/../include
${CMAKE_CURRENT_SOURCE_DIR}/include
)
target_compile_features(rm_realman_modbus_codec PUBLIC cxx_std_17)
add_library(rm_o6_bus_scanner
src/core/o6_bus_scanner.cpp
)
target_include_directories(rm_o6_bus_scanner
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/include
)
target_compile_features(rm_o6_bus_scanner PUBLIC cxx_std_17)
add_library(rm_arm_o6_task_core
src/core/arm_o6_trajectory_task.cpp
)
target_include_directories(rm_arm_o6_task_core
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/include
)
target_compile_features(rm_arm_o6_task_core PUBLIC cxx_std_17)
target_link_libraries(rm_arm_o6_task_core
PUBLIC
rm_drag_teach_core
rm_linker_hand_adapter
Threads::Threads
)
add_library(rm_cli_config
src/config/json_runner_cli_config.cpp
src/config/drag_teach_cli_config.cpp
src/config/drag_teach_safety_config.cpp
src/config/trajectory_convert_cli_config.cpp
src/config/trajectory_o6_task_cli_config.cpp
)
target_include_directories(rm_cli_config
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/include
)
target_compile_features(rm_cli_config PUBLIC cxx_std_17)
target_compile_definitions(rm_cli_config
PRIVATE
RM_PROJECT_ROOT="${CMAKE_CURRENT_SOURCE_DIR}" RM_PROJECT_ROOT="${CMAKE_CURRENT_SOURCE_DIR}"
) )
target_link_libraries(rm_cli_config
PUBLIC
rm_json_core
rm_trajectory_core
)
add_executable(rm_trajectory_convert add_executable(rm_trajectory_convert
src/trajectory_convert_main.cpp src/trajectory_convert_main.cpp
) )
target_link_libraries(rm_trajectory_convert target_link_libraries(rm_trajectory_convert
PRIVATE PRIVATE
rm_cli_config
rm_trajectory_core rm_trajectory_core
) )
set(REALMAN_RM_API2_ROOT "" CACHE PATH "Path to RealMan RM_API2 C++ SDK root, for example /home/mashiro/RM_API2/C++") set(REALMAN_RM_API2_ROOT "" CACHE PATH "Path to the RealMan RM_API2 C++ SDK root")
set(REALMAN_ARM_ROOT "") set(REALMAN_ARM_ROOT "")
set(REALMAN_ARM_INCLUDE_DIR "") set(REALMAN_ARM_INCLUDE_DIR "")
set(REALMAN_ARM_LIB_DIR "") set(REALMAN_ARM_LIB_DIR "")
set(REALMAN_ARM_LIB "") set(REALMAN_ARM_LIB "")
set(REALMAN_ARM_DLL "")
set(REALMAN_ARM_CANDIDATE_ROOTS) set(REALMAN_ARM_CANDIDATE_ROOTS)
if(REALMAN_RM_API2_ROOT) if(REALMAN_RM_API2_ROOT)
list(APPEND REALMAN_ARM_CANDIDATE_ROOTS ${REALMAN_RM_API2_ROOT}) list(APPEND REALMAN_ARM_CANDIDATE_ROOTS ${REALMAN_RM_API2_ROOT})
endif() endif()
list(APPEND REALMAN_ARM_CANDIDATE_ROOTS list(APPEND REALMAN_ARM_CANDIDATE_ROOTS
/home/mashiro/RM_API2/C++
${CMAKE_CURRENT_LIST_DIR}/../third_party/Robotic_Arm ${CMAKE_CURRENT_LIST_DIR}/../third_party/Robotic_Arm
) )
list(REMOVE_DUPLICATES REALMAN_ARM_CANDIDATE_ROOTS) list(REMOVE_DUPLICATES REALMAN_ARM_CANDIDATE_ROOTS)
...@@ -81,7 +187,29 @@ foreach(CANDIDATE_ROOT ${REALMAN_ARM_CANDIDATE_ROOTS}) ...@@ -81,7 +187,29 @@ foreach(CANDIDATE_ROOT ${REALMAN_ARM_CANDIDATE_ROOTS})
endif() endif()
set(CANDIDATE_LIB_DIRS) set(CANDIDATE_LIB_DIRS)
if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64|amd64|AMD64") if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8)
file(GLOB CANDIDATE_RELEASE_LIB_DIRS CONFIGURE_DEPENDS
${CANDIDATE_ROOT}/windows/win_x64_c++_vv*
)
file(GLOB CANDIDATE_DEBUG_LIB_DIRS CONFIGURE_DEPENDS
${CANDIDATE_ROOT}/windows/win_x64_c++_debug*
)
list(APPEND CANDIDATE_LIB_DIRS
${CANDIDATE_RELEASE_LIB_DIRS}
${CANDIDATE_DEBUG_LIB_DIRS}
)
elseif(WIN32)
file(GLOB CANDIDATE_RELEASE_LIB_DIRS CONFIGURE_DEPENDS
${CANDIDATE_ROOT}/windows/win_x86_c++_vv*
)
file(GLOB CANDIDATE_DEBUG_LIB_DIRS CONFIGURE_DEPENDS
${CANDIDATE_ROOT}/windows/win_x86_c++_debug*
)
list(APPEND CANDIDATE_LIB_DIRS
${CANDIDATE_RELEASE_LIB_DIRS}
${CANDIDATE_DEBUG_LIB_DIRS}
)
elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64|amd64|AMD64")
file(GLOB CANDIDATE_RELEASE_LIB_DIRS CONFIGURE_DEPENDS ${CANDIDATE_ROOT}/linux/linux_x86_c++_vv*) file(GLOB CANDIDATE_RELEASE_LIB_DIRS CONFIGURE_DEPENDS ${CANDIDATE_ROOT}/linux/linux_x86_c++_vv*)
file(GLOB CANDIDATE_DEBUG_LIB_DIRS CONFIGURE_DEPENDS ${CANDIDATE_ROOT}/linux/linux_x86_c++_debug*) file(GLOB CANDIDATE_DEBUG_LIB_DIRS CONFIGURE_DEPENDS ${CANDIDATE_ROOT}/linux/linux_x86_c++_debug*)
list(APPEND CANDIDATE_LIB_DIRS list(APPEND CANDIDATE_LIB_DIRS
...@@ -116,10 +244,20 @@ foreach(CANDIDATE_ROOT ${REALMAN_ARM_CANDIDATE_ROOTS}) ...@@ -116,10 +244,20 @@ foreach(CANDIDATE_ROOT ${REALMAN_ARM_CANDIDATE_ROOTS})
set(REALMAN_ARM_INCLUDE_DIR ${CANDIDATE_INCLUDE_DIR}) set(REALMAN_ARM_INCLUDE_DIR ${CANDIDATE_INCLUDE_DIR})
get_filename_component(REALMAN_ARM_LIB_DIR ${REALMAN_ARM_LIB_CANDIDATE} DIRECTORY) get_filename_component(REALMAN_ARM_LIB_DIR ${REALMAN_ARM_LIB_CANDIDATE} DIRECTORY)
set(REALMAN_ARM_LIB ${REALMAN_ARM_LIB_CANDIDATE}) set(REALMAN_ARM_LIB ${REALMAN_ARM_LIB_CANDIDATE})
if(WIN32)
unset(REALMAN_ARM_DLL_CANDIDATE CACHE)
find_file(REALMAN_ARM_DLL_CANDIDATE
NAMES api_cpp.dll
PATHS ${REALMAN_ARM_LIB_DIR}
NO_DEFAULT_PATH
)
set(REALMAN_ARM_DLL ${REALMAN_ARM_DLL_CANDIDATE})
endif()
endif() endif()
endforeach() endforeach()
if(REALMAN_ARM_LIB AND EXISTS ${REALMAN_ARM_INCLUDE_DIR}/rm_interface.h) if(REALMAN_ARM_LIB AND EXISTS ${REALMAN_ARM_INCLUDE_DIR}/rm_interface.h AND
(NOT WIN32 OR REALMAN_ARM_DLL))
# 不同版本 RM_API2 的销毁函数可能拼写不同： # 不同版本 RM_API2 的销毁函数可能拼写不同：
# 有的版本是 rm_destroy，有的版本是 rm_destory。 # 有的版本是 rm_destroy，有的版本是 rm_destory。
# 这里在配置阶段读取头文件，给 C++ 代码注入对应宏。 # 这里在配置阶段读取头文件，给 C++ 代码注入对应宏。
...@@ -131,21 +269,35 @@ if(REALMAN_ARM_LIB AND EXISTS ${REALMAN_ARM_INCLUDE_DIR}/rm_interface.h) ...@@ -131,21 +269,35 @@ if(REALMAN_ARM_LIB AND EXISTS ${REALMAN_ARM_INCLUDE_DIR}/rm_interface.h)
endif() endif()
add_library(realman_api_cpp SHARED IMPORTED GLOBAL) add_library(realman_api_cpp SHARED IMPORTED GLOBAL)
set_target_properties(realman_api_cpp PROPERTIES if(WIN32)
IMPORTED_LOCATION ${REALMAN_ARM_LIB} set_target_properties(realman_api_cpp PROPERTIES
INTERFACE_INCLUDE_DIRECTORIES ${REALMAN_ARM_INCLUDE_DIR} IMPORTED_IMPLIB ${REALMAN_ARM_LIB}
INTERFACE_COMPILE_DEFINITIONS ${REALMAN_DESTROY_COMPILE_DEFINITION} IMPORTED_LOCATION ${REALMAN_ARM_DLL}
) INTERFACE_INCLUDE_DIRECTORIES ${REALMAN_ARM_INCLUDE_DIR}
INTERFACE_COMPILE_DEFINITIONS ${REALMAN_DESTROY_COMPILE_DEFINITION}
)
else()
set_target_properties(realman_api_cpp PROPERTIES
IMPORTED_LOCATION ${REALMAN_ARM_LIB}
INTERFACE_INCLUDE_DIRECTORIES ${REALMAN_ARM_INCLUDE_DIR}
INTERFACE_COMPILE_DEFINITIONS ${REALMAN_DESTROY_COMPILE_DEFINITION}
)
endif()
add_library(rm_realman_driver add_library(rm_realman_driver
src/drivers/realman/realman_robot_arm.cpp ${CMAKE_CURRENT_LIST_DIR}/../src/realman/realman_arm_adapter.cpp
)
target_include_directories(rm_realman_driver
PUBLIC
${CMAKE_CURRENT_LIST_DIR}/../include
${CMAKE_CURRENT_SOURCE_DIR}/include
) )
target_link_libraries(rm_realman_driver target_link_libraries(rm_realman_driver
PUBLIC PUBLIC
rm_json_core
realman_api_cpp realman_api_cpp
PRIVATE PRIVATE
pthread rm_realman_modbus_codec
Threads::Threads
) )
add_executable(rm_json_runner add_executable(rm_json_runner
...@@ -153,6 +305,7 @@ if(REALMAN_ARM_LIB AND EXISTS ${REALMAN_ARM_INCLUDE_DIR}/rm_interface.h) ...@@ -153,6 +305,7 @@ if(REALMAN_ARM_LIB AND EXISTS ${REALMAN_ARM_INCLUDE_DIR}/rm_interface.h)
) )
target_link_libraries(rm_json_runner target_link_libraries(rm_json_runner
PRIVATE PRIVATE
rm_cli_config
rm_json_core rm_json_core
rm_realman_driver rm_realman_driver
) )
...@@ -166,23 +319,80 @@ if(REALMAN_ARM_LIB AND EXISTS ${REALMAN_ARM_INCLUDE_DIR}/rm_interface.h) ...@@ -166,23 +319,80 @@ if(REALMAN_ARM_LIB AND EXISTS ${REALMAN_ARM_INCLUDE_DIR}/rm_interface.h)
) )
target_link_libraries(rm_drag_teach_record target_link_libraries(rm_drag_teach_record
PRIVATE PRIVATE
rm_trajectory_core rm_cli_config
rm_drag_teach_core
rm_realman_driver rm_realman_driver
) )
set_target_properties(rm_drag_teach_record PROPERTIES set_target_properties(rm_drag_teach_record PROPERTIES
BUILD_RPATH ${REALMAN_ARM_LIB_DIR} BUILD_RPATH ${REALMAN_ARM_LIB_DIR}
INSTALL_RPATH ${REALMAN_ARM_LIB_DIR} INSTALL_RPATH ${REALMAN_ARM_LIB_DIR}
) )
if(WIN32)
add_custom_command(TARGET rm_drag_teach_record POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy_if_different
${REALMAN_ARM_DLL}
$<TARGET_FILE_DIR:rm_drag_teach_record>
)
endif()
add_executable(rm_trajectory_o6_grasp
src/trajectory_o6_grasp_main.cpp
)
target_link_libraries(rm_trajectory_o6_grasp
PRIVATE
rm_cli_config
rm_arm_o6_task_core
rm_realman_driver
rm_linker_hand_adapter
)
set_target_properties(rm_trajectory_o6_grasp PROPERTIES
BUILD_RPATH ${REALMAN_ARM_LIB_DIR}
INSTALL_RPATH ${REALMAN_ARM_LIB_DIR}
)
if(WIN32)
add_custom_command(TARGET rm_trajectory_o6_grasp POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy_if_different
${REALMAN_ARM_DLL}
$<TARGET_FILE_DIR:rm_trajectory_o6_grasp>
)
endif()
add_executable(rm_o6_bus_probe
src/o6_bus_probe_main.cpp
)
target_link_libraries(rm_o6_bus_probe
PRIVATE
rm_o6_bus_scanner
rm_realman_driver
rm_linker_hand_adapter
)
set_target_properties(rm_o6_bus_probe PROPERTIES
BUILD_RPATH ${REALMAN_ARM_LIB_DIR}
INSTALL_RPATH ${REALMAN_ARM_LIB_DIR}
)
add_executable(rm_o6_grip_demo
src/o6_grip_demo_main.cpp
)
target_link_libraries(rm_o6_grip_demo
PRIVATE
rm_realman_driver
rm_linker_hand_adapter
)
set_target_properties(rm_o6_grip_demo PROPERTIES
BUILD_RPATH ${REALMAN_ARM_LIB_DIR}
INSTALL_RPATH ${REALMAN_ARM_LIB_DIR}
)
message(STATUS "Found RealMan RM_API2 root: ${REALMAN_ARM_ROOT}") message(STATUS "Found RealMan RM_API2 root: ${REALMAN_ARM_ROOT}")
message(STATUS "Found RealMan RM_API2 library: ${REALMAN_ARM_LIB}") message(STATUS "Found RealMan RM_API2 library: ${REALMAN_ARM_LIB}")
message(STATUS "Found RealMan RM_API2 headers: ${REALMAN_ARM_INCLUDE_DIR}") message(STATUS "Found RealMan RM_API2 headers: ${REALMAN_ARM_INCLUDE_DIR}")
else() else()
message(WARNING "RealMan RM_API2 library or headers not found; rm_json_runner will not be built.") message(WARNING "RealMan RM_API2 library or headers not found; hardware executables will not be built.")
endif() endif()
if(RM_BUILD_TESTING) if(RM_BUILD_TESTING)
# 当前测试只测 JSON/命令解析，不访问真实机械臂。 # 当前测试覆盖解析、轨迹、存储、安全门控和服务状态机，全部禁止访问真实机械臂。
enable_testing() enable_testing()
add_executable(rm_json_command_tests add_executable(rm_json_command_tests
tests/test_json_command_parser.cpp tests/test_json_command_parser.cpp
...@@ -205,4 +415,168 @@ if(RM_BUILD_TESTING) ...@@ -205,4 +415,168 @@ if(RM_BUILD_TESTING)
RM_TEST_OUTPUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/test_outputs" RM_TEST_OUTPUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/test_outputs"
) )
add_test(NAME rm_trajectory_file_store_tests COMMAND rm_trajectory_file_store_tests) add_test(NAME rm_trajectory_file_store_tests COMMAND rm_trajectory_file_store_tests)
add_executable(rm_cli_config_tests
tests/test_cli_config.cpp
)
target_link_libraries(rm_cli_config_tests
PRIVATE
rm_cli_config
)
add_test(NAME rm_cli_config_tests COMMAND rm_cli_config_tests)
add_executable(rm_drag_teach_safety_config_tests
tests/test_drag_teach_safety_config.cpp
)
target_link_libraries(rm_drag_teach_safety_config_tests
PRIVATE
rm_cli_config
)
target_compile_definitions(rm_drag_teach_safety_config_tests
PRIVATE
RM_TEST_OUTPUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/test_outputs"
)
add_test(NAME rm_drag_teach_safety_config_tests
COMMAND rm_drag_teach_safety_config_tests)
add_executable(rm_hardware_execution_gate_tests
tests/test_hardware_execution_gate.cpp
)
target_link_libraries(rm_hardware_execution_gate_tests
PRIVATE
rm_drag_teach_core
)
add_test(NAME rm_hardware_execution_gate_tests
COMMAND rm_hardware_execution_gate_tests)
add_executable(rm_drag_trajectory_validator_tests
tests/test_drag_trajectory_validator.cpp
)
target_link_libraries(rm_drag_trajectory_validator_tests
PRIVATE
rm_trajectory_core
)
add_test(NAME rm_drag_trajectory_validator_tests
COMMAND rm_drag_trajectory_validator_tests)
add_executable(rm_raw_trajectory_store_tests
tests/test_raw_trajectory_store.cpp
)
target_link_libraries(rm_raw_trajectory_store_tests
PRIVATE
rm_json_core
rm_trajectory_core
)
target_compile_definitions(rm_raw_trajectory_store_tests
PRIVATE
RM_TEST_OUTPUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/test_outputs"
)
add_test(NAME rm_raw_trajectory_store_tests
COMMAND rm_raw_trajectory_store_tests)
add_executable(rm_drag_teach_service_tests
tests/test_drag_teach_service.cpp
)
target_link_libraries(rm_drag_teach_service_tests
PRIVATE
rm_drag_teach_core
)
target_compile_definitions(rm_drag_teach_service_tests
PRIVATE
RM_TEST_OUTPUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/test_outputs"
)
add_test(NAME rm_drag_teach_service_tests
COMMAND rm_drag_teach_service_tests)
add_executable(rm_vendor_drag_trajectory_parser_tests
tests/test_vendor_drag_trajectory_parser.cpp
)
target_link_libraries(rm_vendor_drag_trajectory_parser_tests
PRIVATE
rm_trajectory_core
)
target_compile_definitions(rm_vendor_drag_trajectory_parser_tests
PRIVATE
RM_TEST_OUTPUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/test_outputs"
)
add_test(NAME rm_vendor_drag_trajectory_parser_tests
COMMAND rm_vendor_drag_trajectory_parser_tests)
add_executable(rm_reversed_vendor_trajectory_store_tests
tests/test_reversed_vendor_trajectory_store.cpp
)
target_link_libraries(rm_reversed_vendor_trajectory_store_tests
PRIVATE
rm_trajectory_core
)
target_compile_definitions(rm_reversed_vendor_trajectory_store_tests
PRIVATE
RM_TEST_OUTPUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/test_outputs"
)
add_test(NAME rm_reversed_vendor_trajectory_store_tests
COMMAND rm_reversed_vendor_trajectory_store_tests)
add_executable(rm_trajectory_o6_task_cli_config_tests
tests/test_trajectory_o6_task_cli_config.cpp
)
target_link_libraries(rm_trajectory_o6_task_cli_config_tests
PRIVATE
rm_cli_config
)
add_test(NAME rm_trajectory_o6_task_cli_config_tests
COMMAND rm_trajectory_o6_task_cli_config_tests)
add_executable(rm_linker_hand_adapter_tests
tests/test_linker_hand_adapter.cpp
)
target_link_libraries(rm_linker_hand_adapter_tests
PRIVATE
rm_linker_hand_adapter
)
add_test(NAME rm_linker_hand_adapter_tests
COMMAND rm_linker_hand_adapter_tests)
add_executable(rm_realman_modbus_codec_tests
tests/test_modbus_register_codec.cpp
)
target_link_libraries(rm_realman_modbus_codec_tests
PRIVATE
rm_realman_modbus_codec
)
add_test(NAME rm_realman_modbus_codec_tests
COMMAND rm_realman_modbus_codec_tests)
add_executable(rm_trajectory_playback_report_store_tests
tests/test_trajectory_playback_report_store.cpp
)
target_link_libraries(rm_trajectory_playback_report_store_tests
PRIVATE
rm_trajectory_core
)
target_compile_definitions(rm_trajectory_playback_report_store_tests
PRIVATE
RM_TEST_OUTPUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/test_outputs"
)
add_test(NAME rm_trajectory_playback_report_store_tests
COMMAND rm_trajectory_playback_report_store_tests)
add_executable(rm_arm_o6_trajectory_task_tests
tests/test_arm_o6_trajectory_task.cpp
)
target_link_libraries(rm_arm_o6_trajectory_task_tests
PRIVATE
rm_arm_o6_task_core
)
add_test(NAME rm_arm_o6_trajectory_task_tests
COMMAND rm_arm_o6_trajectory_task_tests)
add_executable(rm_o6_bus_scanner_tests
tests/test_o6_bus_scanner.cpp
)
target_link_libraries(rm_o6_bus_scanner_tests
PRIVATE
rm_o6_bus_scanner
)
add_test(NAME rm_o6_bus_scanner_tests
COMMAND rm_o6_bus_scanner_tests)
endif() endif()
RM/README.md
View file @ 8275ee2e
# RM JSON 机械臂控制子工程 # RM JSON 机械臂控制子工程
这个目录只放机械臂相关代码，和上层 LinkerHand 灵巧手代码分开维护。后续如果机械臂从睿尔曼换成其他品牌，优先保留 `interfaces/`、`core/`、`config/`，只新增或替换 `drivers/<vendor>/`。 这个目录只放睿尔曼 RM75 机械臂相关代码，和上层 LinkerHand O6 灵巧手代码分开维护。当前支持边界只覆盖“睿尔曼机械臂 + 灵心巧手 O6”的目标程序，不做其他机械臂品牌或其他灵巧手型号的泛化支持。
## 支持边界
- 机械臂：睿尔曼 RM75 / RM_API2。
- 灵巧手联动目标：灵心巧手 O6。
- 不支持范围：其他机械臂品牌、其他灵巧手型号、跨品牌驱动适配层。
- 保留 `interfaces/` 的目的只是隔离 core 和 RealMan SDK 头文件，不表示当前要支持多品牌机械臂。
## 目录结构 ## 目录结构
...@@ -17,7 +24,7 @@ RM/ ...@@ -17,7 +24,7 @@ RM/
│ ├── config/：JSON 解析和命令结构 │ ├── config/：JSON 解析和命令结构
│ ├── core/：JsonCommandRunner、DragTeachRecorder 等流程编排 │ ├── core/：JsonCommandRunner、DragTeachRecorder 等流程编排
│ ├── modules/trajectory/：轨迹目录、CSV、C++ waypoint 转换 │ ├── modules/trajectory/：轨迹目录、CSV、C++ waypoint 转换
│ └── drivers/realman/：睿尔曼 RM_API2 适配层 │ └── drivers/realman/：旧名称兼容头，不包含厂商 SDK
├── src/ ├── src/
│ ├── config/ │ ├── config/
│ ├── core/ │ ├── core/
...@@ -25,15 +32,14 @@ RM/ ...@@ -25,15 +32,14 @@ RM/
│ ├── drivers/realman/ │ ├── drivers/realman/
│ ├── main.cpp │ ├── main.cpp
│ ├── drag_teach_record_main.cpp │ ├── drag_teach_record_main.cpp
│ ├── trajectory_o6_grasp_main.cpp
│ └── trajectory_convert_main.cpp │ └── trajectory_convert_main.cpp
├── data/trajectories/ └── tests/：不依赖真实硬件的单元与集成测试
│ ├── csv/
│ ├── vendor/
│ ├── converted/
│ └── reports/
└── tests/：不依赖真实硬件的命令解析测试
``` ```
真实 RM_API2 适配器位于仓库根目录 `include/flight_robot/realman/` 与
`src/realman/`；业务层不得包含 `rm_interface.h`。
## JSON 指令格式 ## JSON 指令格式
顶层字段： 顶层字段：
...@@ -129,16 +135,17 @@ RM/ ...@@ -129,16 +135,17 @@ RM/
## 编译 ## 编译
```bash ```bash
cd /home/mashiro/project/linkerhand-cpp-sdk cd linkerhand-cpp-sdk
cmake -S RM -B RM/build -DCMAKE_BUILD_TYPE=Debug cmake -S RM -B RM/build -DCMAKE_BUILD_TYPE=Debug \
-DREALMAN_RM_API2_ROOT=/path/to/RM_API2/C++
cmake --build RM/build -j cmake --build RM/build -j
ctest --test-dir RM/build --output-on-failure ctest --test-dir RM/build --output-on-failure
``` ```
本机默认会优先查找： 工程只从显式缓存变量或仓库内备份查找 SDK：
```text ```text
/home/mashiro/RM_API2/C++ REALMAN_RM_API2_ROOT
RM/../third_party/Robotic_Arm RM/../third_party/Robotic_Arm
``` ```
...@@ -179,56 +186,150 @@ cmake -S RM -B RM/build -DREALMAN_RM_API2_ROOT=/path/to/RM_API2/C++ ...@@ -179,56 +186,150 @@ cmake -S RM -B RM/build -DREALMAN_RM_API2_ROOT=/path/to/RM_API2/C++
} }
``` ```
第一次真实运动必须清空工作空间，低速、小角度，并保证急停可触达。 `rm_json_runner` 是既有程序，不满足本项目新增的完整 Live 双重授权门控；当前阶段只允许
执行 `--validate-only` 或明确 DryRun 配置，不应把它用于真实运动。
## 拖动示教轨迹 ## 拖动示教轨迹
录制 RM75 末端六维力位置+姿态拖动示教，同步保存 CSV： `DragTeachService` 提供以下状态机能力：
- `StartRecording` / `StopRecording`；
- `SaveRawTrajectory`；
- `MoveToTrajectoryOrigin`；
- `ReplayLastDragTrajectory`；
- `PauseReplay` / `ContinueReplay` / `StopReplay`。
默认命令只执行 DryRun 配置检查，不连接设备：
```bash ```bash
./RM/build/rm_drag_teach_record demo_pick_path 15 100 ./RM/build/rm_drag_teach_record demo_pick_path 15 100
``` ```
默认写入： 真实拖动必须显式增加 `--live`，并同时满足安全配置、RM75/O6 实际连接、急停、
控制器七轴限制、虚拟墙、自碰撞、力/力矩阈值、人工二次确认和轨迹校验。当前 RM 独立
入口尚未接入 O6 设备状态服务，因此即使传入 `--live` 也会 fail-closed；不得用命令行
布尔值伪造 O6 已连接。完整服务流程由 `MockRobotArm` 无硬件测试覆盖。
每次成功保存都在构建目录下创建新的唯一会话，例如：
```text ```text
RM/data/trajectories/csv/demo_pick_path.csv RM/build/runtime_data/trajectories/sessions/<清理后的名称>_<UTC毫秒>/
RM/data/trajectories/vendor/demo_pick_path_vendor.txt ├── raw_trajectory.txt
├── synchronized_samples.csv
└── manifest.json
``` ```
把 CSV 转成 C++ TCP waypoint 表： 同步 CSV 使用 `std::chrono::steady_clock`，字段包含连续 `sample_index`、`timestamp_ms`、
`j1_deg..j7_deg`、TCP 位姿和 `source`。manifest 记录 RM75-6F 型号、SDK/固件版本、
保存时间、厂商点数、同步样本数、原始文件大小、SHA-256 和保存接口实际返回码。
厂商原始文件固化后移除写权限，既有会话和 manifest 均拒绝覆盖。
厂商原生回放接口没有速度参数，因此仅勾选“接受风险”仍不足以授权：调用方还必须
提供独立验证，证明实际回放速度不超过安全配置；缺少该事实时服务会拒绝 Replay/Continue。
## RM75 本地轨迹与 O6 夹握联动
`rm_trajectory_o6_grasp` 专用于以下受控流程：只读加载 RM75 厂商 JSONL/TXT 轨迹、严格解析
七个关节、生成推导时间戳并离线校验，随后在 Live 授权下低速移动到首点，并通过
`rm_send_project(project_type=1, only_save=0)` 交给控制器保存并执行。只有观察到指定程序
先进入运行态、再正常结束，并重新校验机械臂状态、六维力和 O6 readiness 后，程序才会
按照“转矩 FC16、速度 FC16、最后位置 FC16”的顺序发出六关节抓握命令，并通过
FC04 实际位置确认最终抓握姿态。
业务层不包含 RM_API2，O6 协议也不直接访问厂商 SDK。
先执行不会连接设备的 DryRun。`--sample-period-ms` 必须填写这份原始轨迹实际采集周期，
不能猜测；以下握姿数值只用于演示离线参数校验，不代表现场安全姿态：
```bash ```bash
./RM/build/rm_trajectory_convert RM/data/trajectories/csv/demo_pick_path.csv ./RM/build/rm_trajectory_o6_grasp \
RM/data/trajectories/sourceTXT/123.txt \
--sample-period-ms 100 \
--grip-pose 140,100,55,55,55,55 \
--grip-speed 0,0,0,0,0,0 \
--grip-torque 0,0,0,0,0,0 \
--safety-config RM/config/drag_teach_safety.json
``` ```
默认输出： DryRun 会输出轨迹点数、时长、文件字节数、SHA-256、七轴首末点和校验峰值，但不会连接
RM75 或 O6。默认安全配置把 O6 速度和转矩上限锁为 0，所以上述命令只能用于验证离线
调用链，不能作为真实抓握参数。真实运行前必须由现场人员确认采样周期、厂商整数单位比例
以及六个位置/速度/转矩值，并在安全配置中逐项审核：
```text - `hardware_execution_enabled`、急停确认、安全限制和工作空间确认；
RM/data/trajectories/converted/demo_pick_path_tcp_path.cpp - `trajectory_sample_period_confirmed` 与 `vendor_raw_units_per_degree_confirmed`；
- `vendor_raw_units_per_degree`，用于把 TXT 中的厂商整数显式转换为 degree；
- `max_speed_percent` 与命令行的首点/轨迹速度；
- `maximum_o6_speed_command` 与 `maximum_o6_torque_command`；
- `maximum_origin_move_delta_deg`，用于限制当前位置到轨迹首点的每轴最大距离；
- 七轴跃变、速度、加速度以及六维力/力矩上限。
Live 还会要求源轨迹是不可写的普通文件，并在移动前和上传前复核文件大小与 SHA-256。
Linux 现场确认文件内容无误后可显式移除写权限：
```bash
chmod a-w RM/data/trajectories/sourceTXT/123.txt
``` ```
维护边界： `--program-id` 必须是控制器中尚未占用的编号；程序会在首点运动前查询，并在上传前再次
查询，拒绝覆盖已有控制器工程。程序不会自动删除控制器中执行后保存的工程。
- `drivers/realman` 只封装 RM SDK 调用。 O6 readiness 会显式初始化工具端口 1、Modbus RTU 主站、115200 baud、24V 和有效超时，
- `core/drag_teach_recorder` 负责编排拖动示教、采样、停止和保存。 再验证右手 `0x27/'R'`（或左手 `0x28/'L'`）、六自由度、0..255 的位置/转矩/速度、温度
- `modules/trajectory` 负责目录、CSV 和 C++ 文件转换，不访问硬件。 不高于 70 摄氏度且六个故障码均为 0。RM_API2 v1.1.5 的多输入寄存器路径在当前控制器上
返回 `-2`，因此 O6 状态采用 FC04 单寄存器顺序读取；通信或写入失败后不自动重试。
## 后续换机械臂怎么改 只演示 O6 张开再抓握、完全不移动 RM75 时，可人工运行：
不要改 `JsonCommandRunner` 和 JSON 格式。新增一个驱动： ```bash
./RM/build/rm_o6_grip_demo --live-grip
```
```text 演示程序保持 O6 供电，不触发上电标定；先验证安全张开，再以低速、低转矩抓握并逐项
include/rm_control/drivers/new_vendor/new_vendor_robot_arm.h 读回位置。当前现场食指位置保持在已弯曲的 `0`，其余五个关节执行可见往返。
src/drivers/new_vendor/new_vendor_robot_arm.cpp
只有上述现场条件均已确认后，操作者才可手工增加 `--live`：
```bash
./RM/build/rm_trajectory_o6_grasp \
RM/data/trajectories/sourceTXT/123.txt \
--sample-period-ms <已确认的实际采样周期毫秒> \
--origin-speed-percent 5 \
--plan-speed-percent 5 \
--program-id 1 \
--o6-side right \
--grip-pose <拇指弯曲>,<拇指横摆>,<食指>,<中指>,<无名指>,<小指> \
--grip-speed <拇指弯曲>,<拇指横摆>,<食指>,<中指>,<无名指>,<小指> \
--grip-torque <拇指弯曲>,<拇指横摆>,<食指>,<中指>,<无名指>,<小指> \
--safety-config RM/config/drag_teach_safety.json \
--live
``` ```
新驱动实现 `IRobotArm`： Live 仍会在运动前显示文件校验值、首点、O6 身份和当前状态，并要求键盘二次确认；任一
设备读取、轨迹校验、首点距离、控制器运行状态或握姿读回不满足条件即 fail-closed。
SIGINT/SIGTERM 会请求 RM75 慢停，并阻止后续控制器上传或 O6 抓握。成功任务会在
`RM/build/runtime_data/trajectory_playback_reports/` 新建不可覆盖 JSON 报告；其中周期和
抖动字段只表示主机对控制器程序状态的轮询，不代表控制器内部逐点下发周期。
硬件程序只允许人工运行，自动化测试只使用 Mock。
```cpp 把 CSV 转成 C++ TCP waypoint 表：
class NewVendorRobotArm final : public rm_control::interfaces::IRobotArm {
// connect / getCurrentArmState / moveJ / moveL ... ```bash
}; ./RM/build/rm_trajectory_convert \
RM/build/runtime_data/trajectories/sessions/SESSION_ID/synchronized_samples.csv
``` ```
然后在 `src/main.cpp` 里把 `RealManRobotArm` 换成新驱动即可。上层 JSON 指令、解析、校验和执行流程不需要重写。 默认输出：
```text
RM/build/runtime_data/trajectories/converted/demo_pick_path_tcp_path.cpp
```
维护边界：
- `src/realman/realman_arm_adapter.cpp` 是 RM SDK 调用的唯一新实现边界。
- `core/drag_teach_service` 负责状态机、安全门控、采样、停止、校验和保存编排。
- `modules/trajectory` 负责唯一目录、CSV、SHA-256、manifest 和转换，不访问硬件。
## 后续维护边界
当前不要为其他机械臂品牌新增 `drivers/<vendor>/`，也不要把 JSON runner 改成多品牌选择器。后续如果继续做机械臂与灵心巧手联动，应优先围绕 RM75 和 O6 的安全 dry-run、只读状态检查、低速小步运动、日志和人工确认流程扩展。
RM/config/drag_teach_safety.json 0 → 100644
View file @ 8275ee2e
{
"hardware_execution_enabled": false,
"emergency_stop_verified": false,
"safety_limits_loaded": false,
"workspace_and_virtual_wall_verified": false,
"native_replay_without_speed_control_accepted": false,
"trajectory_sample_period_confirmed": false,
"vendor_raw_units_per_degree_confirmed": false,
"max_speed_percent": 10,
"vendor_raw_units_per_degree": 1000,
"maximum_origin_move_delta_deg": 30.0,
"max_joint_jump_deg": 5.0,
"max_joint_velocity_deg_per_second": 30.0,
"max_joint_acceleration_deg_per_second_squared": 100.0,
"maximum_absolute_force_newton": 100.0,
"maximum_absolute_torque_newton_meter": 20.0,
"maximum_o6_speed_command": 0,
"maximum_o6_torque_command": 0
}
RM/data/trajectories/sourceTXT/123.txt 0 → 100644
View file @ 8275ee2e
This source diff could not be displayed because it is too large. You can view the blob instead.
RM/include/rm_control/common/rm75_constants.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include <cstddef>
namespace rm_control::common {
// RM75-6F 固定为七自由度；所有 RM75 关节数据都以该常量为唯一长度来源。
inline constexpr std::size_t kRm75JointCount = 7;
} // namespace rm_control::common
RM/include/rm_control/common/robot_types.h
View file @ 8275ee2e
#pragma once #pragma once
#include "rm_control/common/rm75_constants.h"
#include <array> #include <array>
#include <filesystem>
#include <string> #include <string>
#include <vector> #include <vector>
...@@ -31,6 +34,26 @@ struct RobotInfo { ...@@ -31,6 +34,26 @@ struct RobotInfo {
int arm_model = 0; int arm_model = 0;
int force_type = 0; int force_type = 0;
int controller_version = 0; int controller_version = 0;
bool is_rm75 = false;
bool six_axis_force_sensor_available = false;
};
// SDK 与控制器软件信息。firmware_version 明确映射 RM_API2 的 ctrl_info.version，
// 其余分层版本单独保留，避免把不同固件组件误合并成一个版本号。
struct RobotSoftwareInfo {
std::string sdk_version;
std::string product_version;
std::string controller_generation;
std::string firmware_version;
std::string firmware_build_time;
std::string plan_version;
std::string plan_build_time;
std::string algorithm_version;
std::string dynamics_version;
std::string communication_version;
std::string communication_build_time;
std::string program_version;
std::string program_build_time;
}; };
// 当前机械臂状态：关节角、TCP 位姿、错误码。 // 当前机械臂状态：关节角、TCP 位姿、错误码。
...@@ -50,26 +73,92 @@ struct ForceData { ...@@ -50,26 +73,92 @@ struct ForceData {
std::array<float, 6> tool_zero{}; std::array<float, 6> tool_zero{};
}; };
// 安全门控需要的只读机械臂状态。RM_API2 v1.1.5 没有独立的急停查询接口，
// 因此适配器必须保持 emergency_stop_state_available=false，不能根据错误码猜测急停正常。
struct RobotSafetyStatus {
bool real_mode_enabled = false;
bool arm_power_on = false;
bool all_joints_enabled = false;
bool arm_error_free = false;
bool controller_error_free = false;
bool virtual_wall_enabled = false;
bool self_collision_enabled = false;
bool emergency_stop_state_available = false;
bool emergency_stop_normal = false;
int controller_error_code = 0;
float controller_voltage = 0.0F;
float controller_current = 0.0F;
float controller_temperature = 0.0F;
std::array<int, kRm75JointCount> joint_enable_states{};
std::array<int, kRm75JointCount> joint_error_codes{};
std::vector<int> arm_error_codes;
};
// 控制器当前生效的七轴位置、速度和加速度限制。位置为 degree，速度为
// degree/second。RM_API2 v1.1.5 的 get-max-acc 注释误写为 degree/second，
// 而对应 setter 写为 degree/second^2；本工程按函数“加速度”语义显式使用后者。
struct RobotJointLimits {
std::array<float, kRm75JointCount> minimum_position_deg{};
std::array<float, kRm75JointCount> maximum_position_deg{};
std::array<float, kRm75JointCount> maximum_speed_deg_per_second{};
std::array<float, kRm75JointCount> maximum_acceleration_deg_per_second_squared{};
};
enum class DragTeachMode { enum class DragTeachMode {
OrdinaryRecorded, OrdinaryRecorded,
SixDofForcePositionAndOrientation, SixDofForcePositionAndOrientation,
}; };
struct DragTeachStartOptions { struct DragTeachStartOptions {
DragTeachMode mode = DragTeachMode::SixDofForcePositionAndOrientation; DragTeachMode mode = DragTeachMode::OrdinaryRecorded;
bool record_vendor_trajectory = true; bool record_vendor_trajectory = true;
bool enable_singular_wall = true; bool enable_singular_wall = true;
}; };
struct SavedVendorTrajectory { // 未实际调用厂商保存接口时不能把默认值 0 误记为成功。
std::string path; inline constexpr int kVendorApiNotCalledReturnCode = -10000;
// 一次 rm_save_trajectory 调用的完整结果。SDK 返回非零时也必须保留返回码、
// 控制器可能写入的点数和目标路径，供 manifest 与故障报告使用。
struct VendorTrajectorySaveAttempt {
std::filesystem::path path;
bool api_called = false;
int return_code = kVendorApiNotCalledReturnCode;
int point_count = 0; int point_count = 0;
std::string error_message;
};
// 上传到控制器保存或兼容性上传即执行的厂商拖动轨迹工程参数。路径仍使用工程统一的
// std::filesystem::path；具体保存/执行语义由调用的 IRobotArm 方法明确表达。
struct VendorDragTrajectoryExecutionRequest {
std::filesystem::path trajectory_path;
int plan_speed_percent = 0;
int controller_program_id = 0;
};
// 运行控制器中已保存程序的参数。速度必须由调用方显式给出 1..100，禁止传 0
// 隐式沿用控制器中的历史速度；blocking=false 可用于状态机非阻塞监控。
struct ControllerProgramRunRequest {
int controller_program_id = 0;
int speed_percent = 0;
bool blocking = false;
};
// 控制器在线编程运行状态。raw_state 保留 SDK 原值，避免上层把未知状态
// 错当成已完成；当前 SDK 已知 0=停止/结束、1=运行、2=暂停。
struct ControllerProgramRunState {
int raw_state = -1;
int program_id = 0;
int plan_number = 0;
int plan_speed_percent = 0;
}; };
struct TrajectorySample { struct TrajectorySample {
long long elapsed_ms = 0; std::size_t sample_index = 0; // 从 0 连续递增。
std::vector<float> joints_deg; long long elapsed_ms = 0; // steady_clock 相对录制起点，单位 millisecond。
Pose tcp_pose; std::vector<float> joints_deg; // RM75 固定七关节，单位 degree。
Pose tcp_pose; // 位置 meter，欧拉姿态 radian。
std::string source = "realman_rm75_drag_teach";
}; };
} // namespace rm_control::common } // namespace rm_control::common
RM/include/rm_control/config/drag_teach_cli_config.h 0 → 100644
View file @ 8275ee2e
/**
* @file drag_teach_cli_config.h
* @brief RM75 拖动示教录制程序命令行参数解析接口。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件声明 `rm_drag_teach_record` 的 CLI 配置结构和解析函数。拖动示教会访问
* 真实机械臂，因此入口文件必须先得到清晰、可验证的参数配置，再显式建立硬件连接。
*
* 设计原则：
* - 参数解析不连接机械臂；
* - 参数解析不启动拖动示教；
* - 默认使用普通记录拖动模式，确保厂商保存接口有明确记录契约；
* - 保持原有位置参数和选项语义不变。
*
* @dependencies
* - rm_control/common/result.h
* - rm_control/core/drag_teach_recorder.h
*
* @note
* 后续增加人工确认、dry-run 或安全开关时，应优先扩展这里的配置结构。
*/
#pragma once
#include "rm_control/common/result.h"
#include "rm_control/core/drag_teach_recorder.h"
#include <filesystem>
#include <iosfwd>
namespace rm_control::config {
/**
* @brief 拖动示教录制程序的命令行解析结果。
*
* @reason 将危险的命令行授权与静态安全配置路径显式分离，确保入口层可以在连接前拒绝执行。
*
* @author 唐永康
* @date 2026-07-10
*
* @details
* 该结构体把“是否请求帮助”和“录制配置”放在一起返回给入口文件。
* `record_config` 是 core 层真正需要的业务配置，CLI 层不会直接调用硬件。
*/
struct DragTeachRecordCliConfig {
bool help_requested = false; ///< 是否请求帮助信息。
bool live_requested = false; ///< 是否显式请求真实硬件模式。
std::filesystem::path safety_config_path; ///< 静态安全配置文件路径。
core::DragTeachRecordConfig record_config; ///< 拖动示教录制配置。
};
/**
* @brief 打印拖动示教录制程序的命令行使用说明。
*
* @reason 集中维护安全选项说明，避免帮助文本与真实门控条件发生漂移。
*
* @author 唐永康
* @date 2026-07-10
*
* @details
* 该函数集中维护 usage 文本，避免 `main.cpp`、错误分支和测试中出现多份不一致文案。
* 它不访问真实机械臂、不创建文件、不启动拖动示教。
*
* @param[in] program_name 当前进程名，可以为空指针。
* @param[out] out 使用说明输出流。
*
* @return 无返回值。
*
* @calls 仅调用标准输出流，不访问配置文件或设备。
* @safety 本函数不连接硬件、不触发运动。
*
* @throws 本函数不主动抛出异常；输出流异常由调用方处理。
*
* @note
* 帮助文本必须强调拖动示教会访问真实硬件，后续维护时不能删除安全提示。
*
* @warning
* 如果解析规则变化，必须同步更新此函数文本。
*
* @thread_safety
* 函数本身无全局状态；并发写同一输出流需要调用方保护。
*
* @par 边界条件
* `program_name == nullptr` 时使用默认程序名。
*
* @par 后续维护
* 新增选项时应说明默认值和安全影响。
*/
void PrintDragTeachRecordUsage(const char *program_name, std::ostream &out);
/**
* @brief 解析拖动示教录制程序的命令行参数。
*
* @reason 在任何连接动作前解析 `--live` 和安全配置路径，使错误参数保持 fail-closed。
*
* @author 唐永康
* @date 2026-07-10
*
* @details
* 该函数负责解析 `session_name`、录制时长、采样周期、存储根目录以及拖动示教模式选项。
* 它只返回结构化配置，不连接 SDK、不移动机械臂，因此可以被单元测试直接覆盖。
*
* @param[in] argc 命令行参数数量。
* @param[in] argv 命令行参数数组，允许元素为空指针。
*
* @return 成功时返回 `DragTeachRecordCliConfig`。
* @return 失败时返回错误原因，例如未知选项、整数解析失败或位置参数过多。
*
* @calls 不调用厂商接口；只处理命令行文本。
* @safety 未显式传入 `--live` 时 `live_requested` 永远为 false。
*
* @throws 本函数不主动抛出异常；所有解析失败通过 `Result` 返回。
*
* @note
* - 默认 session name 使用时间戳生成。
* - 默认存储目录来自 trajectory 模块。
* - 默认启用厂商轨迹保存。
*
* @warning
* 本函数不校验机械臂连接状态；真实硬件状态由 driver/core 层检查。
*
* @thread_safety
* 函数只使用局部对象和只读输入，不依赖全局可变状态，线程安全。
*
* @par 边界条件
* 空字符串、未知 `-` 开头选项、过多位置参数都会返回失败。
*
* @par 后续维护
* 新增运动相关参数时必须保持“默认不绕过安全检查”的原则。
*/
common::Result<DragTeachRecordCliConfig> ParseDragTeachRecordCliArguments(int argc, char **argv);
} // namespace rm_control::config
RM/include/rm_control/config/drag_teach_safety_config.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include <cstdint>
#include <filesystem>
namespace rm_control::config {
/**
* @brief 拖动示教真实硬件执行所需的静态安全配置。
*
* @reason 将可审计的配置条件与命令行和运行时设备状态分离，避免单个输入源绕过安全门控。
*
* @author 唐永康
* @date 2026-07-11
*
* @note 所有布尔开关默认关闭，O6 速度/转矩上限默认 0；人工确认和设备连接状态
* 不允许从本结构伪造。
*/
struct DragTeachSafetyConfig {
bool hardware_execution_enabled = false;
bool emergency_stop_verified = false;
bool safety_limits_loaded = false;
bool workspace_and_virtual_wall_verified = false;
bool native_replay_without_speed_control_accepted = false;
bool trajectory_sample_period_confirmed = false;
bool vendor_raw_units_per_degree_confirmed = false;
int max_speed_percent = 10; // 1..100。
int vendor_raw_units_per_degree = 1000;
float maximum_origin_move_delta_deg = 30.0F;
float max_joint_jump_deg = 5.0F;
float max_joint_velocity_deg_per_second = 30.0F;
float max_joint_acceleration_deg_per_second_squared = 100.0F;
float maximum_absolute_force_newton = 100.0F;
float maximum_absolute_torque_newton_meter = 20.0F;
std::uint16_t maximum_o6_speed_command = 0U;
std::uint16_t maximum_o6_torque_command = 0U;
};
/**
* @brief 从 JSON 文件加载并校验拖动示教安全配置。
*
* @reason 真实硬件开关必须来自显式配置文件，并在连接设备前完成类型和值域校验。
*
* @author 唐永康
* @date 2026-07-11
*
* @param config_path 安全配置文件路径；调用方保留所有权。
* @return 成功时返回完整配置；文件、JSON、字段或数值非法时返回带路径上下文的失败。
*
* @calls rm_control::config::parseJson。
* @safety 本函数不连接设备、不触发运动；失败时保持硬件执行禁用。
* @note 配置缺少任一必填字段时拒绝加载，不使用隐式危险默认值。
*/
common::Result<DragTeachSafetyConfig>
LoadDragTeachSafetyConfig(const std::filesystem::path &config_path);
} // namespace rm_control::config
RM/include/rm_control/config/json_runner_cli_config.h 0 → 100644
View file @ 8275ee2e
/**
* @file json_runner_cli_config.h
* @brief RM JSON 指令运行器命令行参数解析接口。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件只声明 `rm_json_runner` 可执行程序的命令行参数结构和解析函数。
* 设计上把命令行解析从 `main.cpp` 中移出，是为了让入口文件只负责装配依赖
* 和启动应用流程，同时让参数解析逻辑可以在不连接真实机械臂的情况下单元测试。
*
* 设计原则：
* - 只处理命令行文本，不读取 JSON 文件；
* - 只返回结构化配置和错误原因，不直接退出进程；
* - 不依赖 RealMan RM_API2，避免 CLI 单元测试访问真实硬件；
* - 保持现有 `rm_json_runner <commands.json> [--validate-only]` 运行方式不变。
*
* @dependencies
* - rm_control/common/result.h
*
* @note
* 后续如果新增 `rm_json_runner` 命令行选项，应优先扩展本文件中的配置结构，
* 不应把解析逻辑重新写回 `RM/src/main.cpp`。
*/
#pragma once
#include "rm_control/common/result.h"
#include <iosfwd>
#include <string>
namespace rm_control::config {
/**
* @brief `rm_json_runner` 命令行解析后的结构化配置。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该结构体只表达启动参数层面的意图，不保存已经解析好的 JSON 指令内容。
* 这样 `main.cpp` 可以先处理帮助信息，再把 `command_file_path` 交给 JSON
* 配置模块读取，避免 CLI 层和 JSON 文件解析层相互耦合。
*
* @note
* `help_requested=true` 时，调用方应打印 usage 并返回成功退出码。
*/
struct JsonRunnerCliConfig {
bool help_requested = false; ///< 是否请求帮助信息。
bool validate_only = false; ///< 是否只校验 JSON 文件，不连接 SDK。
std::string command_file_path; ///< JSON 指令文件路径。
};
/**
* @brief 打印 `rm_json_runner` 的命令行使用说明。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数只负责把稳定的 usage 文本写入输出流，不解析参数、不访问文件、
* 不连接真实机械臂。之所以单独封装，是为了让 `main.cpp` 在错误和帮助场景
* 复用同一份提示文本，避免不同分支输出不一致。
*
* @param[in] program_name 当前进程名，可以为空指针；为空时使用固定占位名称。
* @param[out] out 使用说明输出流，通常是 `std::cout` 或测试中的字符串流。
*
* @return 无返回值。
*
* @throws 本函数不主动抛出异常；如果输出流自身异常，由调用方处理。
*
* @note
* - 不读取环境变量。
* - 不修改全局状态。
*
* @warning
* 如果后续新增命令行选项，必须同步更新此函数的输出内容。
*
* @thread_safety
* 函数内部不使用全局可变状态；多个线程写同一个输出流时需要调用方加锁。
*
* @par 边界条件
* `program_name == nullptr` 时仍能输出可读 usage。
*
* @par 后续维护
* 保持 usage 和 `ParseJsonRunnerCliArguments()` 的实际解析规则一致。
*/
void PrintJsonRunnerUsage(const char *program_name, std::ostream &out);
/**
* @brief 解析 `rm_json_runner` 的命令行参数。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数把 `argc/argv` 转换为 `JsonRunnerCliConfig`。它只做参数数量、
* 帮助选项和 `--validate-only` 选项校验，不读取 JSON 文件，也不访问真实机械臂。
* 这样设计可以保证命令行解析逻辑可独立测试，并让 `main.cpp` 保持简洁。
*
* @param[in] argc 命令行参数数量，来自 `main()`。
* @param[in] argv 命令行参数数组，来自 `main()`，允许数组元素为空指针。
*
* @return 成功时返回解析后的 `JsonRunnerCliConfig`。
* @return 失败时返回错误原因，例如参数数量错误或未知选项。
*
* @throws 本函数不主动抛出异常；内部通过 `Result` 返回错误。
*
* @note
* - `-h` 和 `--help` 只允许作为第一个业务参数。
* - `--validate-only` 只允许作为第二个业务参数。
*
* @warning
* 本函数不判断 JSON 文件是否存在，文件存在性由配置加载层负责。
*
* @thread_safety
* 函数只读取输入参数并返回局部对象，不使用全局可变状态，线程安全。
*
* @par 边界条件
* `argv[1] == nullptr` 或空路径会返回失败，避免后续文件读取错误不明确。
*
* @par 后续维护
* 新增选项时应保持向后兼容，不破坏现有命令格式。
*/
common::Result<JsonRunnerCliConfig> ParseJsonRunnerCliArguments(int argc, char **argv);
} // namespace rm_control::config
RM/include/rm_control/config/trajectory_convert_cli_config.h 0 → 100644
View file @ 8275ee2e
/**
* @file trajectory_convert_cli_config.h
* @brief RM75 轨迹 CSV 转 C++ waypoint 程序命令行参数解析接口。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件声明 `rm_trajectory_convert` 的 CLI 参数解析接口。该工具只读取 CSV 并写出
* C++ waypoint 文件，不连接真实机械臂。将参数解析单独封装后，入口文件只负责
* 调用读取、转换和写出流程。
*
* @dependencies
* - rm_control/common/result.h
*
* @note
* 轨迹转换工具属于离线工具，任何后续改动都不应引入真实硬件依赖。
*/
#pragma once
#include "rm_control/common/result.h"
#include <iosfwd>
#include <string>
namespace rm_control::config {
/**
* @brief 轨迹转换程序的命令行解析结果。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该结构体保存输入 CSV、输出 C++ 文件路径和从输入文件名推导出的 session name。
* session name 用于生成稳定的 C++ 变量名，输出路径可以由用户显式指定或按默认规则生成。
*/
struct TrajectoryConvertCliConfig {
bool help_requested = false; ///< 是否请求帮助信息。
std::string input_csv_path; ///< 输入轨迹 CSV 文件路径。
std::string output_cpp_path; ///< 输出 C++ waypoint 文件路径。
std::string session_name; ///< 用于生成 C++ 标识符的会话名。
};
/**
* @brief 打印轨迹转换程序的命令行使用说明。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数只输出 usage 文本，不读取 CSV、不创建输出文件。独立函数可以避免多个错误分支
* 各自维护不同提示内容。
*
* @param[in] program_name 当前进程名，可以为空指针。
* @param[out] out 使用说明输出流。
*
* @return 无返回值。
*
* @throws 本函数不主动抛出异常；输出流异常由调用方处理。
*
* @note
* usage 中必须说明该工具不连接真实机器人。
*
* @warning
* 命令行格式变化时必须同步更新此函数。
*
* @thread_safety
* 函数无全局状态；并发写同一输出流时由调用方加锁。
*
* @par 边界条件
* `program_name == nullptr` 时使用默认程序名。
*
* @par 后续维护
* 保持提示文本和解析逻辑一致。
*/
void PrintTrajectoryConvertUsage(const char *program_name, std::ostream &out);
/**
* @brief 解析轨迹转换程序的命令行参数。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数解析 `<input_csv> [output_cpp]`。如果用户没有提供输出路径，则基于输入文件名
* 和默认轨迹目录生成输出 C++ 文件路径。函数不检查输入文件是否存在，文件读取模块会在
* 实际加载时返回明确错误。
*
* @param[in] argc 命令行参数数量。
* @param[in] argv 命令行参数数组，允许元素为空指针。
*
* @return 成功时返回 `TrajectoryConvertCliConfig`。
* @return 失败时返回错误原因，例如参数数量错误或输入路径为空。
*
* @throws 本函数不主动抛出异常；所有错误通过 `Result` 返回。
*
* @note
* `-h` 和 `--help` 只作为第一个业务参数处理。
*
* @warning
* 本函数只处理路径字符串，不保证路径权限或磁盘空间。
*
* @thread_safety
* 函数只使用局部变量和只读输入，线程安全。
*
* @par 边界条件
* 输入路径为空或 `argv[1] == nullptr` 时返回失败。
*
* @par 后续维护
* 如果未来支持更多输出格式，应把格式选项加入该配置结构，而不是在入口文件临时判断。
*/
common::Result<TrajectoryConvertCliConfig> ParseTrajectoryConvertCliArguments(int argc, char **argv);
} // namespace rm_control::config
RM/include/rm_control/config/trajectory_o6_task_cli_config.h 0 → 100644
View file @ 8275ee2e
/**
* @file trajectory_o6_task_cli_config.h
* @brief RM75 轨迹与 LinkerHand O6 抓握任务命令行参数解析接口。
*
* @author 唐永康
* @date 2026-07-11
*
* @details
* 本文件只声明集成任务入口所需的结构化参数。参数解析与硬件连接、轨迹加载、
* O6 通信和运动执行完全分离，因此可以在不连接任何设备的单元测试中验证。
*
* @dependencies
* - rm_control/common/result.h
*
* @note
* 默认运行模式为 DryRun。`--live` 只表达操作者意图，不能单独授权真实运动。
*/
#pragma once
#include "rm_control/common/result.h"
#include <array>
#include <cstddef>
#include <cstdint>
#include <filesystem>
#include <iosfwd>
namespace rm_control::config {
inline constexpr std::size_t kLinkerHandO6JointCount = 6;
/**
* @brief LinkerHand O6 在 RM75 工具 RS-485 总线上的设备侧别。
*
* @reason 将命令行中的 `right|left` 映射集中为强类型，避免入口层散落设备地址魔法数字。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety 该枚举只保存地址，不访问总线，也不触发手部动作。
* @note 当前工程约定右手地址为 0x27，左手地址为 0x28。
*/
enum class O6HandSide : std::uint8_t {
Right = 0x27,
Left = 0x28,
};
/**
* @brief RM75 轨迹与 O6 抓握集成任务的命令行解析结果。
*
* @reason 把轨迹来源、节拍、双程序槽、低速参数、O6 姿态、速度、转矩和真实运动意图
* 集中传给应用层，使入口无需重新解释字符串，也不会因危险默认值进入执行流程。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety `live_requested` 默认 false；后续硬件执行仍必须通过完整安全门控。
* @note `grip_pose`、`grip_speed` 和 `grip_torque` 均为必填的 O6 六关节协议值，
* 每个值的闭区间为 0..255；解析器不会为这些动作参数提供可执行默认值。
*/
struct TrajectoryO6TaskCliConfig {
bool help_requested = false; ///< 是否仅请求帮助文本。
bool live_requested = false; ///< 是否显式传入 `--live`，默认 DryRun。
std::filesystem::path trajectory_path; ///< 只读厂商轨迹源文件路径。
int sample_period_ms = 0; ///< 轨迹采样周期，单位毫秒，范围 1..1000。
int origin_speed_percent = 5; ///< 回到轨迹原点速度比例，范围 1..10。
int plan_speed_percent = 5; ///< 轨迹规划速度比例，范围 1..10。
int program_id = 1; ///< 厂商轨迹规划程序编号，范围 1..100。
bool return_from_trajectory_end = false; ///< 是否先从原轨迹末点沿反向轨迹返回首点。
int return_program_id = 0; ///< 反向返回程序编号；0 表示返回模式未启用。
O6HandSide o6_side = O6HandSide::Right; ///< O6 侧别，默认右手地址 0x27。
/// O6 六关节抓握姿态。
std::array<std::uint8_t, kLinkerHandO6JointCount> grip_pose{};
/// O6 六关节抓握速度，必须由命令行显式提供。
std::array<std::uint8_t, kLinkerHandO6JointCount> grip_speed{};
/// O6 六关节抓握转矩，必须由命令行显式提供。
std::array<std::uint8_t, kLinkerHandO6JointCount> grip_torque{};
std::filesystem::path safety_config_path; ///< 真实执行所需静态安全配置路径。
};
/**
* @brief 打印 RM75 轨迹与 O6 抓握任务的命令行使用说明。
*
* @reason 集中维护必填参数、默认低速值和真实运动警示，防止入口错误分支的说明漂移。
*
* @author 唐永康
* @date 2026-07-11
*
* @param[in] program_name 当前进程名，可以为空指针或空字符串。
* @param[out] out 接收使用说明的输出流。
* @return 无返回值。
*
* @calls 仅调用标准输出流，不读取轨迹、配置文件或设备状态。
* @safety 本函数不连接 RM75 或 O6，不触发真实硬件动作。
* @note 命令行规则变化时必须同步更新本函数和解析单元测试。
*/
void PrintTrajectoryO6TaskUsage(const char *program_name, std::ostream &out);
/**
* @brief 解析 RM75 轨迹与 O6 抓握任务命令行参数。
*
* @reason 在加载轨迹和连接设备前严格验证全部外部输入，确保缺少必填项、超范围值
* 或非法 O6 姿态、速度、转矩时保持 fail-closed。
*
* @author 唐永康
* @date 2026-07-11
*
* @param[in] argc 命令行参数数量，必须为非负数。
* @param[in] argv 命令行参数数组；`argc > 0` 时不能为空。
* @return 成功时返回完整 `TrajectoryO6TaskCliConfig`；失败时返回带参数上下文的错误信息。
*
* @calls 仅调用标准库字符串、数值和路径构造函数，不调用厂商 SDK。
* @safety 默认 `live_requested=false`；本函数不会执行轨迹、连接设备或写入源轨迹。
* @note `--return-from-trajectory-end` 与 `--return-program-id` 必须成对出现，且返回程序号
* 必须与正向 `--program-id` 不同；其余必填项在 `-h|--help` 模式下豁免。
*/
common::Result<TrajectoryO6TaskCliConfig>
ParseTrajectoryO6TaskCliArguments(int argc, char **argv);
} // namespace rm_control::config
RM/include/rm_control/core/arm_o6_trajectory_task.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include "rm_control/core/hardware_execution_gate.h"
#include "rm_control/interfaces/dexterous_hand.h"
#include "rm_control/interfaces/robot_arm.h"
#include "rm_control/modules/trajectory/drag_trajectory_validator.h"
#include "rm_control/modules/trajectory/vendor_drag_trajectory_parser.h"
#include <chrono>
#include <cstddef>
#include <cstdint>
#include <functional>
namespace rm_control::core {
/**
* @brief RM75 厂商轨迹执行后驱动 O6 握夹的安全参数。
*
* @reason 将运动速度、位置/力限制、O6 速度/转矩上限、停止回调和轮询超时作为显式输入，
* 状态机无需读取配置文件，并可在 Mock 测试中复现每一项拒绝条件。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety 所有运动速度仍受 HardwareExecutionGateInput 中的配置上限约束；默认值不授权 Live。
* @note 机械臂角度单位为 degree，力为 newton，力矩为 newton_meter，时间为 millisecond。
*/
struct ArmO6TrajectoryTaskConfig {
modules::trajectory::DragTrajectoryValidationLimits validation_limits;
int origin_speed_percent = 5;
int plan_speed_percent = 5;
int controller_program_id = 1;
int return_controller_program_id = 0;
interfaces::O6JointPose grip_pose{};
interfaces::O6JointPose grip_speed_commands{};
interfaces::O6JointPose grip_torque_commands{};
std::uint16_t maximum_o6_speed_command = 0U;
std::uint16_t maximum_o6_torque_command = 0U;
double maximum_origin_delta_deg = 20.0;
double origin_position_tolerance_deg = 1.0;
double maximum_absolute_force_newton = 100.0;
double maximum_absolute_torque_newton_meter = 20.0;
std::uint16_t grip_position_tolerance = 5U;
std::chrono::milliseconds origin_status_poll_period{100};
std::chrono::milliseconds origin_motion_timeout{300000};
std::chrono::milliseconds program_status_poll_period{100};
std::chrono::milliseconds program_execution_timeout{300000};
std::chrono::milliseconds hand_status_poll_period{100};
std::chrono::milliseconds hand_motion_timeout{20000};
std::function<bool()> stop_requested;
};
/**
* @brief 纯校验 RM75 轨迹与 O6 抓握任务配置。
*
* @reason DryRun 和 Live 必须复用同一组位置、速度、转矩、配置上限及超时规则，避免
* DryRun 在返回前漏报只有真实状态机才会发现的危险参数。
*
* @author 唐永康
* @date 2026-07-11
*
* @param config 待校验的任务配置；函数只读且不保留引用。
* @return 全部字段符合 RM75/O6 范围和配置上限时成功，否则返回首个上下文错误。
*
* @calls 仅调用标准库数值和定长数组校验函数。
* @safety 纯校验，不读取文件、不连接设备，也不触发机械臂或 O6 动作。
* @note O6 位置、速度和转矩每项均限制为 0..255，速度和转矩还不得超过安全配置上限。
*/
common::Result<void> ValidateArmO6TrajectoryTaskConfig(
const ArmO6TrajectoryTaskConfig &config);
/**
* @brief 在基础任务参数之外校验反向与正向两个控制器程序编号。
* @reason DryRun 必须在连接硬件前拒绝缺失、越界或相同的双程序槽配置。
* @author 唐永康
* @date 2026-07-11
* @param config 包含正向和反向程序编号的任务配置。
* @return 基础参数和两个互异的 1..100 程序编号均有效时成功。
* @calls 仅调用纯参数校验，不访问文件或硬件。
* @safety 纯校验，不触发运动。
*/
common::Result<void> ValidateArmO6RoundTripTaskConfig(
const ArmO6TrajectoryTaskConfig &config);
/**
* @brief 控制器轨迹状态轮询的单调时钟统计。
*
* @reason 将实际轮询周期、抖动和观察时长写入成功报告，便于离线判断主机监视是否按
* 配置节拍工作；这些统计不冒充控制器内部真实轨迹下发周期。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety 仅包含 `std::chrono::steady_clock` 的观测结果，不触发任何硬件动作。
* @note 除 poll_count 外的字段单位均为 millisecond；少于两次轮询时周期统计为零。
*/
struct ControllerProgramTimingReport {
std::size_t poll_count = 0U;
double minimum_poll_period_ms = 0.0;
double maximum_poll_period_ms = 0.0;
double average_poll_period_ms = 0.0;
double maximum_absolute_poll_jitter_ms = 0.0;
double observed_execution_duration_ms = 0.0;
};
/**
* @brief 一次 RM75 轨迹与 O6 握夹任务的可审计结果。
*
* @reason 只在完整状态机成功后返回轨迹统计、控制器终态和 O6 读回，避免调用方把
* “上传成功”误记为“机械臂和灵巧手任务完成”。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety 报告不触发动作；只有 `Run` 成功时其中所有字段才有效。
* @note `observed_running_state` 必须为 true，控制器终态必须为 raw_state=0。
*/
struct ArmO6TrajectoryTaskReport {
modules::trajectory::DragTrajectoryValidationReport trajectory_validation;
common::ControllerProgramRunState return_final_program_state;
common::ControllerProgramRunState final_program_state;
interfaces::DexterousHandReadiness final_hand_readiness;
ControllerProgramTimingReport return_controller_timing;
ControllerProgramTimingReport controller_timing;
std::size_t trajectory_point_count = 0U;
bool returned_from_trajectory_end = false;
bool return_observed_running_state = false;
bool observed_running_state = false;
};
/**
* @brief 只读复核已解析厂商轨迹源文件的类型、权限、大小和 SHA-256。
*
* @reason 解析完成到真实上传之间存在文件被替换或篡改的窗口；将完整性检查作为无硬件
* 纯函数，状态机可在首点运动前和上传紧前重复执行同一 fail-closed 规则。
*
* @author 唐永康
* @date 2026-07-10
*
* @param trajectory 解析阶段记录了源路径、字节数和 SHA-256 的只读轨迹元数据。
* @return 文件为非符号链接普通文件、没有任何写权限且大小与摘要均一致时成功，否则失败。
*
* @calls std::filesystem::symlink_status、file_size 和 CalculateFileSha256。
* @safety 仅离线读取文件，不修改权限、内容或时间戳，也不连接或驱动任何硬件。
* @note 该检查不能授权真实动作，仍须与完整 HardwareExecutionGate 和设备复检共同使用。
*/
common::Result<void> VerifySourceFileIntegrity(
const modules::trajectory::VendorDragTrajectoryParseResult &trajectory);
/**
* @brief 以 fail-closed 状态机编排 RM75 本地轨迹执行和 O6 握夹。
*
* @reason 真实机械臂运动与灵巧手命令必须经过同一业务状态机，禁止应用层绕过安全检查
* 直接组合厂商接口。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety 本类会触发真实机械臂和 O6 动作；只能在完整 Live 门控、人工确认和现场安全条件下调用。
* @note 本类不拥有设备对象，也不负责建立或释放连接。
*/
class ArmO6TrajectoryTask final {
public:
/**
* @brief 绑定已连接的机械臂和 O6 抽象接口。
*
* @reason 通过接口注入保持状态机与厂商 SDK 隔离，并允许单元测试完全使用 Mock。
*
* @author 唐永康
* @date 2026-07-11
*
* @param robot RM75 接口引用，必须比本对象存活更久。
* @param hand O6 接口引用，必须比本对象存活更久。
* @return 构造函数无返回值。
*
* @calls 不调用设备接口。
* @safety 仅保存引用，不连接设备，也不触发动作。
* @note 调用方负责保证两个接口对应同一套现场硬件。
*/
ArmO6TrajectoryTask(interfaces::IRobotArm &robot,
interfaces::IDexterousHand &hand) noexcept;
/**
* @brief 校验轨迹与设备后，移动到首点、执行厂商轨迹并命令 O6 握夹。
*
* @reason 用单一入口固定“全量校验→首点→轨迹运行态确认→结束→O6 再确认→握夹读回”顺序，
* 任一未知状态均停止并返回上下文错误。
*
* @author 唐永康
* @date 2026-07-11
*
* @param trajectory 已严格解析且保留只读源路径、七轴样本和推导时间戳的厂商轨迹。
* @param config 速度、轨迹限制、O6 位置/速度/转矩、安全上限、停止回调和轮询超时。
* @param gate_input 已由应用层收集的 Live、安全配置、连接、急停和人工确认事实。
* @return 全流程完成且 O6 位置读回在容差内时返回报告；否则返回失败并在运动开始后尝试慢停止。
*
* @calls ValidateDragTrajectory、RequireHardwareExecutionPermission、IRobotArm 和 IDexterousHand。
* @safety 会触发真实运动；本函数不得用于 DryRun，也不替代现场急停和人工监护。
* @note 原始轨迹只读使用，不修改、不覆盖也不删除源文件。
*/
common::Result<ArmO6TrajectoryTaskReport> Run(
const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
const ArmO6TrajectoryTaskConfig &config,
const HardwareExecutionGateInput &gate_input);
/**
* @brief 从原轨迹末点出发，先执行只读反向产物回首点，再执行原轨迹并抓握。
*
* @reason 当前机械臂位于原轨迹末点时，禁止通过放大首点距离阈值做一次大跨度
* `moveJ`；两个控制器托管程序可以沿同一离散路径安全返回并正向复现。
*
* @author 唐永康
* @date 2026-07-11
*
* @param return_trajectory 已重新解析、校验且逐行逆序生成的只读厂商轨迹。
* @param forward_trajectory 操作者确认的原始只读厂商轨迹。
* @param config 两个互异控制器程序编号、统一低速和 O6 抓握参数。
* @param gate_input 已通过人工确认的完整硬件执行事实。
* @return 两段程序均观察到运行后正常结束且 O6 抓握读回成功时返回报告。
*
* @calls IRobotArm::saveVendorDragTrajectoryProgram、runControllerProgram 和 IDexterousHand。
* @safety 只允许当前位置严格贴近原轨迹末点；任一上传、状态、端点或停止检查失败时
* 禁止后续阶段，并在运动已开始后请求 RM75 慢停。
* @note 两个控制器程序槽均只保存不覆盖，函数不会删除任何已有或新建程序。
*/
common::Result<ArmO6TrajectoryTaskReport> RunFromTrajectoryEnd(
const modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
const modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory,
const ArmO6TrajectoryTaskConfig &config,
const HardwareExecutionGateInput &gate_input);
private:
interfaces::IRobotArm &robot_;
interfaces::IDexterousHand &hand_;
};
} // namespace rm_control::core
RM/include/rm_control/core/drag_teach_recorder.h
View file @ 8275ee2e
/**
* @file drag_teach_recorder.h
* @brief RM75 拖动示教轨迹录制器接口。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件声明拖动示教录制器。录制器属于 core 层，负责组织拖动示教生命周期：
* 校验配置、创建轨迹文件路径、启动拖动、周期采样、停止拖动、写 CSV 和可选保存厂商轨迹。
* 真实 SDK 调用全部通过 `IRobotArm` 抽象接口完成。
*
* 设计原则：
* - core 不直接包含 RealMan SDK 头文件；
* - 采样数据写入 trajectory 模块，不在 core 中处理 CSV 细节；
* - 拖动示教启动后，无论采样成功或失败都必须尝试停止；
* - 配置错误必须在连接硬件动作前尽早返回。
*
* @dependencies
* - rm_control/common/result.h
* - rm_control/common/robot_types.h
* - rm_control/interfaces/robot_arm.h
* - rm_control/modules/trajectory/trajectory_file_store.h
*/
#pragma once #pragma once
#include "rm_control/common/result.h" #include "rm_control/common/result.h"
...@@ -7,9 +32,21 @@ ...@@ -7,9 +32,21 @@
#include <iosfwd> #include <iosfwd>
#include <string> #include <string>
#include <vector>
namespace rm_control::core { namespace rm_control::core {
/**
* @brief 拖动示教录制配置。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该结构体保存一次拖动示教录制所需的业务参数。它不保存硬件连接参数，
* 因为连接生命周期由入口和 driver 管理。
*/
struct DragTeachRecordConfig { struct DragTeachRecordConfig {
std::string storage_root; std::string storage_root;
std::string session_name; std::string session_name;
...@@ -19,6 +56,17 @@ struct DragTeachRecordConfig { ...@@ -19,6 +56,17 @@ struct DragTeachRecordConfig {
common::DragTeachStartOptions drag_options; common::DragTeachStartOptions drag_options;
}; };
/**
* @brief 拖动示教录制结果摘要。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该结构体向入口层返回录制产物路径、采样数量和厂商轨迹保存结果。入口层只负责打印摘要，
* 不需要理解 CSV 写入或厂商轨迹保存细节。
*/
struct DragTeachRecordSummary { struct DragTeachRecordSummary {
modules::trajectory::TrajectoryFileSet files; modules::trajectory::TrajectoryFileSet files;
int sample_count = 0; int sample_count = 0;
...@@ -26,16 +74,277 @@ struct DragTeachRecordSummary { ...@@ -26,16 +74,277 @@ struct DragTeachRecordSummary {
std::string vendor_save_message; std::string vendor_save_message;
}; };
/**
* @brief RM75 拖动示教录制器。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该类通过 `IRobotArm` 控制拖动示教生命周期，并把采样结果交给 trajectory 模块保存。
* 类本身不拥有机械臂驱动，便于测试时注入 fake robot。
*
* @note
* `record()` 会启动真实拖动示教，必须在人工确认安全环境后调用。
*
* @warning
* 调用方需要确保机械臂已经连接且急停可触达。
*/
class DragTeachRecorder { class DragTeachRecorder {
public: public:
/**
* @brief 构造拖动示教录制器。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 构造函数只保存机械臂接口引用，不启动拖动、不读取状态、不创建文件。
*
* @param[in,out] robot 已连接或可用的机械臂抽象接口，生命周期必须长于录制器。
*
* @return 构造函数无显式返回值。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 真实连接状态由 `record()` 内部调用的 driver 方法返回。
*
* @warning
* 不要传入临时对象引用。
*
* @thread_safety
* 构造本身线程安全；同一录制器不支持并发录制。
*
* @par 边界条件
* C++ 引用不能为空。
*
* @par 后续维护
* 多机械臂录制应由更高层任务编排，不应让本类持有多个接口。
*/
explicit DragTeachRecorder(interfaces::IRobotArm &robot); explicit DragTeachRecorder(interfaces::IRobotArm &robot);
/**
* @brief 执行一次拖动示教轨迹录制。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数按固定顺序执行：配置校验、轨迹文件路径准备、安全倒计时、启动拖动示教、
* 周期采样当前机械臂状态、停止拖动示教、写入 CSV、可选保存厂商轨迹。
*
* @param[in] config 本次录制配置。
* @param[out] out 过程日志输出流。
*
* @return 成功时返回录制摘要。
* @return 失败时返回配置、启动、采样、停止、文件写入或厂商保存错误。
*
* @throws 本函数不主动抛出异常；底层错误通过 `Result` 返回。
*
* @note
* 启动拖动示教后，即使采样失败，也会尝试停止拖动示教。
*
* @warning
* 该函数会访问真实机械臂并进入拖动示教模式，必须有人监督。
*
* @thread_safety
* 不支持并发调用；并发访问同一机械臂由调用方禁止。
*
* @par 边界条件
* 空 session、空路径、非法时长或过短采样周期会在启动前失败。
*
* @par 后续维护
* 新增采样字段时应先扩展 `TrajectorySample` 和 trajectory 模块，再修改本流程。
*/
common::Result<DragTeachRecordSummary> record(const DragTeachRecordConfig &config, common::Result<DragTeachRecordSummary> record(const DragTeachRecordConfig &config,
std::ostream &out); std::ostream &out);
private: private:
/**
* @brief 校验拖动示教录制配置。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数在启动真实拖动示教前检查存储根目录、会话名、录制时长和采样周期。
* 提前校验可以避免配置明显错误时仍进入硬件拖动状态。
*
* @param[in] config 待校验的录制配置。
*
* @return 成功时返回空成功结果。
* @return 失败时返回具体配置错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 本函数只校验配置，不访问文件系统和真实硬件。
*
* @warning
* 新增硬件相关参数时，必须在启动前加入校验。
*
* @thread_safety
* 函数只读取输入配置，线程安全。
*
* @par 边界条件
* 空路径、空会话名、非正时长、过短采样周期都会失败。
*
* @par 后续维护
* 不要把文件创建或 SDK 状态读取混入配置校验函数。
*/
common::Result<void> validateConfig(const DragTeachRecordConfig &config) const; common::Result<void> validateConfig(const DragTeachRecordConfig &config) const;
/**
* @brief 创建本次录制需要的轨迹文件路径集合。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数委托 trajectory 模块创建目录并生成 CSV、厂商轨迹、转换 C++ 和报告路径。
* 单独封装是为了让 `record()` 只表达流程顺序。
*
* @param[in] config 录制配置，必须包含存储根目录和会话名。
*
* @return 成功时返回文件路径集合。
* @return 失败时返回目录创建或路径准备错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 该函数可能创建本地目录，但不会连接机械臂。
*
* @warning
* 路径规则由 trajectory 模块统一维护，不应在 core 层手工拼接。
*
* @thread_safety
* 并发创建同一目录依赖底层文件系统处理。
*
* @par 边界条件
* 空路径会由 trajectory 模块返回失败。
*
* @par 后续维护
* 新增输出文件类型时应先扩展 `TrajectoryFileSet`。
*/
common::Result<modules::trajectory::TrajectoryFileSet>
createTrajectoryFileSet(const DragTeachRecordConfig &config) const;
/**
* @brief 在拖动示教状态下周期采集机械臂轨迹样本。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数按配置的采样周期读取当前机械臂状态，并转换为 `TrajectorySample` 列表。
* 它假设拖动示教已经成功启动，不负责启动或停止硬件模式。
*
* @param[in] config 录制配置，提供时长和采样周期。
* @param[out] out 采样过程日志输出流。
*
* @return 成功时返回样本列表。
* @return 失败时返回状态读取错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 采样失败后由 `record()` 负责停止拖动示教。
*
* @warning
* 本函数会阻塞当前线程直到采样结束或失败。
*
* @thread_safety
* 不支持并发调用同一录制器采样同一机械臂。
*
* @par 边界条件
* 配置非法应已由 `validateConfig()` 拒绝。
*
* @par 后续维护
* 新增采样来源时应保持该函数只做采集，不写文件。
*/
common::Result<std::vector<common::TrajectorySample>>
collectTrajectorySamples(const DragTeachRecordConfig &config, std::ostream &out);
/**
* @brief 根据文件集合和样本列表构造录制摘要。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数只填充可直接从已知数据得到的摘要字段，例如文件路径和采样数量。
* 厂商轨迹保存结果由单独函数补充。
*
* @param[in] files 本次录制产物路径集合。
* @param[in] samples 已采集的轨迹样本列表。
*
* @return 录制摘要对象。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 函数不访问文件系统和硬件。
*
* @warning
* 不要在该函数中触发厂商轨迹保存，避免摘要构造带副作用。
*
* @thread_safety
* 函数只读输入并返回局部对象，线程安全。
*
* @par 边界条件
* 空样本列表会返回 sample_count=0；上层写 CSV 会先拒绝空样本。
*
* @par 后续维护
* 新增摘要字段时保持该函数无副作用。
*/
DragTeachRecordSummary buildSummary(
const modules::trajectory::TrajectoryFileSet &files,
const std::vector<common::TrajectorySample> &samples) const;
/**
* @brief 根据配置可选保存厂商拖动示教轨迹。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数在 CSV 成功保存后，根据配置调用 driver 保存厂商轨迹，并把保存结果写入摘要。
* 厂商轨迹保存失败不会覆盖已经成功写出的 CSV 主产物。
*
* @param[in] config 录制配置，提供是否保存厂商轨迹的开关。
* @param[in,out] summary 录制摘要，函数会更新厂商轨迹字段。
*
* @return 无返回值；保存失败写入 `summary->vendor_save_message`。
*
* @throws 本函数不主动抛出异常。
*
* @note
* CSV 是主产物，厂商轨迹是附加产物。
*
* @warning
* `summary == nullptr` 时函数直接返回，避免空指针崩溃。
*
* @thread_safety
* 函数会调用注入的 `IRobotArm`，线程安全取决于具体 driver。
*
* @par 边界条件
* 禁用保存时只写入说明文本，不调用硬件保存接口。
*
* @par 后续维护
* 如果厂商保存结果需要作为强失败，应新增配置开关，不要直接改变当前行为。
*/
void saveVendorTrajectoryIfRequested(const DragTeachRecordConfig &config,
DragTeachRecordSummary *summary);
interfaces::IRobotArm &robot_; interfaces::IRobotArm &robot_;
}; };
......
RM/include/rm_control/core/drag_teach_service.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include "rm_control/common/robot_types.h"
#include "rm_control/core/hardware_execution_gate.h"
#include "rm_control/interfaces/robot_arm.h"
#include "rm_control/modules/trajectory/drag_trajectory_validator.h"
#include "rm_control/modules/trajectory/raw_trajectory_store.h"
#include <atomic>
#include <chrono>
#include <condition_variable>
#include <filesystem>
#include <mutex>
#include <string>
#include <string_view>
#include <thread>
#include <vector>
namespace rm_control::core {
enum class DragTeachServiceState {
Idle,
Recording,
Recorded,
Saved,
AtOrigin,
Replaying,
Paused,
Stopped,
Faulted,
};
struct DragTeachServiceConfig {
std::filesystem::path storage_root;
std::string session_name;
std::chrono::milliseconds sample_period{100}; // 允许 10..1000 ms。
common::DragTeachStartOptions drag_options;
modules::trajectory::DragTrajectoryValidationLimits validation_limits;
double maximum_absolute_force_newton = 100.0; // 外力绝对值上限，newton。
double maximum_absolute_torque_newton_meter = 20.0; // 外力矩上限，newton_meter。
std::size_t maximum_sample_count = 100000;
};
struct StopRecordingSummary {
std::size_t recorded_sample_count = 0;
modules::trajectory::DragTrajectoryValidationReport validation_report;
};
/**
* @brief 编排 RM75 拖动示教采样、原始文件固化和厂商原生回放。
*
* @reason 将状态机、安全门控、内存采样和文件固化从 Application 与厂商 Adapter 中分离，
* 使全部业务状态转换可通过 MockRobotArm 单元测试。
*
* @author 唐永康
* @date 2026-07-10
*
* @calls IRobotArm、RequireHardwareExecutionPermission、ValidateDragTrajectory、
* CreateUniqueRawTrajectorySession 和 FinalizeRawTrajectorySession。
* @safety Start、MoveToOrigin、Replay 和 Continue 可能触发真实硬件；调用前必须提供完整门控。
* @note 类不拥有机械臂对象；传入接口的生命周期必须长于本服务。
*/
class DragTeachService {
public:
/**
* @brief 构造一个尚未启动录制的拖动示教服务。
*
* @reason 服务只借用统一机械臂接口，便于生产代码注入 Adapter、测试注入 Mock。
*
* @author 唐永康
* @date 2026-07-10
*
* @param robot 机械臂接口引用；对象生命周期必须长于本服务。
* @return 构造函数无显式返回值。
*
* @calls 不调用机械臂接口。
* @safety 构造过程不连接设备、不触发硬件动作。
* @note 服务初始状态为 Idle。
*/
explicit DragTeachService(interfaces::IRobotArm &robot);
DragTeachService(const DragTeachService &) = delete;
DragTeachService &operator=(const DragTeachService &) = delete;
/**
* @brief 停止仍在进行的采样、拖动或回放并释放线程资源。
*
* @reason RAII 收尾避免提前返回后遗留采样线程或机械臂拖动模式。
*
* @author 唐永康
* @date 2026-07-10
*
* @return 析构函数无返回值且不会向外抛出异常。
*
* @calls IRobotArm::stopDragTeach、IRobotArm::stopDragTrajectoryReplay。
* @safety 只尝试减险停止动作，不启动或继续运动。
* @note 停止错误无法从析构函数返回，由显式 Stop 方法负责可审计处理。
*/
~DragTeachService() noexcept;
/**
* @brief 完成型号、状态和门控检查后开始拖动示教与内存采样。
*
* @reason Start 与 Stop 分离后，操作者可以在明确的 Recording 状态中手动拖动机械臂。
*
* @author 唐永康
* @date 2026-07-10
*
* @param config 唯一目录、采样周期、拖动模式及轨迹校验上限。
* @param gate_input 来自命令行、配置、设备状态与人工确认的安全事实。
* @return 启动硬件和采样线程成功时返回成功，否则保持非 Recording 状态。
*
* @calls IRobotArm::getRobotInfo、getRobotSafetyStatus、startDragTeach。
* @safety 会让真实 RM75 进入拖动示教；所有门控条件缺一即拒绝。
* @note 默认 OrdinaryRecorded；六维力复合模式的厂商文件保存仅为 best-effort。
*/
common::Result<void> StartRecording(
const DragTeachServiceConfig &config,
const HardwareExecutionGateInput &gate_input);
/**
* @brief 停止采样和拖动示教，并校验完整七关节轨迹。
*
* @reason 无论采样是否失败都必须先退出硬件拖动状态，再决定轨迹能否保存或回放。
*
* @author 唐永康
* @date 2026-07-10
*
* @return 成功时返回样本数和轨迹峰值报告；停止或校验失败时进入 Faulted。
*
* @calls IRobotArm::stopDragTeach 和 ValidateDragTrajectory。
* @safety Stop 是减险动作，不依赖新的执行授权。
* @note 必须在 Recording 状态调用。
*/
common::Result<StopRecordingSummary> StopRecording();
/**
* @brief 将最后一次已校验录制保存为唯一原始目录并生成 manifest。
*
* @reason 原始厂商文件、同步 CSV、返回码、大小和 SHA-256 必须在同一不可覆盖会话中追溯。
*
* @author 唐永康
* @date 2026-07-10
*
* @return 文件固化流程成功时返回产物；厂商返回非零仍返回带失败码的 manifest 产物。
*
* @calls IRobotArm::saveDragTeachTrajectory、writeTrajectoryCsv、
* CreateUniqueRawTrajectorySession 和 FinalizeRawTrajectorySession。
* @safety 会调用厂商保存接口但不会移动机械臂。
* @note 只有厂商返回 0、原始文件存在且同步 CSV 成功时状态才进入 Saved。
*/
common::Result<modules::trajectory::RawTrajectoryArtifact> SaveRawTrajectory();
/**
* @brief 让机械臂移动到控制器内最后拖动轨迹的起点。
*
* @reason 厂商要求原生回放前先到达起点，单独暴露可避免 Replay 隐式触发额外运动。
*
* @author 唐永康
* @date 2026-07-10
*
* @param gate_input 完整安全事实，配置速度上限必须允许厂商固定的 20%。
* @return 到达起点成功时进入 AtOrigin，否则保持 Saved/Stopped。
*
* @calls IRobotArm::moveToDragTrajectoryOrigin。
* @safety 会以厂商固定 20% 速度触发真实运动，必须完整门控。
* @note 只支持控制器当前内存中的最后拖动轨迹，不从磁盘加载。
*/
common::Result<void> MoveToTrajectoryOrigin(
const HardwareExecutionGateInput &gate_input);
/**
* @brief 非阻塞开始回放控制器内最后拖动轨迹。
*
* @reason 非阻塞启动才能在同一服务生命周期内提供 Pause、Continue 和 Stop。
*
* @author 唐永康
* @date 2026-07-10
*
* @param gate_input 完整安全事实，并包含无速度参数风险接受及独立实测速度验证。
* @return 启动成功时进入 Replaying。
*
* @calls IRobotArm::replayLastDragTrajectory。
* @safety 会触发真实轨迹运动，必须完整门控且轨迹已通过校验。
* @note 原生接口没有文件路径和速度参数。
*/
common::Result<void> ReplayLastDragTrajectory(
const HardwareExecutionGateInput &gate_input);
/**
* @brief 暂停正在执行的原生拖动轨迹。
*
* @reason Pause 是减险动作，不应因授权在运动期间失效而被阻止。
*
* @author 唐永康
* @date 2026-07-10
*
* @return 暂停成功时进入 Paused。
* @calls IRobotArm::pauseDragTrajectoryReplay。
* @safety 只降低当前运动风险，不需要新的执行授权。
* @note 仅允许在 Replaying 状态调用。
*/
common::Result<void> PauseReplay();
/**
* @brief 在重新通过完整门控后继续暂停的原生回放。
*
* @reason Continue 会恢复真实运动，因此不能复用回放开始时的旧授权。
*
* @author 唐永康
* @date 2026-07-10
*
* @param gate_input 调用时重新收集的完整安全事实。
* @return 继续成功时回到 Replaying。
* @calls IRobotArm::continueDragTrajectoryReplay。
* @safety 会恢复真实运动，必须重新完整门控。
* @note 仅允许在 Paused 状态调用。
*/
common::Result<void> ContinueReplay(
const HardwareExecutionGateInput &gate_input);
/**
* @brief 停止正在回放或暂停的原生轨迹。
*
* @reason Stop 必须在安全授权失效时仍可尝试，防止门控反而阻断减险操作。
*
* @author 唐永康
* @date 2026-07-10
*
* @return 停止成功时进入 Stopped。
* @calls IRobotArm::stopDragTrajectoryReplay。
* @safety 是减险动作，不依赖新的执行授权。
* @note 仅允许在 Replaying 或 Paused 状态调用。
*/
common::Result<void> StopReplay();
/**
* @brief 返回当前服务状态快照。
* @reason 状态机编排与测试需要只读观察，不允许直接修改内部状态。
* @author 唐永康
* @date 2026-07-10
* @return 调用时刻的状态枚举。
* @calls 不调用设备接口。
* @safety 纯内存读取，不触发硬件动作。
* @note 返回后状态可能被另一个线程推进。
*/
DragTeachServiceState state() const noexcept;
/**
* @brief 返回当前内存中的同步 RM75 样本数。
* @reason Application 需要显示录制进度，但不应取得可变轨迹容器。
* @author 唐永康
* @date 2026-07-10
* @return 已采集并保存在内存中的样本数量。
* @calls 不调用设备接口。
* @safety 纯内存读取，不触发硬件动作。
* @note 录制期间该数值会增长。
*/
std::size_t recordedSampleCount() const;
private:
common::Result<void> ValidateRecordingConfig(
const DragTeachServiceConfig &config) const;
common::Result<void> ReadAndValidateRobotPreflight(
const DragTeachServiceConfig &config);
common::Result<void> ValidateRobotIdentity(
const common::RobotInfo &info,
const common::DragTeachStartOptions &options) const;
common::Result<void> ValidateRobotSafetyStatus(
const common::RobotSafetyStatus &status) const;
common::Result<void> ValidateCurrentStateAndForce();
common::Result<void> ValidateRuntimeMotionPreflight();
common::Result<void> ValidateSampledArmState(
const common::ArmState &state) const;
common::Result<void> ValidateCurrentStateMatchesTrajectoryOrigin();
common::Result<void> StartHardwareAndSamplingThread();
void SamplingLoop() noexcept;
void RunSamplingLoop();
void RecordSamplingFailure(std::string_view message) noexcept;
void RequestSamplingStop() noexcept;
common::Result<void> JoinSamplingThread();
common::Result<void> StopDragTeachIfActive();
common::Result<StopRecordingSummary> ValidateStoppedRecording();
HardwareExecutionGateRequirements OriginRequirements() const;
HardwareExecutionGateRequirements ReplayRequirements() const;
common::Result<void> RequirePermission(
const HardwareExecutionGateInput &gate_input,
const HardwareExecutionGateRequirements &requirements) const;
common::Result<void> RequireState(
DragTeachServiceState expected,
const std::string &operation_name) const;
interfaces::IRobotArm &robot_;
mutable std::mutex operation_mutex_;
mutable std::mutex mutex_;
std::condition_variable sampling_condition_;
std::thread sampling_thread_;
std::atomic<bool> sampling_stop_requested_{false};
std::atomic<bool> sampling_failed_{false};
std::atomic<DragTeachServiceState> state_{DragTeachServiceState::Idle};
DragTeachServiceConfig config_;
common::RobotInfo robot_info_;
common::RobotSoftwareInfo software_info_;
common::RobotJointLimits joint_limits_;
std::vector<common::TrajectorySample> samples_;
modules::trajectory::DragTrajectoryValidationReport validation_report_;
modules::trajectory::RawTrajectoryArtifact last_artifact_;
bool hardware_drag_active_ = false;
bool validation_report_available_ = false;
bool artifact_available_ = false;
bool native_motion_possibly_active_ = false;
std::string sampling_error_;
};
} // namespace rm_control::core
RM/include/rm_control/core/hardware_execution_gate.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include <string>
#include <vector>
namespace rm_control::core {
// 睿尔曼厂商原生“移动到拖动轨迹起点”接口固定使用 20% 速度。
inline constexpr int kNativeTrajectoryOriginRequiredSpeedPercent = 20;
/**
* @brief 真实硬件动作执行前由各层共同提供的安全事实。
*
* @reason 将命令行授权、静态配置和运行时设备状态汇总成不可隐式补全的输入，
* 使业务服务能够在调用任何运动接口前统一执行 fail-closed 判定。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety 所有字段默认拒绝执行；调用方不得用默认 true 或跳过未知状态。
* @note 本结构只承载已经由对应模块验证过的事实，不负责连接设备或读取配置。
*/
struct HardwareExecutionGateInput {
bool live_requested = false;
bool hardware_execution_enabled = false;
bool arm_connected = false;
bool hand_connected = false;
bool emergency_stop_verified = false;
bool safety_limits_loaded = false;
bool operator_confirmed = false;
bool trajectory_validated = false;
bool trajectory_sample_period_confirmed = false;
bool vendor_raw_units_per_degree_confirmed = false;
bool workspace_and_virtual_wall_verified = false;
int configured_max_speed_percent = 0;
bool native_replay_without_speed_control_accepted = false;
bool native_replay_speed_within_config_verified = false;
};
/**
* @brief 当前硬件动作对安全门控提出的附加要求。
*
* @reason 厂商原生回放和普通受控动作的速度、时间及单位前提不同，显式传入动作要求
* 可以复用同一基础门控，同时避免把轨迹文件特有条件错误施加给其他动作。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety `required_speed_percent` 为动作实际需要的速度比例；0 表示该动作不声明速度。
* @note 移动到原生轨迹起点应传入固定 20%；文件回放应显式要求采样周期和厂商单位比例确认。
*/
struct HardwareExecutionGateRequirements {
int required_speed_percent = 0;
bool require_native_replay_without_speed_control_acceptance = false;
bool require_native_replay_speed_verification = false;
bool require_confirmed_trajectory_sample_period = false;
bool require_confirmed_vendor_raw_units_per_degree = false;
};
/**
* @brief 安全门控的完整判定结果。
*
* @reason 保留全部拒绝原因便于操作者一次修正多个缺失条件，并为审计日志提供稳定输入。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety 只有 `execution_allowed` 为 true 时才允许调用真实硬件动作接口。
* @note 拒绝原因不包含设备密钥、网络地址或其他敏感数据。
*/
struct HardwareExecutionGateDecision {
bool execution_allowed = false;
std::vector<std::string> rejection_reasons;
};
/**
* @brief 评估真实硬件动作的全部安全门控条件。
*
* @reason 使用无副作用的纯软件判定，使服务层、单元测试和审计工具共享同一套拒绝规则。
*
* @author 唐永康
* @date 2026-07-11
*
* @param input 已经由命令行、配置和设备服务确认的安全事实。
* @param requirements 当前动作的速度与原生回放附加要求。
* @return 返回允许标志及全部拒绝原因；本函数本身不以异常或错误码表示门控拒绝。
*
* @calls 仅调用本模块内部的纯软件校验函数。
* @safety 本函数不连接设备、不触发运动；未知或非法输入一律产生拒绝结果。
* @note `configured_max_speed_percent` 和声明的动作速度有效范围均为 1..100；动作速度可为 0 表示不声明。
*/
HardwareExecutionGateDecision EvaluateHardwareExecutionGate(
const HardwareExecutionGateInput &input,
const HardwareExecutionGateRequirements &requirements = {});
/**
* @brief 以统一 `Result<void>` 形式要求真实硬件执行权限。
*
* @reason 服务层通常需要失败即返回的控制流，本函数把完整门控判定转换为包含上下文的错误。
*
* @author 唐永康
* @date 2026-07-11
*
* @param input 已经由命令行、配置和设备服务确认的安全事实。
* @param requirements 当前动作的速度与原生回放附加要求。
* @return 全部条件满足时成功；否则返回合并后的全部拒绝原因。
*
* @calls EvaluateHardwareExecutionGate。
* @safety 调用方必须在每次真实运动开始或继续前调用；成功结果不替代设备侧急停。
* @note Pause 和 Stop 属于减险动作，应由服务层直接执行，不应被本门控阻止。
*/
common::Result<void> RequireHardwareExecutionPermission(
const HardwareExecutionGateInput &input,
const HardwareExecutionGateRequirements &requirements = {});
} // namespace rm_control::core
RM/include/rm_control/core/json_command_runner.h
View file @ 8275ee2e
/**
* @file json_command_runner.h
* @brief RM JSON 指令执行器接口。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件声明 JSON 指令执行器。执行器属于 core 层，只负责连接生命周期、
* 命令流程编排和运动安全校验；真实硬件能力通过 `IRobotArm` 注入，
* 因而本头文件不包含 RealMan RM_API2 头文件。
*
* 设计原则：
* - core 只依赖接口，不依赖具体 SDK；
* - 所有运动命令发送前必须统一安全校验；
* - `dry_run=true` 时只读取当前状态和校验目标，不发送真实运动；
* - 单条命令执行拆分成小函数，便于后续单元测试和维护。
*
* @dependencies
* - rm_control/common/result.h
* - rm_control/config/json_command.h
* - rm_control/interfaces/robot_arm.h
*/
#pragma once #pragma once
#include "rm_control/common/result.h" #include "rm_control/common/result.h"
...@@ -8,23 +32,246 @@ ...@@ -8,23 +32,246 @@
namespace rm_control::core { namespace rm_control::core {
// JSON 指令执行器。 /**
// 这个类只负责流程编排和安全校验，不直接包含 rm_interface.h。 * @brief JSON 指令执行器。
// 真实硬件动作通过 IRobotArm 注入，保证后续可替换机械臂驱动。 *
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该类把已经解析好的 `JsonCommandProgram` 转换为机械臂接口调用。它不读取文件，
* 不包含具体 RealMan SDK 类型，只通过 `IRobotArm` 访问硬件能力。当前工程仅面向
* 睿尔曼 RM75；接口隔离用于控制依赖边界和测试，不作为多品牌机械臂扩展承诺。
*
* @note
* 运动安全策略来自 `JsonCommandProgram::safety`，默认 dry-run 且禁止真实运动。
*
* @warning
* 调用 `run()` 可能连接真实机械臂；调用方必须先确认运行环境安全。
*/
class JsonCommandRunner { class JsonCommandRunner {
public: public:
/**
* @brief 构造 JSON 指令执行器。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 构造函数只保存 `IRobotArm` 引用，不连接硬件、不读取状态。这样调用方可以在
* 测试中注入 fake robot，也可以在生产环境中注入 RealMan driver。
*
* @param[in,out] robot 已创建的机械臂抽象接口对象，生命周期必须长于 runner。
*
* @return 构造函数无显式返回值。
*
* @throws 本构造函数不主动抛出异常。
*
* @note
* `robot` 以引用保存，避免 runner 拥有硬件驱动生命周期。
*
* @warning
* 调用方不能传入即将销毁的临时对象。
*
* @thread_safety
* 构造本身线程安全；后续并发调用同一个 runner 不做保证。
*
* @par 边界条件
* C++ 引用不能为空，因此不做空指针检查。
*
* @par 后续维护
* 如果需要多机械臂支持，应引入更高层 orchestration，不应让本类持有容器。
*/
explicit JsonCommandRunner(interfaces::IRobotArm &robot); explicit JsonCommandRunner(interfaces::IRobotArm &robot);
/**
* @brief 执行一个已解析的 JSON 指令程序。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数先连接机械臂，再按 JSON 中 `commands` 顺序逐条执行。只读命令会输出状态，
* 运动命令会先读取当前状态并执行限幅校验；如果 `dry_run=true`，运动命令只校验不发送。
*
* @param[in] program 已解析并完成字段校验的 JSON 指令程序。
* @param[out] out 流程日志输出流。
*
* @return 成功时返回 `Result<void>::success()`。
* @return 失败时返回连接、状态读取、安全校验或运动执行错误。
*
* @throws 本函数不主动抛出异常；底层接口错误通过 `Result` 返回。
*
* @note
* 本函数不负责解析 JSON 文件，文件解析由 config 层完成。
*
* @warning
* `dry_run=false && allow_motion=true` 时可能发送真实运动指令。
*
* @thread_safety
* 该函数会调用注入的 `IRobotArm`，线程安全取决于具体 driver；默认不并发调用。
*
* @par 边界条件
* 空命令列表通常已由 config 层拒绝；若传入空列表，本函数会连接后直接成功返回。
*
* @par 后续维护
* 新增命令类型时应新增专用执行函数和校验测试，不要扩大本函数职责。
*/
common::Result<void> run(const config::JsonCommandProgram &program, std::ostream &out); common::Result<void> run(const config::JsonCommandProgram &program, std::ostream &out);
private: private:
// 所有运动命令发送前都必须经过这里。 /**
// 绝对 movej 检查“目标与当前值差值”，相对 movej_delta 检查“单次增量”， * @brief 执行一条 JSON 命令。
// movej_delta_steps 检查“每步增量”，避免 JSON 写错导致大幅跳动。 *
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数负责打印命令头并根据命令类型分发到只读、睡眠或运动执行路径。
* 单独封装是为了让 `run()` 只保留顶层循环和连接流程。
*
* @param[in] program 完整 JSON 程序，提供安全策略。
* @param[in] command 当前命令。
* @param[in] command_index 当前命令序号，从 0 开始。
* @param[in] command_count 命令总数。
* @param[out] out 流程日志输出流。
*
* @return 成功或失败原因。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 睡眠命令也在此分发，但实际 sleep 保持在 core 层。
*
* @warning
* 运动命令会继续进入真实硬件执行路径，必须依赖后续安全校验。
*
* @thread_safety
* 不保证并发安全。
*
* @par 边界条件
* 未知枚举值会落入运动路径前的 switch 兜底失败逻辑。
*
* @par 后续维护
* 新增命令类型必须在此处显式分支。
*/
common::Result<void> runSingleCommand(const config::JsonCommandProgram &program,
const config::ArmCommand &command,
size_t command_index,
size_t command_count,
std::ostream &out);
/**
* @brief 执行只读命令。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数处理机器人信息、当前状态和六维力数据读取。只读命令不会发送运动指令，
* 适合作为硬件状态诊断和 dry-run 前置检查。
*
* @param[in] command 当前只读命令。
* @param[out] out 状态输出流。
*
* @return 成功或底层状态读取错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 只读命令仍要求机器人已连接。
*
* @warning
* 如果未来新增只读命令，应确保不会触发运动或写寄存器。
*
* @thread_safety
* 线程安全取决于注入的 `IRobotArm`。
*
* @par 边界条件
* 非只读命令传入时返回失败，避免静默忽略。
*
* @par 后续维护
* 输出格式变化可能影响脚本解析，应谨慎修改。
*/
common::Result<void> runReadOnlyCommand(const config::ArmCommand &command,
std::ostream &out);
/**
* @brief 执行运动类命令。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数统一读取当前状态、执行安全校验、处理 dry-run 和 allow-motion 开关，
* 然后分发到具体运动命令执行函数。
*
* @param[in] program 完整 JSON 程序，提供安全策略。
* @param[in] command 当前运动命令。
* @param[out] out 流程日志输出流。
*
* @return 成功或状态读取、安全校验、运动执行错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* dry-run 仍会读取当前状态，以验证目标相对当前姿态是否安全。
*
* @warning
* `dry_run=false && allow_motion=true` 时会发送真实运动指令。
*
* @thread_safety
* 线程安全取决于注入的 `IRobotArm`。
*
* @par 边界条件
* 状态读取失败时不会继续执行运动。
*
* @par 后续维护
* 所有新增运动命令都必须接入此函数，不能绕过 safety。
*/
common::Result<void> runMotionCommand(const config::JsonCommandProgram &program,
const config::ArmCommand &command,
std::ostream &out);
common::Result<void> validateMotionCommand(const config::JsonCommandProgram &program, common::Result<void> validateMotionCommand(const config::JsonCommandProgram &program,
const config::ArmCommand &command, const config::ArmCommand &command,
const common::ArmState &current_state) const; const common::ArmState &current_state) const;
common::Result<void> validateMoveJCommand(const config::JsonCommandProgram &program,
const config::ArmCommand &command,
const common::ArmState &current_state) const;
common::Result<void> validateMoveJDeltaCommand(const config::JsonCommandProgram &program,
const config::ArmCommand &command,
const common::ArmState &current_state) const;
common::Result<void> validateMoveJDeltaStepsCommand(const config::JsonCommandProgram &program,
const config::ArmCommand &command,
const common::ArmState &current_state) const;
common::Result<void> validateMoveLCommand(const config::JsonCommandProgram &program,
const config::ArmCommand &command,
const common::ArmState &current_state) const;
common::Result<void> executeMoveJCommand(const config::ArmCommand &command,
std::ostream &out);
common::Result<void> executeMoveJDeltaCommand(const config::ArmCommand &command,
const common::ArmState &current_state,
std::ostream &out);
common::Result<void> executeMoveJDeltaStepsCommand(const config::ArmCommand &command,
std::ostream &out);
common::Result<void> executeMoveLCommand(const config::ArmCommand &command,
std::ostream &out);
interfaces::IRobotArm &robot_; interfaces::IRobotArm &robot_;
}; };
......
RM/include/rm_control/core/o6_bus_scanner.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include "rm_control/interfaces/tool_modbus_bus.h"
#include <cstddef>
#include <cstdint>
#include <string>
#include <vector>
namespace rm_control::core {
struct O6BusScanConfig {
std::vector<std::uint8_t> device_ids{0x27U, 0x28U};
std::uint16_t identity_start_address = 30U;
std::size_t identity_register_count = 6U;
};
struct O6BusScanResponse {
std::uint8_t device_id = 0U;
bool multiple_read_succeeded = false;
std::vector<std::uint16_t> identity_registers;
std::string multiple_read_error;
bool single_read_succeeded = false;
std::uint16_t single_freedom_register = 0U;
std::string single_read_error;
bool single_and_multiple_reads_match = false;
bool matches_o6_identity_shape = false;
};
struct O6BusScanReport {
std::size_t attempted_device_count = 0U;
std::size_t failed_device_count = 0U;
std::size_t confirmed_o6_device_count = 0U;
std::vector<O6BusScanResponse> responses;
};
/**
* @brief 对 O6 默认左右手地址执行目标化只读身份探测。
* @reason 固定探测 0x27/0x28 的 FC04 地址 30..35，并用单寄存器地址 30 交叉验证
* RM_API2 的两条读取路径，避免无边界全地址扫描。
* @author 唐永康
* @date 2026-07-11
* @param tool_bus 已连接并显式初始化的工具端 Modbus 主站。
* @param config 固定候选设备和身份寄存器范围；偏离 O6 默认值会被拒绝。
* @return 参数有效时返回两个设备的双路径结果和原始错误上下文。
* @calls IToolModbusBus::ReadInputRegisters 和 ReadInputRegister。
* @safety 只调用功能码 04，不写保持寄存器，不触发 O6 或机械臂动作。
* @note 每条读取路径仅调用一次，不自动重试。
*/
common::Result<O6BusScanReport> ScanO6IdentityRegisters(
interfaces::IToolModbusBus &tool_bus,
const O6BusScanConfig &config);
} // namespace rm_control::core
RM/include/rm_control/drivers/realman/realman_robot_arm.h
View file @ 8275ee2e
#pragma once #pragma once
#include "rm_control/interfaces/robot_arm.h" #include "flight_robot/realman/realman_arm_adapter.h"
#include "rm_interface.h"
namespace rm_control::drivers::realman { namespace rm_control::drivers::realman {
// 睿尔曼 RM_API2 的具体驱动实现。 // 旧名称仅作为源码兼容别名；唯一实现是允许目录中的 RealmanArmAdapter。
// 这是 RM 子工程中唯一直接依赖 rm_interface.h 的位置之一； using RealManRobotArm = ::flight_robot::realman::RealmanArmAdapter;
// core/config/interfaces 都不应该 include 睿尔曼 SDK 头文件。
class RealManRobotArm final : public interfaces::IRobotArm {
public:
RealManRobotArm() = default;
RealManRobotArm(const RealManRobotArm &) = delete;
RealManRobotArm &operator=(const RealManRobotArm &) = delete;
~RealManRobotArm() override;
common::Result<void> connect(const common::RobotConnectionConfig &config) override;
void disconnect() override;
common::Result<common::RobotInfo> getRobotInfo() override;
common::Result<common::ArmState> getCurrentArmState() override;
common::Result<common::ForceData> getForceData() override;
common::Result<void> moveJ(const std::vector<float> &joints_deg,
int speed_percent,
int blend_radius,
int trajectory_connect,
bool blocking) override;
common::Result<void> moveL(const common::Pose &pose,
int speed_percent,
int blend_radius,
int trajectory_connect,
bool blocking) override;
common::Result<void> startDragTeach(const common::DragTeachStartOptions &options) override;
common::Result<void> stopDragTeach() override;
common::Result<common::SavedVendorTrajectory>
saveDragTeachTrajectory(const std::string &path) override;
private:
// 所有 SDK 读写前先检查连接状态，防止空句柄传入 C SDK。
common::Result<void> ensureConnected() const;
// RM_API2 使用裸指针句柄，因此必须由本类析构函数集中释放。
rm_robot_handle *handle_ = nullptr;
bool initialized_ = false;
int dof_ = 0;
};
} // namespace rm_control::drivers::realman } // namespace rm_control::drivers::realman
RM/include/rm_control/interfaces/dexterous_hand.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include "rm_control/interfaces/tool_modbus_bus.h"
#include <array>
#include <cstddef>
#include <cstdint>
namespace rm_control::interfaces {
inline constexpr std::size_t kO6JointCount = 6U;
enum class DexterousHandSide {
Left,
Right,
};
using O6JointPose = std::array<std::uint16_t, kO6JointCount>;
/**
* @brief 一次 O6 动作所需的六关节位置、速度和转矩命令。
*
* @reason 把三个必须一致下发的寄存器组放入单一输入，适配器可以先完整校验，再按
* “转矩读回、速度读回、最后位置”顺序执行，避免使用设备残留配置。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety 该结构体自身不触发动作；传给 CommandMotion 后目标位置可能立即驱动 O6。
* @note 三组值均按拇指弯曲、拇指横摆、食指、中指、无名指、小指排列，范围 0..255。
*/
struct O6MotionCommand {
O6JointPose target_positions{};
O6JointPose joint_speed_commands{};
O6JointPose joint_torque_commands{};
};
/**
* @brief 经 O6 协议字段校验后的灵巧手 readiness 快照。
*
* @reason 将总线状态、身份、六关节位置、转矩、速度、温度和故障放在同一结果中，
* 状态机只在完整快照合规时才能把 `ready` 作为真实硬件门控事实。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety 只有 `ReadReadiness` 成功返回的快照才允许 `ready=true`。
* @note 位置、转矩和速度为 O6 无量纲 0..255，温度为摄氏度整数。
*/
struct DexterousHandReadiness {
bool ready = false;
DexterousHandSide side = DexterousHandSide::Right;
std::uint8_t device_id = 0;
ToolModbusBusStatus tool_bus_status;
std::uint16_t freedom = 0;
std::uint16_t hand_version = 0;
std::array<std::uint16_t, 3U> hand_number_parts{};
char direction = '\0';
O6JointPose joint_positions{};
O6JointPose joint_torques{};
O6JointPose joint_speeds{};
O6JointPose temperatures_celsius{};
O6JointPose fault_codes{};
};
/**
* @brief 飞行操纵业务使用的 O6 灵巧手抽象接口。
*
* @reason 状态机只依赖 readiness 和完整六关节动作语义，不包含 LinkerHand 或 RealMan
* 厂商头文件。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety readiness 为只读；动作命令可能触发真实握夹动作并必须受上层门控。
* @note 当前接口只覆盖项目目标 O6，不承诺其他灵巧手型号兼容性。
*/
class IDexterousHand {
public:
/**
* @brief 销毁灵巧手接口对象。
*
* @reason 允许通过抽象接口安全释放具体 O6 适配器。
*
* @author 唐永康
* @date 2026-07-10
*
* @return 析构函数无返回值且不得抛出异常。
*
* @calls 具体适配器析构函数。
* @safety 只释放资源，不发送新的 O6 动作命令。
* @note 总线所有权由具体依赖组装决定。
*/
virtual ~IDexterousHand() = default;
/**
* @brief 读取并验证当前 O6 总线、身份、位置、转矩、速度、温度和故障状态。
*
* @reason 单一成功结果可以作为硬件门控输入，避免业务层把“总线已连接”误当成“O6 可用”。
*
* @author 唐永康
* @date 2026-07-11
*
* @return 所有强制字段合规时返回 ready 快照；任一读取、维度或安全值非法时失败。
*
* @calls IToolModbusBus::GetStatus 和 ReadInputRegister。
* @safety 只读，不触发 O6 或机械臂运动。
* @note 调用方应在每次握夹前重新读取，不能永久缓存 readiness。
*/
virtual common::Result<DexterousHandReadiness> ReadReadiness() = 0;
/**
* @brief 校验并下发 O6 六关节位置、速度和转矩动作命令。
*
* @reason 单一命令入口强制适配器先写转矩并读回、再写速度并读回，只有两组精确一致
* 后才允许写位置，防止使用未知的残留速度或转矩执行抓握。
*
* @author 唐永康
* @date 2026-07-11
*
* @param command 六关节位置、速度和转矩命令，每项有效范围 0..255；调用期间只读。
* @return 三组命令按安全顺序完成时成功；输入越界、读写失败或读回不一致时返回错误。
*
* @calls IToolModbusBus::WriteHoldingRegisters；动作后的状态由 ReadReadiness 读取。
* @safety 可能触发真实握夹动作，必须由状态机在 readiness 和 Live 门控通过后调用。
* @note 不自动重试；转矩或速度任一步失败时禁止下发位置，成功也不替代动作后状态验证。
*/
virtual common::Result<void> CommandMotion(
const O6MotionCommand &command) = 0;
};
} // namespace rm_control::interfaces
RM/include/rm_control/interfaces/robot_arm.h
View file @ 8275ee2e
...@@ -3,48 +3,364 @@ ...@@ -3,48 +3,364 @@
#include "rm_control/common/result.h" #include "rm_control/common/result.h"
#include "rm_control/common/robot_types.h" #include "rm_control/common/robot_types.h"
#include <string> #include <filesystem>
#include <vector> #include <vector>
namespace rm_control::interfaces { namespace rm_control::interfaces {
// 机械臂抽象接口。 // 机械臂抽象接口。
// core 只依赖这个接口，不直接依赖 RealManRobotArm 或 rm_interface.h。 // core 只依赖这个接口，不直接依赖 RealManRobotArm 或 rm_interface.h。
// 后续如果更换机械臂品牌，只要新增一个实现 IRobotArm 的 driver， // 当前支持边界只覆盖睿尔曼 RM75；保留接口是为了隔离 SDK 头文件和便于测试，
// JSON 指令解析和 JsonCommandRunner 都可以继续复用。 // 不是为了引入其他机械臂品牌适配。
class IRobotArm { class IRobotArm {
public: public:
/**
* @brief 销毁机械臂接口对象。
* @reason 通过虚析构函数保证经接口释放适配器时正确回收 SDK 资源。
* @author 唐永康
* @date 2026-07-10
* @return 无返回值，析构不得抛出异常。
* @calls 具体适配器析构函数。
* @safety 只执行资源清理，不主动发送运动指令。
* @note 接口不拥有外部安全门控对象。
*/
virtual ~IRobotArm() = default; virtual ~IRobotArm() = default;
// 建立/释放硬件连接。具体 SDK 初始化细节放在 drivers 层。 /**
* @brief 建立机械臂连接。
* @reason 将连接和 SDK 初始化隔离在适配器中，使服务可注入 Mock。
* @author 唐永康
* @date 2026-07-10
* @param config 控制器 IP、端口和超时，超时单位为毫秒。
* @return 成功返回空结果，失败返回带上下文的连接错误。
* @calls 具体实现调用厂商 SDK 初始化与连接接口。
* @safety 只建立通信，不应触发运动。
* @note 重复连接的资源释放策略由实现负责。
*/
virtual common::Result<void> connect(const common::RobotConnectionConfig &config) = 0; virtual common::Result<void> connect(const common::RobotConnectionConfig &config) = 0;
/**
* @brief 释放机械臂连接和 SDK 资源。
* @reason 统一硬件资源生命周期，防止 C SDK 句柄泄漏。
* @author 唐永康
* @date 2026-07-10
* @return 无返回值，且实现不得抛出异常。
* @calls 具体实现调用厂商断开和销毁接口。
* @safety 不发送运动指令。
* @note 必须允许重复调用。
*/
virtual void disconnect() = 0; virtual void disconnect() = 0;
// 只读接口：适合自动诊断和状态显示，不会造成机械臂运动。 /**
* @brief 查询当前是否持有有效连接。
* @reason 安全门控必须在所有硬件动作前检查连接状态。
* @author 唐永康
* @date 2026-07-10
* @return 已初始化且句柄有效时返回 true。
* @calls 不调用厂商接口。
* @safety 纯只读查询，不触发运动。
* @note 本函数不得抛出异常。
*/
virtual bool isConnected() const noexcept = 0;
/**
* @brief 查询机械臂型号、自由度、力传感器与控制器代际。
* @reason 上层安全检查必须使用厂商无关字段确认 RM75 和七自由度。
* @author 唐永康
* @date 2026-07-10
* @return 成功返回机械臂基础信息，失败返回带接口名的读取错误。
* @calls 具体实现调用厂商机械臂信息查询接口。
* @safety 只读，不触发运动。
* @note 上层应优先使用 is_rm75 等语义字段，避免比较厂商枚举整数。
*/
virtual common::Result<common::RobotInfo> getRobotInfo() = 0; virtual common::Result<common::RobotInfo> getRobotInfo() = 0;
/**
* @brief 查询 SDK、控制器固件及各软件组件版本。
* @reason manifest 需要记录可追溯的软件环境，且不同组件版本不能混写。
* @author 唐永康
* @date 2026-07-10
* @return 成功返回版本信息，失败返回带接口名的读取错误。
* @calls 具体实现调用 SDK 版本与控制器软件信息接口。
* @safety 只读，不触发运动。
* @note firmware_version 的具体来源由适配器文档明确。
*/
virtual common::Result<common::RobotSoftwareInfo> getRobotSoftwareInfo() = 0;
/**
* @brief 读取七个关节角、TCP 位姿和非零机械臂错误码。
* @reason 轨迹采样和运动前首点检查需要同一份带明确单位的状态快照。
* @author 唐永康
* @date 2026-07-10
* @return 成功返回当前状态，失败返回带接口名的读取错误。
* @calls 具体实现调用厂商当前机械臂状态接口。
* @safety 只读，不触发运动。
* @note 关节角单位为 degree，位置为 meter，姿态为 radian。
*/
virtual common::Result<common::ArmState> getCurrentArmState() = 0; virtual common::Result<common::ArmState> getCurrentArmState() = 0;
/**
* @brief 读取六维力传感器的四组力和力矩数据。
* @reason 真实运动前检查需要可验证的当前末端受力状态。
* @author 唐永康
* @date 2026-07-10
* @return 成功返回力数据，失败返回带接口名的读取错误。
* @calls 具体实现调用厂商六维力查询接口。
* @safety 只读，不触发运动。
* @note 力单位为 newton，力矩单位为 newton_meter。
*/
virtual common::Result<common::ForceData> getForceData() = 0; virtual common::Result<common::ForceData> getForceData() = 0;
// 关节空间运动。joints_deg 是绝对关节角，单位为度。 /**
* @brief 聚合运行模式、上电、关节、控制器和安全功能状态。
* @reason 安全门控需要一次 fail-closed 的厂商无关状态快照。
* @author 唐永康
* @date 2026-07-10
* @return 全部必要查询成功时返回状态，任一查询失败时返回失败。
* @calls 具体实现调用模式、电源、全状态、控制器、虚拟墙和自碰撞查询接口。
* @safety 只读，不触发运动；不得推断 SDK 未提供的急停状态。
* @note virtual_wall_enabled 和 self_collision_enabled 仅表示厂商原始返回值。
*/
virtual common::Result<common::RobotSafetyStatus> getRobotSafetyStatus() = 0;
/**
* @brief 查询控制器当前生效的 RM75 七轴位置、速度和加速度限制。
* @reason 运动前检查和离线轨迹校验必须使用设备实际限制，不能凭记忆硬编码。
* @author 唐永康
* @date 2026-07-10
* @return 全部四组限制读取成功时返回七轴限制，否则返回具体厂商查询错误。
* @calls 具体实现调用厂商关节最小/最大位置、最大速度和最大加速度查询接口。
* @safety 只读，不触发运动，也不会修改控制器限制。
* @note RM_API2 v1.1.5 的加速度 getter/setter 单位注释不一致；本接口按函数语义使用 degree/second^2，Live 验收前仍需与厂商确认。
*/
virtual common::Result<common::RobotJointLimits> getJointLimits() = 0;
/**
* @brief 发送 RM75 七关节绝对角度运动指令。
* @reason 将厂商数组转换和长度校验集中在适配器边界。
* @author 唐永康
* @date 2026-07-10
* @param joints_deg 七个绝对关节角，单位 degree。
* @param speed_percent 厂商速度百分比。
* @param blend_radius 厂商轨迹交融半径参数。
* @param trajectory_connect 厂商轨迹连接标志。
* @param blocking 是否等待运动结束。
* @return 成功返回空结果，失败返回校验或厂商接口错误。
* @calls 具体实现调用厂商关节运动接口。
* @safety 会触发真实运动，必须由上层状态机和安全门控授权。
* @note 本接口不替代轨迹限位、速度、加速度和工作空间校验。
*/
virtual common::Result<void> moveJ(const std::vector<float> &joints_deg, virtual common::Result<void> moveJ(const std::vector<float> &joints_deg,
int speed_percent, int speed_percent,
int blend_radius, int blend_radius,
int trajectory_connect, int trajectory_connect,
bool blocking) = 0; bool blocking) = 0;
// 笛卡尔直线运动。pose 是绝对 TCP 位姿，单位见 common::Pose。 /**
* @brief 发送绝对 TCP 直线运动指令。
* @reason 将统一 Pose 显式转换为厂商位姿结构，防止单位与 SDK 类型泄漏。
* @author 唐永康
* @date 2026-07-10
* @param pose 目标 TCP 位姿，位置 meter，欧拉姿态 radian。
* @param speed_percent 厂商速度百分比。
* @param blend_radius 厂商轨迹交融半径参数。
* @param trajectory_connect 厂商轨迹连接标志。
* @param blocking 是否等待运动结束。
* @return 成功返回空结果，失败返回校验或厂商接口错误。
* @calls 具体实现调用厂商直线运动接口。
* @safety 会触发真实运动，必须由上层状态机和安全门控授权。
* @note 本接口不替代工作空间、虚拟墙和力限制校验。
*/
virtual common::Result<void> moveL(const common::Pose &pose, virtual common::Result<void> moveL(const common::Pose &pose,
int speed_percent, int speed_percent,
int blend_radius, int blend_radius,
int trajectory_connect, int trajectory_connect,
bool blocking) = 0; bool blocking) = 0;
// 拖动示教生命周期。上层只表达“开始/停止/保存”，具体使用普通拖动、 /**
// 六维力复合拖动或厂商文件格式都由 driver 实现。 * @brief 启动普通记录拖动或六维力位置姿态拖动示教。
* @reason 上层只表达拖动语义，厂商模式整数由适配器集中维护。
* @author 唐永康
* @date 2026-07-10
* @param options 拖动模式、厂商记录开关和奇异墙选项。
* @return 成功返回空结果，失败返回厂商接口错误。
* @calls 普通模式或六维力复合拖动厂商接口。
* @safety 会改变真实机械臂控制模式，必须完成硬件安全门控和人工确认。
* @note 六维力接口不提供厂商轨迹记录参数，保存能力只能 best-effort。
*/
virtual common::Result<void> startDragTeach(const common::DragTeachStartOptions &options) = 0; virtual common::Result<void> startDragTeach(const common::DragTeachStartOptions &options) = 0;
/**
* @brief 停止当前拖动示教。
* @reason 采样成功或失败后都必须通过统一接口退出拖动状态。
* @author 唐永康
* @date 2026-07-10
* @return 成功返回空结果，失败返回厂商接口错误。
* @calls 具体实现调用厂商停止拖动示教接口。
* @safety 属于减险操作；拖动已启动时应优先尝试执行。
* @note 上层应保留采样错误和停止错误两者的上下文。
*/
virtual common::Result<void> stopDragTeach() = 0; virtual common::Result<void> stopDragTeach() = 0;
virtual common::Result<common::SavedVendorTrajectory>
saveDragTeachTrajectory(const std::string &path) = 0; /**
* @brief 将控制器内最后一次拖动轨迹保存到指定原始文件。
* @reason manifest 必须保留实际返回码，即使厂商保存失败也不能丢失调用记录。
* @author 唐永康
* @date 2026-07-10
* @param path 新建会话目录中的厂商原始轨迹路径。
* @return 返回完整保存尝试，包括是否调用、返回码、点数、路径和错误文本。
* @calls 具体实现调用厂商轨迹保存接口。
* @safety 不触发运动，但要求有效连接和已结束的拖动记录。
* @note 调用方必须保证目标路径唯一且不覆盖已有原始文件。
*/
virtual common::VendorTrajectorySaveAttempt
saveDragTeachTrajectory(const std::filesystem::path &path) = 0;
/**
* @brief 控制机械臂移动到控制器内最后一次拖动轨迹的起点。
* @reason 厂商原生回放要求先到达记录起点，故作为独立受控步骤暴露。
* @author 唐永康
* @date 2026-07-10
* @param blocking 是否等待运动结束。
* @return 成功返回空结果，失败返回厂商接口错误。
* @calls 具体实现调用厂商拖动轨迹起点接口。
* @safety 会以厂商固定 20% 速度触发真实运动，必须通过完整安全门控。
* @note 安全配置允许速度低于 20% 时，上层必须拒绝调用。
*/
virtual common::Result<void> moveToDragTrajectoryOrigin(bool blocking) = 0;
/**
* @brief 回放控制器内最后一次拖动示教轨迹。
* @reason 保持厂商原生回放能力，同时由服务状态机实施强制门控。
* @author 唐永康
* @date 2026-07-10
* @param blocking 是否等待回放结束。
* @return 成功返回空结果，失败返回厂商接口错误。
* @calls 具体实现调用厂商拖动轨迹回放接口。
* @safety 会触发真实运动，必须通过完整安全门控且已到达轨迹起点。
* @note 该接口不能从磁盘重新加载任意轨迹文件。
*/
virtual common::Result<void> replayLastDragTrajectory(bool blocking) = 0;
/**
* @brief 暂停正在执行的厂商拖动轨迹回放。
* @reason 将暂停纳入服务状态机，禁止业务层绕过流程直接调用 SDK。
* @author 唐永康
* @date 2026-07-10
* @return 成功返回空结果，失败返回厂商接口错误。
* @calls 具体实现调用厂商暂停回放接口。
* @safety 属于减险操作，应允许在授权状态变化后继续尝试。
* @note 仅在回放状态有效。
*/
virtual common::Result<void> pauseDragTrajectoryReplay() = 0;
/**
* @brief 继续已暂停的厂商拖动轨迹回放。
* @reason 继续动作会恢复真实运动，必须与暂停接口采用不同门控策略。
* @author 唐永康
* @date 2026-07-10
* @return 成功返回空结果，失败返回厂商接口错误。
* @calls 具体实现调用厂商继续回放接口。
* @safety 会恢复真实运动，必须重新通过完整安全门控。
* @note 仅在暂停状态有效。
*/
virtual common::Result<void> continueDragTrajectoryReplay() = 0;
/**
* @brief 停止厂商拖动轨迹回放。
* @reason 所有回放退出路径都必须通过统一减险操作终止运动。
* @author 唐永康
* @date 2026-07-10
* @return 成功返回空结果，失败返回厂商接口错误。
* @calls 具体实现调用厂商停止回放接口。
* @safety 属于减险操作，应允许在授权状态变化后继续尝试。
* @note 停止后是否需要回安全位由更高层任务状态机决定。
*/
virtual common::Result<void> stopDragTrajectoryReplay() = 0;
/**
* @brief 仅保存磁盘中的 RM75 厂商拖动轨迹程序，不启动运动。
* @reason 反向返回和正向复现必须在运动前分别占用独立程序槽，避免上传即运动。
* @author 唐永康
* @date 2026-07-11
* @param request 已校验的文件路径、保存速度百分比和控制器程序编号。
* @return 槽位为空、保存成功且保存后能查询到该编号时返回成功。
* @calls 具体实现调用 RM_API2 工程发送和在线编程列表查询接口。
* @safety 固定 only_save=1，不触发机械臂运动，也不覆盖已有控制器程序。
* @note 该接口不解析轨迹内容；调用方仍须提前完成全量轨迹校验。
*/
virtual common::Result<void> saveVendorDragTrajectoryProgram(
const common::VendorDragTrajectoryExecutionRequest &request) = 0;
/**
* @brief 运行控制器中已保存的指定程序。
* @reason 将程序保存与真实运动解耦，使上层可先确认两个程序槽均准备完成。
* @author 唐永康
* @date 2026-07-11
* @param request 程序编号、显式速度百分比和阻塞方式。
* @return 控制器接受运行请求时返回成功，否则返回参数或 SDK 错误。
* @calls 具体实现调用 RM_API2 指定编号程序运行接口。
* @safety 会触发真实运动，必须由上层安全门控授权并校验当前位置接近首点。
* @note speed_percent 必须为 1..100；blocking=false 支持非阻塞状态监控。
*/
virtual common::Result<void> runControllerProgram(
const common::ControllerProgramRunRequest &request) = 0;
/**
* @brief 上传并执行磁盘中的 RM75 厂商拖动轨迹工程文件。
* @reason sourceTXT 文件不能由“回放控制器最后轨迹”接口加载，必须通过独立接口
* 明确表达本地文件、控制器程序编号和受限速度。
* @author 唐永康
* @date 2026-07-10
* @param request 已校验的文件路径、计划速度百分比和控制器程序编号。
* @return 上传并成功启动时返回成功，否则返回路径校验、控制器错误行或 SDK 错误。
* @calls 具体实现调用 RM_API2 工程发送接口，工程类型固定为拖动轨迹。
* @safety 会触发真实机械臂运动，调用前必须通过完整安全门控并到达轨迹首点。
* @note 该接口不解析或校验轨迹内容；调用方必须提前完成全量校验与校验值确认。
* @deprecated 新流程应先调用 saveVendorDragTrajectoryProgram，再调用 runControllerProgram；
* 本接口仅为兼容既有调用链保留。
*/
virtual common::Result<void> executeVendorDragTrajectoryFile(
const common::VendorDragTrajectoryExecutionRequest &request) = 0;
/**
* @brief 查询控制器在线编程的当前运行状态。
* @reason 本地任务状态机必须确认轨迹确实进入运行态并正常结束，不能仅凭上传成功推断完成。
* @author 唐永康
* @date 2026-07-10
* @return 成功返回厂商原始状态、程序编号、轨迹编号和速度；失败返回 SDK 查询错误。
* @calls 具体实现调用 RM_API2 在线编程运行状态查询接口。
* @safety 只读，不触发运动。
* @note 未知 raw_state 必须由上层 fail-closed 处理。
*/
virtual common::Result<common::ControllerProgramRunState>
getControllerProgramRunState() = 0;
/**
* @brief 以厂商慢停止方式终止当前机械臂运动。
* @reason 轨迹监控超时、未知状态或后续设备检查失败时需要统一减险出口。
* @author 唐永康
* @date 2026-07-10
* @return SDK 接受停止指令时成功，否则返回带接口名的错误。
* @calls 具体实现调用 RM_API2 慢停止接口。
* @safety 属于减险动作，不应被普通运动门控阻止。
* @note 调用成功只表示控制器接受请求，现场操作者仍须观察设备实际停止状态。
*/
virtual common::Result<void> slowStopMotion() = 0;
/**
* @brief 查询控制器程序编号是否尚未被已有工程占用。
* @reason 上传厂商轨迹会保存到控制器编号，必须在移动到首点前拒绝覆盖已有程序。
* @author 唐永康
* @date 2026-07-10
* @param controller_program_id 待使用的编号，有效范围 1..100。
* @return 查询成功时返回可用标志；列表查询或输入失败时返回带上下文错误。
* @calls 具体实现调用 RM_API2 在线编程列表查询接口。
* @safety 只读，不触发运动，也不删除或覆盖控制器文件。
* @note 上传前适配器仍须再次检查，以关闭预检与上传之间的竞争窗口。
*/
virtual common::Result<bool> isControllerProgramSlotAvailable(
int controller_program_id) = 0;
}; };
} // namespace rm_control::interfaces } // namespace rm_control::interfaces
RM/include/rm_control/interfaces/tool_modbus_bus.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include <cstddef>
#include <cstdint>
#include <vector>
namespace rm_control::interfaces {
/**
* @brief RM75 工具端 Modbus 总线的当前只读配置快照。
*
* @reason O6 readiness 必须依据实际工具端口、电压和通信参数，而不能由业务层用布尔值伪造。
*
* @author 唐永康
* @date 2026-07-10
*
* @safety `connected` 只表示总线连接事实，不代表 O6 已通过身份、温度和故障检查。
* @note `timeout_units` 沿用 RM_API2 的百毫秒单位。
*/
struct ToolModbusBusStatus {
bool connected = false;
int port = 0;
int mode = 0;
int baudrate = 0;
int voltage_type = 0;
int timeout_units = 0;
};
/**
* @brief RM75 工具端 Modbus RTU 会话的显式初始化参数。
* @reason 控制器保存的 mode 状态不能替代机械臂启动后的 `rm_set_modbus_mode` 调用，
* 因此每个真实 O6 会话必须在读写寄存器前主动初始化。
* @author 唐永康
* @date 2026-07-11
* @safety 配置端口和供电但不写 O6 动作寄存器，也不移动机械臂。
*/
struct ToolModbusBusConfiguration {
int port = 1;
int baudrate = 115200;
int voltage_type = 3;
int timeout_units = 10;
bool reset_existing_mode = true;
bool power_cycle_tool = false;
int power_off_delay_ms = 500;
int startup_delay_ms = 2000;
};
/**
* @brief 隔离 RM75 工具端 Modbus 实现的厂商无关接口。
*
* @reason LinkerHandAdapter 只应表达 O6 寄存器协议，RM_API2 句柄、结构体和函数必须留在
* RealMan 厂商适配层。
*
* @author 唐永康
* @date 2026-07-11
*
* @safety 读取方法不触发设备动作；写保持寄存器可能触发真实 O6 关节运动。
* @note 接口不负责自动重试，调用失败必须原样返回给上层状态机。
*/
class IToolModbusBus {
public:
/**
* @brief 销毁工具端 Modbus 总线接口对象。
*
* @reason 允许业务层通过接口安全释放具体总线实现。
*
* @author 唐永康
* @date 2026-07-10
*
* @return 析构函数无返回值且不得抛出异常。
*
* @calls 具体实现的析构函数。
* @safety 只释放资源，不发送新的 O6 动作命令。
* @note 具体实现负责自身连接资源的所有权。
*/
virtual ~IToolModbusBus() = default;
/**
* @brief 显式初始化 RM75 工具端供电和 Modbus RTU 主站会话。
* @reason RM_API2 要求机械臂启动后在任何寄存器读写前调用 Modbus 配置接口。
* @author 唐永康
* @date 2026-07-11
* @param configuration 端口、波特率、供电、超时和是否先关闭既有模式。
* @return 成功时返回控制器实际读回的总线状态；任一设置或读回不一致时失败。
* @calls 具体实现的工具电压、关闭/启动 Modbus 和状态查询接口。
* @safety 不发送 O6 位置、速度或转矩命令；可能重新初始化工具端通信会话。
*/
virtual common::Result<ToolModbusBusStatus> Configure(
const ToolModbusBusConfiguration &configuration) = 0;
/**
* @brief 读取 RM75 工具端 Modbus 和供电配置。
*
* @reason O6 readiness 必须确认端口 1、主站模式、115200 波特率、24V 和有效超时均已生效。
*
* @author 唐永康
* @date 2026-07-10
*
* @return 成功时返回当前总线状态；任一底层查询失败时返回带接口上下文的错误。
*
* @calls 具体 RealMan 实现的工具电压和 RS485 状态查询接口。
* @safety 只读，不触发机械臂或 O6 运动。
* @note 返回成功不代表目标 Modbus 从站已经响应。
*/
virtual common::Result<ToolModbusBusStatus> GetStatus() = 0;
/**
* @brief 使用 Modbus 功能码 04 读取单个输入寄存器。
* @reason RM_API2 的单寄存器接口和多寄存器接口走不同代码路径，诊断时交叉读取可区分
* O6 无响应与多寄存器解码问题。
* @author 唐永康
* @date 2026-07-11
* @param device_id Modbus RTU 从站地址，合法范围 1..247。
* @param address 输入寄存器 PDU 地址，不使用 30001 风格的人机显示地址。
* @return 成功时返回一个无符号 16 位寄存器值；失败时保留 RM_API2 返回码上下文。
* @calls 具体 RealMan 实现的单输入寄存器读取接口。
* @safety 只读，不触发机械臂或 O6 运动。
* @note 不自动重试，避免把连续超时误判为设备响应。
*/
virtual common::Result<std::uint16_t> ReadInputRegister(
std::uint8_t device_id,
std::uint16_t address) = 0;
/**
* @brief 使用 Modbus 功能码 04 读取连续输入寄存器。
*
* @reason O6 的位置、转矩、速度、温度、故障和身份均位于输入寄存器，集中接口可避免
* LinkerHand 协议层接触 RM_API2 参数结构体。
*
* @author 唐永康
* @date 2026-07-11
*
* @param device_id Modbus RTU 从站地址。
* @param start_address 首个输入寄存器地址。
* @param register_count 要读取的 16 位寄存器数量，必须大于零。
* @return 成功时返回按地址递增排列的寄存器值；通信失败时返回原始上下文。
*
* @calls 具体 RealMan 实现的多输入寄存器读取接口。
* @safety 只读，不触发机械臂或 O6 运动。
* @note 实现必须把 Modbus 字节序转换为 16 位寄存器值。
*/
virtual common::Result<std::vector<std::uint16_t>> ReadInputRegisters(
std::uint8_t device_id,
std::uint16_t start_address,
std::size_t register_count) = 0;
/**
* @brief 使用 Modbus 功能码 03 读取连续保持寄存器。
*
* @reason O6 速度和转矩在位置动作下发前必须精确读回，独立的功能码 03 接口使
* LinkerHand 协议层能够验证写入结果，同时继续隔离 RM_API2 类型。
*
* @author 唐永康
* @date 2026-07-11
*
* @param device_id Modbus RTU 从站地址。
* @param start_address 首个保持寄存器地址。
* @param register_count 要读取的 16 位寄存器数量，必须大于零。
* @return 成功时返回按地址递增排列的寄存器值；通信失败时返回带接口上下文的错误。
*
* @calls 具体 RealMan 实现的多保持寄存器读取接口。
* @safety 只读，不触发机械臂或 O6 运动。
* @note 实现必须把 Modbus 高低字节转换为 16 位寄存器值，且不得自动重试。
*/
virtual common::Result<std::vector<std::uint16_t>> ReadHoldingRegisters(
std::uint8_t device_id,
std::uint16_t start_address,
std::size_t register_count) = 0;
/**
* @brief 使用 Modbus 功能码 16 写入连续保持寄存器。
*
* @reason O6 六关节位置、转矩和速度命令位于保持寄存器 0..17，统一写接口使上层可在
* Mock 中验证地址、从站和数据，同时阻止厂商 API 泄漏。
*
* @author 唐永康
* @date 2026-07-11
*
* @param device_id Modbus RTU 从站地址。
* @param start_address 首个保持寄存器地址。
* @param values 按地址递增排列的 16 位寄存器值；调用期间只读且不转移所有权。
* @return 写入成功时返回成功；通信或设备拒绝时返回带上下文的失败。
*
* @calls 具体 RealMan 实现的多保持寄存器写接口。
* @safety 可能立即触发真实 O6 关节运动，调用前必须通过状态机和硬件安全门控。
* @note 本接口不自动重试，避免超时后重复执行不确定的手部动作。
*/
virtual common::Result<void> WriteHoldingRegisters(
std::uint8_t device_id,
std::uint16_t start_address,
const std::vector<std::uint16_t> &values) = 0;
};
} // namespace rm_control::interfaces
RM/include/rm_control/modules/trajectory/drag_trajectory_validator.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include "rm_control/common/robot_types.h"
#include <cstddef>
#include <vector>
namespace rm_control::modules::trajectory {
/**
* @brief RM75 拖动示教轨迹的关节运动安全上限。
*
* @reason 将可调安全限制作为显式输入，使校验算法不读取配置文件且能够在 Mock 测试中复现。
*
* @author 唐永康
* @date 2026-07-10
*
* @safety 三项限制都必须是有限正数；单位分别为 degree、degree/second 和 degree/second^2。
* @note 这些限制用于离线轨迹校验，不会改变机械臂控制器内部限制。
*/
struct DragTrajectoryValidationLimits {
double maximum_joint_jump_deg = 5.0;
double maximum_joint_velocity_deg_per_second = 30.0;
double maximum_joint_acceleration_deg_per_second_squared = 100.0;
};
/**
* @brief 通过校验的 RM75 拖动示教轨迹统计报告。
*
* @reason 在允许回放前保留计算得到的峰值和位置，便于 manifest、审计日志和回放报告复用。
*
* @author 唐永康
* @date 2026-07-10
*
* @safety 报告仅在 `ValidateDragTrajectory` 成功时有效，关节索引使用 0..6。
* @note 时间单位为 millisecond，运动指标单位与 `DragTrajectoryValidationLimits` 一致。
*/
struct DragTrajectoryValidationReport {
std::size_t sample_count = 0;
long long duration_ms = 0;
double observed_maximum_joint_jump_deg = 0.0;
double observed_maximum_joint_velocity_deg_per_second = 0.0;
double observed_maximum_joint_acceleration_deg_per_second_squared = 0.0;
std::size_t maximum_jump_sample_index = 0;
std::size_t maximum_jump_joint_index = 0;
std::size_t maximum_velocity_sample_index = 0;
std::size_t maximum_velocity_joint_index = 0;
std::size_t maximum_acceleration_sample_index = 0;
std::size_t maximum_acceleration_joint_index = 0;
};
/**
* @brief 校验 RM75 七关节拖动示教轨迹并计算运动峰值。
*
* @reason 在进入 DryRun 或硬件回放前一次性拒绝空轨迹、维度错误、非法浮点数、时间倒退及
* 超出跃变、速度或加速度限制的轨迹，避免播放线程中临时发现结构性错误。
*
* @author 唐永康
* @date 2026-07-10
*
* @param samples 已全部载入内存的轨迹点；关节角单位为 degree，时间为单调 elapsed millisecond。
* @param limits 关节跃变、速度和加速度的有限正数上限。
* @return 轨迹完全合规时返回峰值报告；任一输入或安全限制非法时返回带样本和关节上下文的失败。
*
* @calls 仅调用标准数学函数和本模块内部纯算法函数。
* @safety 本函数不连接设备、不触发运动；失败结果不得被转换为可回放状态。
* @note 相邻区间速度按各自时间差计算，加速度按两个区间中点之间的时间差计算。
*/
common::Result<DragTrajectoryValidationReport> ValidateDragTrajectory(
const std::vector<common::TrajectorySample> &samples,
const DragTrajectoryValidationLimits &limits);
} // namespace rm_control::modules::trajectory
RM/include/rm_control/modules/trajectory/raw_trajectory_store.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include <cstdint>
#include <filesystem>
#include <string>
namespace rm_control::modules::trajectory {
struct RawTrajectorySessionPaths {
std::filesystem::path directory;
std::filesystem::path raw_trajectory;
std::filesystem::path synchronized_csv;
std::filesystem::path manifest;
std::string session_id;
std::string saved_at_utc;
};
struct RawTrajectoryManifestMetadata {
std::string robot_model;
std::string sdk_version;
std::string firmware_version;
std::uint64_t trajectory_point_count = 0;
std::uint64_t recorded_sample_count = 0;
bool vendor_save_api_called = false;
int vendor_save_return_code = 0;
std::string vendor_save_error_message;
bool synchronized_csv_write_succeeded = false;
};
struct RawTrajectoryArtifact {
RawTrajectorySessionPaths paths;
RawTrajectoryManifestMetadata metadata;
bool raw_file_present = false;
std::uintmax_t raw_file_size_bytes = 0;
std::string raw_file_sha256;
std::string manifest_written_at_utc;
};
/**
* @brief 将操作者提供的会话名清理为安全的 ASCII 文件名片段。
*
* @reason 会话名来自外部输入，统一折叠非法字符、限制长度并规避 Windows 保留名，可以让同一轨迹目录在 Linux 和 Windows 上安全复用。
*
* @author 唐永康
* @date 2026-07-10
*
* @param requested_session_name 原始会话名；允许为空、包含空白或非 ASCII 字符。
* @return 最长 64 字符的非空 ASCII 名称，只包含字母、数字、下划线和连字符。
*
* @calls 不调用厂商接口；仅执行确定性的字符串清理。
* @safety 不访问文件系统，不连接机械臂，也不会触发任何硬件动作。
* @note 连续非法字符折叠为单个下划线；空结果使用 `trajectory`。
*/
std::string SanitizeRawTrajectorySessionName(const std::string &requested_session_name);
/**
* @brief 在 `storage_root/sessions` 下原子创建一个不会覆盖既有数据的唯一会话目录。
*
* @reason 厂商保存接口需要先拿到目标路径；先用 `create_directory` 独占目录，可以在并发采集和同名会话场景下保持原始轨迹不可覆盖。
*
* @author 唐永康
* @date 2026-07-10
*
* @param storage_root 轨迹存储根目录；不得为空，父目录可尚未存在。
* @param requested_session_name 操作者提供的会话名，函数会先进行安全清理。
* @return 成功时返回会话目录、原始轨迹、同步 CSV、manifest、会话 ID 和 UTC 保存时间；失败时返回文件系统上下文。
*
* @calls std::filesystem::create_directories 和 std::filesystem::create_directory。
* @safety 只创建新的本地目录，不连接机械臂，也不会触发任何硬件动作。
* @note 会话 ID 格式为 `<清理名>_<UTC毫秒>[_NNN]`，所有产物路径都位于新目录内。
*/
common::Result<RawTrajectorySessionPaths>
CreateUniqueRawTrajectorySession(const std::filesystem::path &storage_root,
const std::string &requested_session_name);
/**
* @brief 校验厂商原始文件、计算大小和 SHA-256、移除写权限并原子生成 manifest。
*
* @reason 把不可变保护和 manifest 固化放在同一收尾边界，可以确保成功或失败的厂商保存尝试都有可审计记录，同时不改写厂商原始内容。
*
* @author 唐永康
* @date 2026-07-10
*
* @param paths 由 `CreateUniqueRawTrajectorySession` 返回的受控会话路径。
* @param metadata 机械臂型号、SDK/固件版本、点数、同步样本数和厂商保存返回码。
* @return 成功时返回完整原始轨迹产物；即使厂商保存失败且原始文件不存在，也会成功生成 manifest。文件系统或摘要失败时返回上下文错误。
*
* @calls CalculateFileSha256、std::filesystem::file_size、std::filesystem::permissions 和 std::filesystem::create_hard_link。
* @safety 只读取并保护本地轨迹文件，不连接机械臂，也不会触发任何硬件动作。
* @note 已存在的 manifest 不会被覆盖；完成后原始文件、同步 CSV、manifest 和会话目录均移除全部写权限。
*/
common::Result<RawTrajectoryArtifact>
FinalizeRawTrajectorySession(const RawTrajectorySessionPaths &paths,
const RawTrajectoryManifestMetadata &metadata);
} // namespace rm_control::modules::trajectory
RM/include/rm_control/modules/trajectory/reversed_vendor_trajectory_store.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include "rm_control/modules/trajectory/vendor_drag_trajectory_parser.h"
#include <filesystem>
namespace rm_control::modules::trajectory {
/**
* @brief 返回运行期反向厂商轨迹文件的默认保存目录。
*
* @reason 反向轨迹是由只读原始轨迹派生的运行期产物，必须与 sourceTXT 原件分开保存，
* 避免误改、覆盖或把派生文件冒充原始记录。
*
* @author 唐永康
* @date 2026-07-11
*
* @return CMake 注入的 runtime_data/reversed_vendor_trajectories 路径。
*
* @calls 仅构造 std::filesystem::path，不访问文件系统。
* @safety 不读取轨迹、不连接设备，也不会触发 RM75 或 O6 动作。
*/
std::filesystem::path DefaultReversedVendorTrajectoryDirectory();
/**
* @brief 从已解析的只读厂商轨迹创建逐物理行严格倒序的只读副本。
*
* @reason RM75 位于原轨迹末点时，不能通过放大首点距离限制直接跨越到首点；本函数生成
* 可由控制器沿相同关节路点反向执行的独立 TXT 产物，同时保留原文件不变。
*
* @author 唐永康
* @date 2026-07-11
*
* @param forward_trajectory 已由 ParseVendorDragTrajectoryJsonLines 严格解析的正向轨迹；
* 源文件必须仍是无写权限、非符号链接的普通文件，大小和 SHA-256 必须与元数据一致。
* @param output_directory 反向产物根目录；函数在其下原子创建唯一子目录，绝不覆盖既有文件。
* @return 成功时返回对 0444 反向文件重新解析得到的完整元数据；失败时只清理本次新建产物。
*
* @calls CalculateFileSha256、ParseVendorDragTrajectoryJsonLines 和 std::filesystem 文件接口。
* @safety 仅执行离线文件读取、派生写入和校验，不连接硬件，也不发送任何运动指令。
* @note 输出保持每一物理行的字节内容和换行风格，只反转行顺序；重新解析后的第 i 个样本
* 必须逐字段等于正向轨迹的第 N-1-i 个关节样本。
*/
common::Result<VendorDragTrajectoryParseResult>
CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(
const VendorDragTrajectoryParseResult &forward_trajectory,
const std::filesystem::path &output_directory);
} // namespace rm_control::modules::trajectory
RM/include/rm_control/modules/trajectory/sha256_file_checksum.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include <filesystem>
#include <string>
namespace rm_control::modules::trajectory {
/**
* @brief 流式计算文件的 SHA-256 十六进制校验值。
*
* @reason 原始轨迹可能较大，流式摘要可以避免把完整文件加载到内存，并为只读原始产物提供可复核的完整性证据。
*
* @author 唐永康
* @date 2026-07-10
*
* @param file_path 待校验的普通文件路径；路径必须存在且不能是目录。
* @return 成功时返回 64 个小写十六进制字符，失败时返回带路径上下文的错误信息。
*
* @calls OpenSSL EVP_DigestInit_ex、EVP_DigestUpdate 和 EVP_DigestFinal_ex。
* @safety 只读取本地文件，不连接机械臂，也不会触发任何硬件动作。
* @note 文件在摘要计算期间不得被其他进程修改，否则调用方应重新计算并校验。
*/
common::Result<std::string> CalculateFileSha256(const std::filesystem::path &file_path);
} // namespace rm_control::modules::trajectory
RM/include/rm_control/modules/trajectory/trajectory_file_store.h
View file @ 8275ee2e
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
#include "rm_control/common/result.h" #include "rm_control/common/result.h"
#include "rm_control/common/robot_types.h" #include "rm_control/common/robot_types.h"
#include <filesystem>
#include <string> #include <string>
#include <vector> #include <vector>
...@@ -34,6 +35,26 @@ common::Result<TrajectoryFileSet> makeTrajectoryFileSet(const std::string &root_ ...@@ -34,6 +35,26 @@ common::Result<TrajectoryFileSet> makeTrajectoryFileSet(const std::string &root_
common::Result<void> writeTrajectoryCsv(const std::string &csv_path, common::Result<void> writeTrajectoryCsv(const std::string &csv_path,
const std::vector<common::TrajectorySample> &samples); const std::vector<common::TrajectorySample> &samples);
/**
* @brief 使用 `std::filesystem::path` 新建同步 RM75 轨迹 CSV，目标存在时拒绝覆盖。
*
* @reason 原始采集会话必须保持唯一且可追溯，不能沿用旧接口的截断覆盖语义。
*
* @author 唐永康
* @date 2026-07-10
*
* @param csv_path 新会话内的同步 CSV 路径；调用方保留路径所有权。
* @param samples 已全部保存在内存中的连续七关节样本。
* @return 新文件完整写入、刷新并关闭时成功；目标已存在或任何文件操作失败时返回上下文。
*
* @calls std::filesystem::create_directories、std::filesystem::exists 和 std::ofstream。
* @safety 只写新的本地 CSV，不连接设备，也不触发硬件动作。
* @note 当前格式使用 sample_index、timestamp_ms、J1..J7、TCP 和 source 共 16 列。
*/
common::Result<void> writeTrajectoryCsvWithoutOverwrite(
const std::filesystem::path &csv_path,
const std::vector<common::TrajectorySample> &samples);
common::Result<std::vector<common::TrajectorySample>> loadTrajectoryCsv(const std::string &csv_path); common::Result<std::vector<common::TrajectorySample>> loadTrajectoryCsv(const std::string &csv_path);
common::Result<void> exportTcpPathCpp(const std::string &cpp_path, common::Result<void> exportTcpPathCpp(const std::string &cpp_path,
......
RM/include/rm_control/modules/trajectory/trajectory_playback_report_store.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include <array>
#include <cstddef>
#include <cstdint>
#include <filesystem>
#include <string>
namespace rm_control::modules::trajectory {
inline constexpr std::size_t kPlaybackReportO6JointCount = 6U;
using PlaybackReportO6Values =
std::array<std::uint16_t, kPlaybackReportO6JointCount>;
struct ControllerManagedPlaybackReport {
std::string report_identifier;
std::filesystem::path source_file;
std::string source_file_sha256;
bool returned_from_trajectory_end = false;
std::filesystem::path return_source_file;
std::string return_source_file_sha256;
std::size_t trajectory_point_count = 0U;
std::int64_t sample_period_ms = 0;
std::int64_t source_duration_ms = 0;
bool timestamp_is_derived = true;
int plan_speed_percent = 0;
int controller_program_id = 0;
int return_controller_program_id = 0;
double return_observed_execution_duration_ms = 0.0;
std::size_t return_program_status_poll_count = 0U;
double return_minimum_poll_period_ms = 0.0;
double return_maximum_poll_period_ms = 0.0;
double return_average_poll_period_ms = 0.0;
double return_maximum_absolute_poll_jitter_ms = 0.0;
int return_final_controller_state = -1;
double observed_execution_duration_ms = 0.0;
std::size_t program_status_poll_count = 0U;
double minimum_poll_period_ms = 0.0;
double maximum_poll_period_ms = 0.0;
double average_poll_period_ms = 0.0;
double maximum_absolute_poll_jitter_ms = 0.0;
int final_controller_state = -1;
std::uint8_t o6_device_id = 0U;
PlaybackReportO6Values target_positions{};
PlaybackReportO6Values speed_commands{};
PlaybackReportO6Values torque_commands{};
PlaybackReportO6Values final_positions{};
};
/**
* @brief 返回构建目录中的 RM75/O6 控制器托管回放报告目录。
*
* @reason 运行数据必须与源码目录分离，集中默认目录也避免 Application 硬编码本机绝对路径。
*
* @author 唐永康
* @date 2026-07-11
*
* @return CMake 注入的 runtime_data/trajectory_playback_reports 路径。
*
* @calls 不访问文件系统。
* @safety 只构造路径，不连接设备，也不触发机械臂或 O6 动作。
* @note 测试可以显式传入其他目录，不依赖此默认值。
*/
std::filesystem::path DefaultTrajectoryPlaybackReportDirectory();
/**
* @brief 将一次厂商控制器托管回放与 O6 抓握结果保存为不可覆盖 JSON 报告。
*
* @reason 厂商控制器内部不暴露逐点下发时间，因此报告明确记录可观测的程序轮询周期、
* 总执行时长、轨迹摘要和 O6 命令/读回，避免把主机轮询统计误称为逐点 jitter。
*
* @author 唐永康
* @date 2026-07-11
*
* @param report 已完成任务的只读结构化结果，所有时间单位为 millisecond。
* @param output_directory 新报告所在目录；不存在时创建，已有同名报告时拒绝覆盖。
* @return 成功时返回新 JSON 文件路径；字段非法、目录或写入失败时返回上下文错误。
*
* @calls std::filesystem::create_directories 和 std::ofstream。
* @safety 只写新的构建目录报告，不连接设备，也不会继续、暂停或停止任何动作。
* @note `report_identifier` 由调用方生成且只允许字母、数字、下划线和连字符；
* `maximum_absolute_poll_jitter_ms` 是状态轮询抖动，不是控制器内部轨迹点下发抖动。
*/
common::Result<std::filesystem::path>
WriteControllerManagedPlaybackReportWithoutOverwrite(
const ControllerManagedPlaybackReport &report,
const std::filesystem::path &output_directory);
} // namespace rm_control::modules::trajectory
RM/include/rm_control/modules/trajectory/vendor_drag_trajectory_parser.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include "rm_control/common/robot_types.h"
#include <cstddef>
#include <cstdint>
#include <filesystem>
#include <string>
#include <vector>
namespace rm_control::modules::trajectory {
inline constexpr std::int64_t kRealmanVendorRawUnitsPerDegree = 1000;
inline constexpr const char *kRealmanVendorDragTrajectorySource =
"realman_rm_save_trajectory_jsonl";
struct VendorDragTrajectoryParseResult {
std::filesystem::path source_file;
std::vector<common::TrajectorySample> samples;
std::size_t trajectory_point_count = 0U;
std::uintmax_t source_file_size_bytes = 0U;
std::string source_file_sha256;
std::string source = kRealmanVendorDragTrajectorySource;
std::int64_t sample_period_ms = 0;
std::int64_t raw_units_per_degree = kRealmanVendorRawUnitsPerDegree;
bool timestamp_is_derived = true;
};
/**
* @brief 严格解析 RealMan 厂商拖动轨迹 JSONL，并生成 RM75 七轴工程样本。
*
* @reason 厂商原始文件只有七个整数关节值，没有时间戳和单位元数据；调用方必须显式提供采样周期，解析器再以可追溯比例转换为 degree 并标记时间戳为推导值。
*
* @author 唐永康
* @date 2026-07-10
*
* @param file_path 厂商原始 JSONL 普通文件；本函数只读该路径且拒绝符号链接、目录和空文件。
* @param sample_period_ms 相邻样本的明确采样周期，单位 millisecond，必须大于 0。
* @param raw_units_per_degree 每 degree 对应的厂商整数单位数，必须大于 0；本项目当前默认显式声明为 1000。
* @return 成功时返回七轴 degree 样本、推导时间戳、点数、原文件字节数与 SHA-256；任何格式、范围、文件一致性或 I/O 错误均失败。
*
* @calls CalculateFileSha256、std::filesystem::symlink_status、file_size 和 last_write_time。
* @safety 仅离线读取和转换文件，不连接机械臂，也不会触发拖动示教或轨迹回放。
* @note 每个物理行必须严格为 `{"point":[七个 JSON 整数]}`，空行也会失败；原格式没有 TCP 位姿，因此输出样本的 tcp_pose 保持默认零值，不能据此执行 TCP 回放。
*/
common::Result<VendorDragTrajectoryParseResult> ParseVendorDragTrajectoryJsonLines(
const std::filesystem::path &file_path,
std::int64_t sample_period_ms,
std::int64_t raw_units_per_degree = kRealmanVendorRawUnitsPerDegree);
} // namespace rm_control::modules::trajectory
RM/src/config/drag_teach_cli_config.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file drag_teach_cli_config.cpp
* @brief RM75 拖动示教录制程序命令行参数解析实现。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件实现 `rm_drag_teach_record` 的参数解析。拖动示教属于真实硬件流程，
* 因此解析阶段必须保持纯文本处理，不连接 SDK、不启动拖动、不写轨迹文件。
* 入口文件拿到结构化配置后，再显式执行连接和录制流程。
*
* 设计原则：
* - 保持原有位置参数顺序；
* - 所有解析失败通过 `Result` 返回；
* - 不在解析函数中调用 `std::exit`；
* - 默认参数使用安全、可追踪的命名和路径规则。
*
* @dependencies
* - rm_control/config/drag_teach_cli_config.h
* - rm_control/modules/trajectory/trajectory_file_store.h
*/
#include "rm_control/config/drag_teach_cli_config.h"
#include "rm_control/modules/trajectory/trajectory_file_store.h"
#include <cerrno>
#include <cstdlib>
#include <limits>
#include <ostream>
#include <string>
namespace rm_control::config {
namespace {
/**
* @brief 将进程名转换为可显示文本。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* usage 输出需要稳定的程序名。该函数统一处理空指针和空字符串，
* 防止错误路径输出空 usage，影响用户排查命令行参数问题。
*
* @param[in] program_name `argv[0]`，允许为空。
* @param[in] fallback_name 默认程序名。
*
* @return 可显示的程序名。
*
* @throws 本函数不主动抛出异常；标准库字符串分配失败由调用方处理。
*
* @note
* 不做路径截断，保留 shell 传入的显示形式。
*
* @warning
* 不要在此函数中访问文件系统或环境变量。
*
* @thread_safety
* 函数只使用局部变量，线程安全。
*
* @par 边界条件
* 空指针或空字符串返回 fallback。
*
* @par 后续维护
* 若 CLI 模块数量继续增加，可统一抽到公共工具模块。
*/
std::string SafeProgramName(const char *program_name, const char *fallback_name) {
if (program_name == nullptr || program_name[0] == '\0') {
return fallback_name;
}
return program_name;
}
/**
* @brief 解析十进制整数字符串。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 拖动示教 CLI 中的录制时长和采样周期都来自用户输入。该函数集中处理空指针、
* 空字符串、非数字尾缀和输出指针为空等边界，避免调用方重复写不完整的校验逻辑。
*
* @param[in] value 待解析的 C 字符串，允许为空指针。
* @param[out] out 解析结果输出指针，不能为空。
*
* @return true 解析成功。
* @return false 输入为空、格式错误或输出指针为空。
*
* @throws 本函数不主动抛出异常；`std::strtol` 不抛异常。
*
* @note
* 数值范围的业务校验由 `DragTeachRecorder::validateConfig()` 负责。
*
* @warning
* 超出 int 范围或 `strtol` 报告 ERANGE 时必须拒绝，不能截断为运动参数。
*
* @thread_safety
* 函数只使用局部变量，线程安全。
*
* @par 边界条件
* `value == nullptr`、空字符串、包含非数字字符时返回 false。
*
* @par 后续维护
* 如需解析浮点或带单位的参数，应新增专用函数，不要扩展本函数职责。
*/
bool ParseDecimalInteger(const char *value, int *out) {
if (value == nullptr || out == nullptr || value[0] == '\0') {
return false;
}
errno = 0;
char *end = nullptr;
const long parsed_value = std::strtol(value, &end, 10);
if (errno == ERANGE || end == value || *end != '\0' ||
parsed_value < std::numeric_limits<int>::min() ||
parsed_value > std::numeric_limits<int>::max()) {
return false;
}
*out = static_cast<int>(parsed_value);
return true;
}
/**
* @brief 生成默认拖动示教会话名。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 默认会话名使用 `rm75_drag_pose_` 前缀加轨迹模块提供的时间戳。这样录制产物
* 在同一目录下不容易互相覆盖，也能从文件名追踪录制时间。
*
* @param 无输入参数。
*
* @return 默认会话名字符串。
*
* @throws 本函数不主动抛出异常；字符串拼接失败属于标准库内存异常。
*
* @note
* 时间戳格式由 trajectory 模块维护。
*
* @warning
* 不要在这里访问真实机械臂状态，默认命名应保持纯本地逻辑。
*
* @thread_safety
* 依赖 `timestampString()`，该函数使用线程安全的 `localtime_r`。
*
* @par 边界条件
* 系统时间不可用时由 C 运行库返回当前可用时间值。
*
* @par 后续维护
* 如需加入操作者或任务名，应通过显式参数传入，不应读取全局配置。
*/
std::string BuildDefaultSessionName() {
return "rm75_drag_pose_" + rm_control::modules::trajectory::timestampString();
}
/**
* @brief 创建拖动示教 CLI 的默认配置。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数集中设置拖动示教程序默认值，包括会话名、存储根目录、拖动模式、
* 奇异墙和厂商轨迹保存开关。集中默认值可以避免入口文件和测试各自构造不一致配置。
*
* @param 无输入参数。
*
* @return 带默认值的 `DragTeachRecordCliConfig`。
*
* @throws 本函数不主动抛出异常；字符串构造失败由标准库报告。
*
* @note
* 默认模式为 RM75 普通记录拖动示教，六维力复合模式必须显式请求。
*
* @warning
* 默认值不能绕过后续 `DragTeachRecorder` 的安全校验。
*
* @thread_safety
* 函数使用局部对象，线程安全。
*
* @par 边界条件
* 默认运行数据路径由编译期 `RM_RUNTIME_DATA_ROOT` 推导，不写入源码目录。
*
* @par 后续维护
* 新增默认值时应同步 CLI 测试。
*/
DragTeachRecordCliConfig BuildDefaultDragTeachCliConfig() {
DragTeachRecordCliConfig cli_config;
cli_config.record_config.session_name = BuildDefaultSessionName();
cli_config.record_config.storage_root =
rm_control::modules::trajectory::defaultTrajectoryStorageRoot();
cli_config.record_config.drag_options.mode =
rm_control::common::DragTeachMode::OrdinaryRecorded;
cli_config.record_config.drag_options.enable_singular_wall = true;
cli_config.record_config.drag_options.record_vendor_trajectory = true;
cli_config.record_config.save_vendor_trajectory = true;
cli_config.safety_config_path =
std::filesystem::path(RM_PROJECT_ROOT) / "config" / "drag_teach_safety.json";
return cli_config;
}
} // namespace
void PrintDragTeachRecordUsage(const char *program_name, std::ostream &out) {
const std::string display_name = SafeProgramName(program_name, "rm_drag_teach_record");
out << "Usage: " << display_name
<< " [session_name] [duration_seconds] [sample_period_ms] [storage_root]"
<< " [--live] [--safety-config <path>] [--six-dof-force]"
<< " [--no-vendor-save] [--ordinary-recorded] [--no-singular-wall]\n"
<< "\n"
<< "Default mode is ordinary recorded drag teach so vendor save/replay has an explicit SDK contract.\n"
<< "Six-dof force mode is best-effort for vendor save because its SDK call has no record flag.\n"
<< "The program records synchronized CSV as the primary artifact.\n"
<< "--no-vendor-save is retained only for legacy parsing and is rejected by DragTeachService.\n"
<< "Safety: default is DryRun/refuse. Live use requires --live, enabled config,"
<< " runtime device readiness, and operator confirmation.\n";
}
common::Result<DragTeachRecordCliConfig> ParseDragTeachRecordCliArguments(int argc, char **argv) {
DragTeachRecordCliConfig cli_config = BuildDefaultDragTeachCliConfig();
if (argc < 0 || (argc > 0 && argv == nullptr)) {
return common::Result<DragTeachRecordCliConfig>::failure(
"invalid argc/argv input");
}
if (argc == 2) {
const std::string first_argument = argv[1] == nullptr ? "" : argv[1];
if (first_argument == "-h" || first_argument == "--help") {
cli_config.help_requested = true;
return common::Result<DragTeachRecordCliConfig>::success(cli_config);
}
}
int positional_index = 0;
for (int argument_index = 1; argument_index < argc; ++argument_index) {
const std::string argument = argv[argument_index] == nullptr ? "" : argv[argument_index];
if (argument == "--live") {
cli_config.live_requested = true;
continue;
}
if (argument == "--safety-config") {
if (argument_index + 1 >= argc || argv[argument_index + 1] == nullptr ||
argv[argument_index + 1][0] == '\0') {
return common::Result<DragTeachRecordCliConfig>::failure(
"--safety-config requires a path");
}
cli_config.safety_config_path = argv[++argument_index];
continue;
}
if (argument == "--six-dof-force") {
cli_config.record_config.drag_options.mode =
rm_control::common::DragTeachMode::SixDofForcePositionAndOrientation;
continue;
}
if (argument == "--no-vendor-save") {
cli_config.record_config.save_vendor_trajectory = false;
continue;
}
if (argument == "--ordinary-recorded") {
cli_config.record_config.drag_options.mode =
rm_control::common::DragTeachMode::OrdinaryRecorded;
continue;
}
if (argument == "--no-singular-wall") {
cli_config.record_config.drag_options.enable_singular_wall = false;
continue;
}
if (!argument.empty() && argument[0] == '-') {
return common::Result<DragTeachRecordCliConfig>::failure("unknown option: " + argument);
}
++positional_index;
if (positional_index == 1) {
cli_config.record_config.session_name = argument;
} else if (positional_index == 2) {
if (!ParseDecimalInteger(argument.c_str(), &cli_config.record_config.duration_seconds)) {
return common::Result<DragTeachRecordCliConfig>::failure(
"invalid duration_seconds: " + argument);
}
} else if (positional_index == 3) {
if (!ParseDecimalInteger(argument.c_str(), &cli_config.record_config.sample_period_ms)) {
return common::Result<DragTeachRecordCliConfig>::failure(
"invalid sample_period_ms: " + argument);
}
} else if (positional_index == 4) {
cli_config.record_config.storage_root = argument;
} else {
return common::Result<DragTeachRecordCliConfig>::failure(
"too many positional arguments");
}
}
return common::Result<DragTeachRecordCliConfig>::success(cli_config);
}
} // namespace rm_control::config
RM/src/config/drag_teach_safety_config.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/config/drag_teach_safety_config.h"
#include "rm_control/config/simple_json.h"
#include <cmath>
#include <fstream>
#include <sstream>
#include <string>
namespace rm_control::config {
namespace {
common::Result<std::string> ReadTextFile(const std::filesystem::path &path) {
std::ifstream input(path);
if (!input) {
return common::Result<std::string>::failure(
"failed to open drag teach safety config: " + path.string());
}
std::ostringstream buffer;
buffer << input.rdbuf();
if (!input.good() && !input.eof()) {
return common::Result<std::string>::failure(
"failed to read drag teach safety config: " + path.string());
}
return common::Result<std::string>::success(buffer.str());
}
common::Result<bool> ReadRequiredBool(const JsonValue &root, const std::string &name) {
const JsonValue *value = root.find(name);
if (value == nullptr || !value->isBool()) {
return common::Result<bool>::failure(
"drag teach safety field '" + name + "' must be bool");
}
return common::Result<bool>::success(value->asBool());
}
common::Result<double> ReadRequiredNumber(const JsonValue &root, const std::string &name) {
const JsonValue *value = root.find(name);
if (value == nullptr || !value->isNumber() || !std::isfinite(value->asNumber())) {
return common::Result<double>::failure(
"drag teach safety field '" + name + "' must be a finite number");
}
return common::Result<double>::success(value->asNumber());
}
common::Result<int> ReadRequiredIntegerInRange(const JsonValue &root,
const std::string &name,
int minimum_value,
int maximum_value) {
const auto value = ReadRequiredNumber(root, name);
if (!value.ok) {
return common::Result<int>::failure(value.error_message);
}
if (std::floor(value.value) != value.value) {
return common::Result<int>::failure(
"drag teach safety field '" + name + "' must be an integer");
}
if (value.value < minimum_value || value.value > maximum_value) {
return common::Result<int>::failure(
"drag teach safety field '" + name + "' must be in range " +
std::to_string(minimum_value) + ".." + std::to_string(maximum_value));
}
return common::Result<int>::success(static_cast<int>(value.value));
}
common::Result<void> AssignBooleanFields(const JsonValue &root,
DragTeachSafetyConfig *config) {
const auto hardware_enabled = ReadRequiredBool(root, "hardware_execution_enabled");
const auto emergency_stop = ReadRequiredBool(root, "emergency_stop_verified");
const auto limits_loaded = ReadRequiredBool(root, "safety_limits_loaded");
const auto workspace = ReadRequiredBool(root, "workspace_and_virtual_wall_verified");
const auto native_replay =
ReadRequiredBool(root, "native_replay_without_speed_control_accepted");
const auto sample_period_confirmed =
ReadRequiredBool(root, "trajectory_sample_period_confirmed");
const auto raw_units_confirmed =
ReadRequiredBool(root, "vendor_raw_units_per_degree_confirmed");
if (!hardware_enabled.ok || !emergency_stop.ok || !limits_loaded.ok ||
!workspace.ok || !native_replay.ok || !sample_period_confirmed.ok ||
!raw_units_confirmed.ok) {
const std::string error = !hardware_enabled.ok ? hardware_enabled.error_message
: !emergency_stop.ok ? emergency_stop.error_message
: !limits_loaded.ok ? limits_loaded.error_message
: !workspace.ok ? workspace.error_message
: !native_replay.ok ? native_replay.error_message
: !sample_period_confirmed.ok
? sample_period_confirmed.error_message
: raw_units_confirmed.error_message;
return common::Result<void>::failure(error);
}
config->hardware_execution_enabled = hardware_enabled.value;
config->emergency_stop_verified = emergency_stop.value;
config->safety_limits_loaded = limits_loaded.value;
config->workspace_and_virtual_wall_verified = workspace.value;
config->native_replay_without_speed_control_accepted = native_replay.value;
config->trajectory_sample_period_confirmed = sample_period_confirmed.value;
config->vendor_raw_units_per_degree_confirmed = raw_units_confirmed.value;
return common::Result<void>::success();
}
common::Result<void> AssignCommandLimits(const JsonValue &root,
DragTeachSafetyConfig *config) {
const auto max_speed =
ReadRequiredIntegerInRange(root, "max_speed_percent", 1, 100);
const auto raw_units =
ReadRequiredIntegerInRange(root, "vendor_raw_units_per_degree", 1, 1000000);
const auto maximum_o6_speed =
ReadRequiredIntegerInRange(root, "maximum_o6_speed_command", 0, 255);
const auto maximum_o6_torque =
ReadRequiredIntegerInRange(root, "maximum_o6_torque_command", 0, 255);
if (!max_speed.ok || !raw_units.ok || !maximum_o6_speed.ok ||
!maximum_o6_torque.ok) {
return common::Result<void>::failure(
!max_speed.ok ? max_speed.error_message
: !raw_units.ok ? raw_units.error_message
: !maximum_o6_speed.ok ? maximum_o6_speed.error_message
: maximum_o6_torque.error_message);
}
config->max_speed_percent = max_speed.value;
config->vendor_raw_units_per_degree = raw_units.value;
config->maximum_o6_speed_command =
static_cast<std::uint16_t>(maximum_o6_speed.value);
config->maximum_o6_torque_command =
static_cast<std::uint16_t>(maximum_o6_torque.value);
return common::Result<void>::success();
}
common::Result<void> AssignTrajectoryAndForceLimits(
const JsonValue &root,
DragTeachSafetyConfig *config) {
const auto maximum_origin_delta =
ReadRequiredNumber(root, "maximum_origin_move_delta_deg");
const auto max_jump = ReadRequiredNumber(root, "max_joint_jump_deg");
const auto max_velocity =
ReadRequiredNumber(root, "max_joint_velocity_deg_per_second");
const auto max_acceleration =
ReadRequiredNumber(root, "max_joint_acceleration_deg_per_second_squared");
const auto maximum_force =
ReadRequiredNumber(root, "maximum_absolute_force_newton");
const auto maximum_torque =
ReadRequiredNumber(root, "maximum_absolute_torque_newton_meter");
if (!maximum_origin_delta.ok || !max_jump.ok || !max_velocity.ok ||
!max_acceleration.ok ||
!maximum_force.ok || !maximum_torque.ok) {
const std::string error = !maximum_origin_delta.ok
? maximum_origin_delta.error_message
: !max_jump.ok ? max_jump.error_message
: !max_velocity.ok ? max_velocity.error_message
: !max_acceleration.ok ? max_acceleration.error_message
: !maximum_force.ok ? maximum_force.error_message
: maximum_torque.error_message;
return common::Result<void>::failure(error);
}
config->maximum_origin_move_delta_deg =
static_cast<float>(maximum_origin_delta.value);
config->max_joint_jump_deg = static_cast<float>(max_jump.value);
config->max_joint_velocity_deg_per_second = static_cast<float>(max_velocity.value);
config->max_joint_acceleration_deg_per_second_squared =
static_cast<float>(max_acceleration.value);
config->maximum_absolute_force_newton =
static_cast<float>(maximum_force.value);
config->maximum_absolute_torque_newton_meter =
static_cast<float>(maximum_torque.value);
return common::Result<void>::success();
}
common::Result<void> AssignNumericFields(const JsonValue &root,
DragTeachSafetyConfig *config) {
const auto command_limits = AssignCommandLimits(root, config);
if (!command_limits.ok) {
return command_limits;
}
return AssignTrajectoryAndForceLimits(root, config);
}
common::Result<void> ValidateSafetyConfig(const DragTeachSafetyConfig &config) {
if (config.max_speed_percent <= 0 || config.max_speed_percent > 100) {
return common::Result<void>::failure("max_speed_percent must be in range 1..100");
}
if (!std::isfinite(config.maximum_origin_move_delta_deg) ||
!std::isfinite(config.max_joint_jump_deg) ||
!std::isfinite(config.max_joint_velocity_deg_per_second) ||
!std::isfinite(config.max_joint_acceleration_deg_per_second_squared) ||
!std::isfinite(config.maximum_absolute_force_newton) ||
!std::isfinite(config.maximum_absolute_torque_newton_meter) ||
config.maximum_origin_move_delta_deg <= 0.0F ||
config.maximum_origin_move_delta_deg > 180.0F ||
config.max_joint_jump_deg <= 0.0F ||
config.max_joint_velocity_deg_per_second <= 0.0F ||
config.max_joint_acceleration_deg_per_second_squared <= 0.0F ||
config.maximum_absolute_force_newton <= 0.0F ||
config.maximum_absolute_torque_newton_meter <= 0.0F) {
return common::Result<void>::failure(
"origin, trajectory and force safety limits must be positive; "
"maximum_origin_move_delta_deg must not exceed 180");
}
return common::Result<void>::success();
}
} // namespace
common::Result<DragTeachSafetyConfig>
LoadDragTeachSafetyConfig(const std::filesystem::path &config_path) {
if (config_path.empty()) {
return common::Result<DragTeachSafetyConfig>::failure(
"drag teach safety config path must not be empty");
}
const auto text = ReadTextFile(config_path);
if (!text.ok) {
return common::Result<DragTeachSafetyConfig>::failure(text.error_message);
}
const auto json = parseJson(text.value);
if (!json.ok || !json.value.isObject()) {
return common::Result<DragTeachSafetyConfig>::failure(
"invalid drag teach safety JSON in " + config_path.string() + ": " +
(json.ok ? "root must be an object" : json.error_message));
}
DragTeachSafetyConfig config;
const auto booleans = AssignBooleanFields(json.value, &config);
const auto numbers = AssignNumericFields(json.value, &config);
if (!booleans.ok || !numbers.ok) {
return common::Result<DragTeachSafetyConfig>::failure(
"invalid drag teach safety config " + config_path.string() + ": " +
(!booleans.ok ? booleans.error_message : numbers.error_message));
}
const auto validation = ValidateSafetyConfig(config);
if (!validation.ok) {
return common::Result<DragTeachSafetyConfig>::failure(
"invalid drag teach safety config " + config_path.string() + ": " +
validation.error_message);
}
return common::Result<DragTeachSafetyConfig>::success(config);
}
} // namespace rm_control::config
RM/src/config/json_command.cpp
View file @ 8275ee2e
/**
* @file json_command.cpp
* @brief RM JSON 指令文件解析实现。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件负责把 JSON 文本转换为 `JsonCommandProgram`。解析层只处理字段类型、
* 默认值和基础边界校验，不连接真实机械臂，也不判断当前位姿是否适合运动。
* 运动安全校验统一放在 `JsonCommandRunner`。
*
* 设计原则：
* - JSON 语法解析和命令语义解析分离；
* - 每种命令 payload 使用独立函数解析；
* - 所有错误通过 `Result` 返回；
* - 默认 safety 保持 dry-run 和禁止真实运动。
*
* @dependencies
* - rm_control/config/json_command.h
* - rm_control/config/simple_json.h
*/
#include "rm_control/config/json_command.h" #include "rm_control/config/json_command.h"
#include "rm_control/config/simple_json.h" #include "rm_control/config/simple_json.h"
...@@ -335,6 +358,310 @@ Result<MotionOptions> parseMotionOptions(const JsonValue &command, const std::st ...@@ -335,6 +358,310 @@ Result<MotionOptions> parseMotionOptions(const JsonValue &command, const std::st
return Result<MotionOptions>::success(motion); return Result<MotionOptions>::success(motion);
} }
/**
* @brief 解析 sleep_ms 命令的专有字段。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* sleep 命令只需要 `duration_ms`。单独封装该函数是为了让通用 `parseCommand()`
* 只负责类型分发，不继续堆积每种命令的字段细节。
*
* @param[in] value JSON 命令对象。
* @param[in] context 错误信息上下文。
* @param[in,out] command 解析结果，函数会写入 `duration_ms`。
*
* @return 成功或字段错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 允许 0 ms，表示立即继续下一条命令。
*
* @warning
* 负数会被拒绝，避免转换成异常长的 sleep。
*
* @thread_safety
* 函数只操作入参对象，无全局状态。
*
* @par 边界条件
* 缺少 `duration_ms`、非数字或负数均返回失败。
*
* @par 后续维护
* 若 sleep 支持秒单位，应新增字段，不要改变当前字段语义。
*/
Result<void> parseSleepCommandPayload(const JsonValue &value,
const std::string &context,
ArmCommand *command) {
auto duration = requiredNumber(value, "duration_ms", context);
if (!duration.ok) {
return Result<void>::failure(duration.error_message);
}
command->duration_ms = static_cast<int>(duration.value);
if (command->duration_ms < 0) {
return Result<void>::failure(context + ".duration_ms must not be negative");
}
return Result<void>::success();
}
/**
* @brief 解析 movej 命令的绝对关节目标。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* movej 命令需要 `joints_deg` 字段。该函数只校验字段存在、数组非空且元素为有限数字；
* 数组长度是否匹配真实 DOF 由 runner 在读取当前状态后判断。
*
* @param[in] value JSON 命令对象。
* @param[in] context 错误信息上下文。
* @param[in,out] command 解析结果，函数会写入 `joints_deg`。
*
* @return 成功或字段错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 关节角单位固定为度。
*
* @warning
* 本函数不判断关节目标是否安全，安全限幅在 core 层执行。
*
* @thread_safety
* 函数只操作入参对象，无全局状态。
*
* @par 边界条件
* 空数组会失败，避免后续运动校验错误不明确。
*
* @par 后续维护
* 如需支持命名关节字段，应新增命令类型或解析函数。
*/
Result<void> parseMoveJCommandPayload(const JsonValue &value,
const std::string &context,
ArmCommand *command) {
auto joints = requiredNumberArray(value, "joints_deg", context);
if (!joints.ok) {
return Result<void>::failure(joints.error_message);
}
if (joints.value.empty()) {
return Result<void>::failure(context + ".joints_deg must not be empty");
}
command->joints_deg = joints.value;
return Result<void>::success();
}
/**
* @brief 解析相对关节增量字段。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* `movej_delta` 和 `movej_delta_steps` 都使用 `delta_deg` 字段。该函数集中解析该字段，
* 避免两个命令分支重复处理数组非空和有限数字检查。
*
* @param[in] value JSON 命令对象。
* @param[in] context 错误信息上下文。
* @param[in,out] command 解析结果，函数会写入 `joint_delta_deg`。
*
* @return 成功或字段错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 增量单位固定为度。
*
* @warning
* 单次或每步增量是否超过安全阈值由 runner 校验。
*
* @thread_safety
* 函数只操作入参对象，无全局状态。
*
* @par 边界条件
* 空数组会失败。
*
* @par 后续维护
* 多轴数量仍应由真实机器人 DOF 校验，不要在解析层写死。
*/
Result<void> parseJointDeltaPayload(const JsonValue &value,
const std::string &context,
ArmCommand *command) {
auto joints = requiredNumberArray(value, "delta_deg", context);
if (!joints.ok) {
return Result<void>::failure(joints.error_message);
}
if (joints.value.empty()) {
return Result<void>::failure(context + ".delta_deg must not be empty");
}
command->joint_delta_deg = joints.value;
return Result<void>::success();
}
/**
* @brief 解析阶梯相对运动的步数和步间等待。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* `movej_delta_steps` 在相对增量基础上额外支持 `steps` 和 `step_sleep_ms`。
* 单独封装可以让相对增量解析和阶梯参数解析保持单一职责。
*
* @param[in] value JSON 命令对象。
* @param[in] context 错误信息上下文。
* @param[in,out] command 解析结果，函数会写入 `steps` 和 `step_sleep_ms`。
*
* @return 成功或字段错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* `steps` 省略时使用结构体默认值 1。
*
* @warning
* `steps <= 0` 会导致除零风险，因此解析阶段直接拒绝。
*
* @thread_safety
* 函数只操作入参对象，无全局状态。
*
* @par 边界条件
* `step_sleep_ms < 0` 会失败。
*
* @par 后续维护
* 如果增加总时长控制，应避免和 `step_sleep_ms` 形成冲突语义。
*/
Result<void> parseMoveJDeltaStepsPayload(const JsonValue &value,
const std::string &context,
ArmCommand *command) {
auto result = parseJointDeltaPayload(value, context, command);
if (!result.ok) {
return result;
}
result = optionalInt(value, "steps", &command->steps, context);
if (!result.ok) {
return result;
}
result = optionalInt(value, "step_sleep_ms", &command->step_sleep_ms, context);
if (!result.ok) {
return result;
}
if (command->steps <= 0) {
return Result<void>::failure(context + ".steps must be greater than 0");
}
if (command->step_sleep_ms < 0) {
return Result<void>::failure(context + ".step_sleep_ms must not be negative");
}
return Result<void>::success();
}
/**
* @brief 解析 movel 命令的 TCP 目标位姿。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* movel 命令使用 `pose` 字段，支持对象和数组两种格式。该函数只负责把 pose 写入
* `ArmCommand`，空间位移和欧拉角变化限幅由 runner 根据当前状态判断。
*
* @param[in] value JSON 命令对象。
* @param[in] context 错误信息上下文。
* @param[in,out] command 解析结果，函数会写入 `pose`。
*
* @return 成功或字段错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 位置单位为 m，欧拉角单位为 rad。
*
* @warning
* 本函数不判断目标位姿是否可达。
*
* @thread_safety
* 函数只操作入参对象，无全局状态。
*
* @par 边界条件
* 缺少 pose 或字段非有限数字会失败。
*
* @par 后续维护
* 如果支持四元数输入，应扩展 `Pose` 或新增命令，不要复用欧拉角字段。
*/
Result<void> parseMoveLCommandPayload(const JsonValue &value,
const std::string &context,
ArmCommand *command) {
auto pose = parsePose(value, context);
if (!pose.ok) {
return Result<void>::failure(pose.error_message);
}
command->pose = pose.value;
return Result<void>::success();
}
/**
* @brief 根据命令类型解析命令专有 payload。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数集中做命令类型分发，将每种命令的字段细节交给专用函数，避免 `parseCommand()`
* 持续膨胀。只读命令没有额外 payload，直接成功。
*
* @param[in] value JSON 命令对象。
* @param[in] context 错误信息上下文。
* @param[in,out] command 解析结果。
*
* @return 成功或对应命令 payload 错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 新增命令类型时必须在这里显式处理。
*
* @warning
* 不要在解析层访问硬件状态。
*
* @thread_safety
* 函数只操作入参对象，无全局状态。
*
* @par 边界条件
* 未知枚举值返回失败。
*
* @par 后续维护
* 保持每个命令 payload 解析函数只做字段解析，不做运动安全判断。
*/
Result<void> parseCommandPayload(const JsonValue &value,
const std::string &context,
ArmCommand *command) {
switch (command->type) {
case CommandType::GetRobotInfo:
case CommandType::GetCurrentArmState:
case CommandType::GetForceData:
return Result<void>::success();
case CommandType::SleepMs:
return parseSleepCommandPayload(value, context, command);
case CommandType::MoveJ:
return parseMoveJCommandPayload(value, context, command);
case CommandType::MoveJDelta:
return parseJointDeltaPayload(value, context, command);
case CommandType::MoveJDeltaSteps:
return parseMoveJDeltaStepsPayload(value, context, command);
case CommandType::MoveL:
return parseMoveLCommandPayload(value, context, command);
}
return Result<void>::failure(context + ": unsupported command enum value");
}
Result<ArmCommand> parseCommand(const JsonValue &value, size_t index) { Result<ArmCommand> parseCommand(const JsonValue &value, size_t index) {
// 单条命令解析只负责把 JSON 转成结构体，不访问硬件、不做运动安全判断。 // 单条命令解析只负责把 JSON 转成结构体，不访问硬件、不做运动安全判断。
// 运动安全判断统一放在 JsonCommandRunner::validateMotionCommand。 // 运动安全判断统一放在 JsonCommandRunner::validateMotionCommand。
...@@ -367,56 +694,9 @@ Result<ArmCommand> parseCommand(const JsonValue &value, size_t index) { ...@@ -367,56 +694,9 @@ Result<ArmCommand> parseCommand(const JsonValue &value, size_t index) {
} }
command.motion = motion.value; command.motion = motion.value;
if (command.type == CommandType::SleepMs) { auto payload_result = parseCommandPayload(value, context, &command);
auto duration = requiredNumber(value, "duration_ms", context); if (!payload_result.ok) {
if (!duration.ok) { return Result<ArmCommand>::failure(payload_result.error_message);
return Result<ArmCommand>::failure(duration.error_message);
}
command.duration_ms = static_cast<int>(duration.value);
if (command.duration_ms < 0) {
return Result<ArmCommand>::failure(context + ".duration_ms must not be negative");
}
} else if (command.type == CommandType::MoveJ) {
auto joints = requiredNumberArray(value, "joints_deg", context);
if (!joints.ok) {
return Result<ArmCommand>::failure(joints.error_message);
}
if (joints.value.empty()) {
return Result<ArmCommand>::failure(context + ".joints_deg must not be empty");
}
command.joints_deg = joints.value;
} else if (command.type == CommandType::MoveJDelta ||
command.type == CommandType::MoveJDeltaSteps) {
auto joints = requiredNumberArray(value, "delta_deg", context);
if (!joints.ok) {
return Result<ArmCommand>::failure(joints.error_message);
}
if (joints.value.empty()) {
return Result<ArmCommand>::failure(context + ".delta_deg must not be empty");
}
command.joint_delta_deg = joints.value;
if (command.type == CommandType::MoveJDeltaSteps) {
auto steps_result = optionalInt(value, "steps", &command.steps, context);
if (!steps_result.ok) {
return Result<ArmCommand>::failure(steps_result.error_message);
}
steps_result = optionalInt(value, "step_sleep_ms", &command.step_sleep_ms, context);
if (!steps_result.ok) {
return Result<ArmCommand>::failure(steps_result.error_message);
}
if (command.steps <= 0) {
return Result<ArmCommand>::failure(context + ".steps must be greater than 0");
}
if (command.step_sleep_ms < 0) {
return Result<ArmCommand>::failure(context + ".step_sleep_ms must not be negative");
}
}
} else if (command.type == CommandType::MoveL) {
auto pose = parsePose(value, context);
if (!pose.ok) {
return Result<ArmCommand>::failure(pose.error_message);
}
command.pose = pose.value;
} }
return Result<ArmCommand>::success(command); return Result<ArmCommand>::success(command);
......
RM/src/config/json_runner_cli_config.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file json_runner_cli_config.cpp
* @brief RM JSON 指令运行器命令行参数解析实现。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件实现 `rm_json_runner` 的参数解析。实现代码只处理 `argc/argv`，
* 不读取 JSON 文件、不连接 RealMan SDK、不访问真实机械臂。这样可以把入口文件
* 控制在依赖装配和流程启动范围内，也便于通过普通单元测试验证参数边界。
*
* 设计原则：
* - 帮助、错误和正常参数路径都返回结构化结果；
* - 不在解析函数中调用 `std::exit`；
* - 不在解析函数中打印错误，错误文本交给调用方统一输出；
* - 不改变原有命令行兼容性。
*
* @dependencies
* - rm_control/config/json_runner_cli_config.h
*/
#include "rm_control/config/json_runner_cli_config.h"
#include <ostream>
#include <string>
namespace rm_control::config {
namespace {
/**
* @brief 将可能为空的进程名转换为可显示文本。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 命令行 usage 需要展示程序名，但测试或异常调用场景可能传入空指针。
* 该函数集中处理空指针和空字符串，避免每个 usage 函数重复写保护逻辑。
*
* @param[in] program_name `main()` 传入的 `argv[0]`，允许为空指针。
* @param[in] fallback_name 程序名不可用时使用的默认名称。
*
* @return 可安全打印的程序名字符串。
*
* @throws 本函数不主动抛出异常；字符串构造失败属于标准库内存异常。
*
* @note
* 该函数不做路径裁剪，保留调用方传入的原始进程名。
*
* @warning
* 不要在此函数中读取 `/proc` 或环境变量，避免 CLI 解析依赖运行环境。
*
* @thread_safety
* 函数只读输入并返回局部对象，线程安全。
*
* @par 边界条件
* `program_name == nullptr` 或 `program_name[0] == '\0'` 时返回 fallback。
*
* @par 后续维护
* 如果多个 CLI 模块继续需要该逻辑，可再抽到公共 utils；当前保持局部以减少改动面。
*/
std::string SafeProgramName(const char *program_name, const char *fallback_name) {
if (program_name == nullptr || program_name[0] == '\0') {
return fallback_name;
}
return program_name;
}
} // namespace
void PrintJsonRunnerUsage(const char *program_name, std::ostream &out) {
const std::string display_name = SafeProgramName(program_name, "rm_json_runner");
out << "Usage: " << display_name << " <commands.json> [--validate-only]\n"
<< "\n"
<< "Examples:\n"
<< " " << display_name << " ../config/sample_readonly.json --validate-only\n"
<< " " << display_name << " ../config/sample_readonly.json\n"
<< "\n"
<< "Safety:\n"
<< " JSON defaults should keep dry_run=true and allow_motion=false first.\n"
<< " Set safety.dry_run=false and safety.allow_motion=true only after checking the target.\n";
}
common::Result<JsonRunnerCliConfig> ParseJsonRunnerCliArguments(int argc, char **argv) {
JsonRunnerCliConfig config;
if (argc == 2) {
const std::string first_argument = argv[1] == nullptr ? "" : argv[1];
if (first_argument == "-h" || first_argument == "--help") {
config.help_requested = true;
return common::Result<JsonRunnerCliConfig>::success(config);
}
}
if (argc < 2 || argc > 3) {
return common::Result<JsonRunnerCliConfig>::failure(
"expected <commands.json> and optional --validate-only");
}
config.command_file_path = argv[1] == nullptr ? "" : argv[1];
if (config.command_file_path.empty()) {
return common::Result<JsonRunnerCliConfig>::failure("commands.json path must not be empty");
}
if (argc == 3) {
const std::string option = argv[2] == nullptr ? "" : argv[2];
if (option != "--validate-only") {
return common::Result<JsonRunnerCliConfig>::failure("unknown option: " + option);
}
config.validate_only = true;
}
return common::Result<JsonRunnerCliConfig>::success(config);
}
} // namespace rm_control::config
RM/src/config/trajectory_convert_cli_config.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file trajectory_convert_cli_config.cpp
* @brief RM75 轨迹 CSV 转 C++ waypoint 程序命令行参数解析实现。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件实现 `rm_trajectory_convert` 的参数解析和默认输出路径推导。该工具是离线工具，
* 不连接真实机械臂；解析函数也不读取 CSV 文件，文件存在性和格式错误由 trajectory
* 模块在加载阶段返回。
*
* 设计原则：
* - 参数解析和文件转换分离；
* - 默认输出路径由 trajectory 存储模块统一生成；
* - 不在解析函数中打印或退出；
* - 保持 `<input_csv> [output_cpp]` 命令格式不变。
*
* @dependencies
* - rm_control/config/trajectory_convert_cli_config.h
* - rm_control/modules/trajectory/trajectory_file_store.h
*/
#include "rm_control/config/trajectory_convert_cli_config.h"
#include "rm_control/modules/trajectory/trajectory_file_store.h"
#include <ostream>
#include <string>
namespace rm_control::config {
namespace {
/**
* @brief 将进程名转换为可显示文本。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* usage 输出需要程序名，但测试或异常调用可能传入空指针。该函数统一处理空值，
* 让错误路径也能输出明确命令名。
*
* @param[in] program_name `argv[0]`，允许为空。
* @param[in] fallback_name 默认程序名。
*
* @return 可显示程序名。
*
* @throws 本函数不主动抛出异常；字符串分配失败由标准库报告。
*
* @note
* 保留原始路径，不截取 basename。
*
* @warning
* 不访问文件系统，避免 usage 输出依赖运行环境。
*
* @thread_safety
* 函数只使用局部对象，线程安全。
*
* @par 边界条件
* 空指针或空字符串返回 fallback。
*
* @par 后续维护
* 可在公共 utils 稳定后统一复用。
*/
std::string SafeProgramName(const char *program_name, const char *fallback_name) {
if (program_name == nullptr || program_name[0] == '\0') {
return fallback_name;
}
return program_name;
}
/**
* @brief 从路径中提取文件主名。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 默认输出 C++ 文件名需要基于输入 CSV 文件名生成 session name。该函数只做字符串处理，
* 不访问文件系统，因此输入文件即使不存在也可以得到可预测的默认输出路径。
*
* @param[in] path 输入路径字符串，可以是相对路径或绝对路径。
*
* @return 去掉目录和最后一个扩展名后的文件主名。
*
* @throws 本函数不主动抛出异常；字符串操作失败由标准库报告。
*
* @note
* 仅识别 `/` 作为路径分隔符，符合当前 Linux RM 子工程环境。
*
* @warning
* Windows 反斜杠路径不会被当作目录分隔符；如需跨平台支持需扩展此函数。
*
* @thread_safety
* 函数不使用全局状态，线程安全。
*
* @par 边界条件
* 无扩展名时返回文件名本身；点号出现在目录名前时不视作扩展名。
*
* @par 后续维护
* 如需支持多扩展名规则，应保持该函数单一职责，只做路径主名提取。
*/
std::string FileStem(const std::string &path) {
const size_t slash_position = path.find_last_of('/');
const size_t begin_position = slash_position == std::string::npos ? 0 : slash_position + 1;
const size_t dot_position = path.find_last_of('.');
const size_t end_position =
dot_position == std::string::npos || dot_position < begin_position ? path.size() : dot_position;
return path.substr(begin_position, end_position - begin_position);
}
/**
* @brief 根据输入 CSV 路径生成默认 C++ 输出路径。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数使用 trajectory 模块的存储目录规则生成默认 C++ 输出路径。如果默认目录创建失败，
* 函数退化为当前工作目录下的 `<session>_tcp_path.cpp`，避免 CLI 解析阶段直接失败。
*
* @param[in] input_csv_path 输入 CSV 路径。
* @param[in] session_name 已清洗过的会话名。
*
* @return 默认输出 C++ 文件路径。
*
* @throws 本函数不主动抛出异常；字符串和路径构造失败由标准库报告。
*
* @note
* 真正写文件时仍会再次校验父目录和写权限。
*
* @warning
* 该函数可能创建默认轨迹目录，因为 `makeTrajectoryFileSet()` 会确保目录存在。
*
* @thread_safety
* 函数本身无全局状态；并发创建同一目录由底层 `mkdir` 处理。
*
* @par 边界条件
* 默认目录创建失败时返回相对 fallback 文件名。
*
* @par 后续维护
* 如果不希望解析阶段创建目录，应调整 trajectory 模块，提供只构造路径的函数。
*/
std::string BuildDefaultOutputPath(const std::string &input_csv_path,
const std::string &session_name) {
(void)input_csv_path;
auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(
rm_control::modules::trajectory::defaultTrajectoryStorageRoot(), session_name);
if (!files.ok) {
return session_name + "_tcp_path.cpp";
}
return files.value.converted_cpp_path;
}
} // namespace
void PrintTrajectoryConvertUsage(const char *program_name, std::ostream &out) {
const std::string display_name = SafeProgramName(program_name, "rm_trajectory_convert");
out << "Usage: " << display_name << " <input_csv> [output_cpp]\n"
<< "\n"
<< "Converts recorded CSV into a C++ TCP waypoint table.\n"
<< "This offline tool does not connect to the robot.\n";
}
common::Result<TrajectoryConvertCliConfig> ParseTrajectoryConvertCliArguments(int argc, char **argv) {
TrajectoryConvertCliConfig config;
if (argc == 2) {
const std::string first_argument = argv[1] == nullptr ? "" : argv[1];
if (first_argument == "-h" || first_argument == "--help") {
config.help_requested = true;
return common::Result<TrajectoryConvertCliConfig>::success(config);
}
}
if (argc < 2 || argc > 3) {
return common::Result<TrajectoryConvertCliConfig>::failure(
"expected <input_csv> and optional [output_cpp]");
}
config.input_csv_path = argv[1] == nullptr ? "" : argv[1];
if (config.input_csv_path.empty()) {
return common::Result<TrajectoryConvertCliConfig>::failure("input_csv path must not be empty");
}
config.session_name =
rm_control::modules::trajectory::sanitizeSessionName(FileStem(config.input_csv_path));
config.output_cpp_path = argc == 3
? std::string(argv[2] == nullptr ? "" : argv[2])
: BuildDefaultOutputPath(config.input_csv_path, config.session_name);
if (config.output_cpp_path.empty()) {
return common::Result<TrajectoryConvertCliConfig>::failure("output_cpp path must not be empty");
}
return common::Result<TrajectoryConvertCliConfig>::success(config);
}
} // namespace rm_control::config
RM/src/config/trajectory_o6_task_cli_config.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file trajectory_o6_task_cli_config.cpp
* @brief RM75 轨迹与 LinkerHand O6 抓握任务命令行参数解析实现。
*
* @author 唐永康
* @date 2026-07-11
*
* @details
* 本文件只解析和校验命令行文本，不检查文件是否存在，不连接 RM75 或 O6，也不创建
* 输出文件。所有数值使用闭区间校验；机械臂百分比速度默认 5%，O6 姿态、速度和转矩
* 均不提供可执行默认值，必须由命令行显式给出。
*
* @dependencies
* - rm_control/config/trajectory_o6_task_cli_config.h
*/
#include "rm_control/config/trajectory_o6_task_cli_config.h"
#include <charconv>
#include <ostream>
#include <string>
#include <system_error>
#ifndef RM_PROJECT_ROOT
#define RM_PROJECT_ROOT "."
#endif
namespace rm_control::config {
namespace {
inline constexpr int kMinimumSamplePeriodMilliseconds = 1;
inline constexpr int kMaximumSamplePeriodMilliseconds = 1000;
inline constexpr int kMinimumSpeedPercent = 1;
inline constexpr int kMaximumSpeedPercent = 10;
inline constexpr int kMinimumProgramId = 1;
inline constexpr int kMaximumProgramId = 100;
inline constexpr int kMinimumO6JointValue = 0;
inline constexpr int kMaximumO6JointValue = 255;
using O6JointValues = std::array<std::uint8_t, kLinkerHandO6JointCount>;
struct ParseState {
TrajectoryO6TaskCliConfig config;
bool trajectory_path_seen = false;
bool sample_period_seen = false;
bool origin_speed_seen = false;
bool plan_speed_seen = false;
bool program_id_seen = false;
bool return_from_trajectory_end_seen = false;
bool return_program_id_seen = false;
bool o6_side_seen = false;
bool grip_pose_seen = false;
bool grip_speed_seen = false;
bool grip_torque_seen = false;
bool live_seen = false;
bool safety_config_seen = false;
};
struct IntegerOptionTarget {
int *destination = nullptr;
bool *seen = nullptr;
int minimum_value = 0;
int maximum_value = 0;
};
struct O6JointValuesOptionTarget {
O6JointValues *destination = nullptr;
bool *seen = nullptr;
};
TrajectoryO6TaskCliConfig BuildDefaultConfig() {
TrajectoryO6TaskCliConfig config;
config.safety_config_path =
std::filesystem::path(RM_PROJECT_ROOT) / "config" / "drag_teach_safety.json";
return config;
}
std::string SafeProgramName(const char *program_name) {
if (program_name == nullptr || program_name[0] == '\0') {
return "rm_trajectory_o6_grasp";
}
return program_name;
}
common::Result<int> ParseBoundedDecimalInteger(const std::string &text,
const std::string &option_name,
int minimum_value,
int maximum_value) {
int parsed_value = 0;
const char *begin = text.data();
const char *end = begin + text.size();
const auto parse_result = std::from_chars(begin, end, parsed_value, 10);
if (text.empty() || parse_result.ec != std::errc{} || parse_result.ptr != end) {
return common::Result<int>::failure(
option_name + " requires a decimal integer: " + text);
}
if (parsed_value < minimum_value || parsed_value > maximum_value) {
return common::Result<int>::failure(
option_name + " must be in range " + std::to_string(minimum_value) + ".." +
std::to_string(maximum_value) + ": " + text);
}
return common::Result<int>::success(parsed_value);
}
common::Result<void> MarkOptionSeen(bool *seen, const std::string &option_name) {
if (seen == nullptr) {
return common::Result<void>::failure("internal parser state is invalid for " + option_name);
}
if (*seen) {
return common::Result<void>::failure("duplicate option: " + option_name);
}
*seen = true;
return common::Result<void>::success();
}
common::Result<std::string> TakeOptionValue(int argc,
char **argv,
int *argument_index,
const std::string &option_name) {
if (argument_index == nullptr || *argument_index + 1 >= argc) {
return common::Result<std::string>::failure(option_name + " requires a value");
}
++(*argument_index);
const char *raw_value = argv[*argument_index];
if (raw_value == nullptr || raw_value[0] == '\0') {
return common::Result<std::string>::failure(option_name + " requires a non-empty value");
}
return common::Result<std::string>::success(raw_value);
}
common::Result<O6JointValues> ParseO6JointValues(const std::string &text,
const std::string &option_name) {
O6JointValues joint_values{};
std::size_t token_begin = 0;
for (std::size_t joint_index = 0; joint_index < joint_values.size(); ++joint_index) {
const std::size_t comma_position = text.find(',', token_begin);
const bool parsing_last_joint = joint_index + 1 == joint_values.size();
if ((!parsing_last_joint && comma_position == std::string::npos) ||
(parsing_last_joint && comma_position != std::string::npos)) {
return common::Result<O6JointValues>::failure(
option_name + " requires exactly six comma-separated decimal values");
}
const std::size_t token_end =
comma_position == std::string::npos ? text.size() : comma_position;
const std::string token = text.substr(token_begin, token_end - token_begin);
const auto parsed_value = ParseBoundedDecimalInteger(
token, option_name, kMinimumO6JointValue, kMaximumO6JointValue);
if (!parsed_value.ok) {
return common::Result<O6JointValues>::failure(parsed_value.error_message);
}
joint_values[joint_index] = static_cast<std::uint8_t>(parsed_value.value);
token_begin = token_end + 1;
}
return common::Result<O6JointValues>::success(joint_values);
}
bool IsIntegerOption(const std::string &argument) {
return argument == "--sample-period-ms" || argument == "--origin-speed-percent" ||
argument == "--plan-speed-percent" || argument == "--program-id" ||
argument == "--return-program-id";
}
bool IsO6JointValuesOption(const std::string &argument) {
return argument == "--grip-pose" || argument == "--grip-speed" ||
argument == "--grip-torque";
}
IntegerOptionTarget SelectIntegerOptionTarget(const std::string &option_name,
ParseState *state) {
if (option_name == "--sample-period-ms") {
return {&state->config.sample_period_ms,
&state->sample_period_seen,
kMinimumSamplePeriodMilliseconds,
kMaximumSamplePeriodMilliseconds};
}
if (option_name == "--origin-speed-percent") {
return {&state->config.origin_speed_percent,
&state->origin_speed_seen,
kMinimumSpeedPercent,
kMaximumSpeedPercent};
}
if (option_name == "--plan-speed-percent") {
return {&state->config.plan_speed_percent,
&state->plan_speed_seen,
kMinimumSpeedPercent,
kMaximumSpeedPercent};
}
if (option_name == "--program-id") {
return {&state->config.program_id,
&state->program_id_seen,
kMinimumProgramId,
kMaximumProgramId};
}
return {&state->config.return_program_id,
&state->return_program_id_seen,
kMinimumProgramId,
kMaximumProgramId};
}
common::Result<void> ParseIntegerOption(const std::string &option_name,
int argc,
char **argv,
int *argument_index,
ParseState *state) {
const IntegerOptionTarget target = SelectIntegerOptionTarget(option_name, state);
const auto unique = MarkOptionSeen(target.seen, option_name);
if (!unique.ok) {
return unique;
}
const auto value = TakeOptionValue(argc, argv, argument_index, option_name);
if (!value.ok) {
return common::Result<void>::failure(value.error_message);
}
const auto parsed = ParseBoundedDecimalInteger(
value.value, option_name, target.minimum_value, target.maximum_value);
if (!parsed.ok) {
return common::Result<void>::failure(parsed.error_message);
}
*target.destination = parsed.value;
return common::Result<void>::success();
}
O6JointValuesOptionTarget SelectO6JointValuesOptionTarget(
const std::string &option_name, ParseState *state) {
if (option_name == "--grip-pose") {
return {&state->config.grip_pose, &state->grip_pose_seen};
}
if (option_name == "--grip-speed") {
return {&state->config.grip_speed, &state->grip_speed_seen};
}
return {&state->config.grip_torque, &state->grip_torque_seen};
}
common::Result<void> ParseO6JointValuesOption(const std::string &option_name,
int argc,
char **argv,
int *argument_index,
ParseState *state) {
const O6JointValuesOptionTarget target =
SelectO6JointValuesOptionTarget(option_name, state);
const auto unique = MarkOptionSeen(target.seen, option_name);
if (!unique.ok) {
return unique;
}
const auto value = TakeOptionValue(argc, argv, argument_index, option_name);
if (!value.ok) {
return common::Result<void>::failure(value.error_message);
}
const auto joint_values = ParseO6JointValues(value.value, option_name);
if (!joint_values.ok) {
return common::Result<void>::failure(joint_values.error_message);
}
*target.destination = joint_values.value;
return common::Result<void>::success();
}
common::Result<void> ParseO6SideOption(int argc,
char **argv,
int *argument_index,
ParseState *state) {
const auto unique = MarkOptionSeen(&state->o6_side_seen, "--o6-side");
if (!unique.ok) {
return unique;
}
const auto value = TakeOptionValue(argc, argv, argument_index, "--o6-side");
if (!value.ok) {
return common::Result<void>::failure(value.error_message);
}
if (value.value == "right") {
state->config.o6_side = O6HandSide::Right;
} else if (value.value == "left") {
state->config.o6_side = O6HandSide::Left;
} else {
return common::Result<void>::failure("--o6-side must be right or left: " + value.value);
}
return common::Result<void>::success();
}
common::Result<void> ParseSafetyConfigOption(int argc,
char **argv,
int *argument_index,
ParseState *state) {
const auto unique = MarkOptionSeen(&state->safety_config_seen, "--safety-config");
if (!unique.ok) {
return unique;
}
const auto value = TakeOptionValue(argc, argv, argument_index, "--safety-config");
if (!value.ok) {
return common::Result<void>::failure(value.error_message);
}
state->config.safety_config_path = value.value;
return common::Result<void>::success();
}
common::Result<void> ParseLiveOption(ParseState *state) {
const auto unique = MarkOptionSeen(&state->live_seen, "--live");
if (!unique.ok) {
return unique;
}
state->config.live_requested = true;
return common::Result<void>::success();
}
common::Result<void> ParseReturnFromTrajectoryEndOption(ParseState *state) {
const auto unique = MarkOptionSeen(
&state->return_from_trajectory_end_seen, "--return-from-trajectory-end");
if (!unique.ok) {
return unique;
}
state->config.return_from_trajectory_end = true;
return common::Result<void>::success();
}
common::Result<void> ParseNamedOption(const std::string &argument,
int argc,
char **argv,
int *argument_index,
ParseState *state) {
if (IsIntegerOption(argument)) {
return ParseIntegerOption(argument, argc, argv, argument_index, state);
}
if (IsO6JointValuesOption(argument)) {
return ParseO6JointValuesOption(argument, argc, argv, argument_index, state);
}
if (argument == "--o6-side") {
return ParseO6SideOption(argc, argv, argument_index, state);
}
if (argument == "--safety-config") {
return ParseSafetyConfigOption(argc, argv, argument_index, state);
}
if (argument == "--live") {
return ParseLiveOption(state);
}
if (argument == "--return-from-trajectory-end") {
return ParseReturnFromTrajectoryEndOption(state);
}
return common::Result<void>::failure("unknown option: " + argument);
}
common::Result<void> ParseArgument(const std::string &argument,
int argc,
char **argv,
int *argument_index,
ParseState *state) {
if (argument.empty()) {
return common::Result<void>::failure("command-line argument must not be empty");
}
if (argument[0] == '-') {
return ParseNamedOption(argument, argc, argv, argument_index, state);
}
if (state->trajectory_path_seen) {
return common::Result<void>::failure(
"expected exactly one trajectory_path; unexpected argument: " + argument);
}
state->config.trajectory_path = std::filesystem::path(argument);
state->trajectory_path_seen = true;
return common::Result<void>::success();
}
common::Result<void> ValidateRequiredArguments(const ParseState &state) {
if (!state.trajectory_path_seen || state.config.trajectory_path.empty()) {
return common::Result<void>::failure("missing required trajectory_path");
}
if (!state.sample_period_seen) {
return common::Result<void>::failure("missing required option: --sample-period-ms");
}
if (!state.grip_pose_seen) {
return common::Result<void>::failure("missing required option: --grip-pose");
}
if (!state.grip_speed_seen) {
return common::Result<void>::failure("missing required option: --grip-speed");
}
if (!state.grip_torque_seen) {
return common::Result<void>::failure("missing required option: --grip-torque");
}
if (state.return_from_trajectory_end_seen && !state.return_program_id_seen) {
return common::Result<void>::failure(
"--return-from-trajectory-end requires --return-program-id");
}
if (!state.return_from_trajectory_end_seen && state.return_program_id_seen) {
return common::Result<void>::failure(
"--return-program-id requires --return-from-trajectory-end");
}
if (state.return_from_trajectory_end_seen &&
state.config.return_program_id == state.config.program_id) {
return common::Result<void>::failure(
"--return-program-id must differ from --program-id");
}
return common::Result<void>::success();
}
} // namespace
void PrintTrajectoryO6TaskUsage(const char *program_name, std::ostream &out) {
out << "Usage: " << SafeProgramName(program_name)
<< " <trajectory_path> --sample-period-ms N"
<< " [--origin-speed-percent N] [--plan-speed-percent N] [--program-id N]"
<< " [--return-from-trajectory-end --return-program-id N]"
<< " [--o6-side right|left]"
<< " --grip-pose v1,v2,v3,v4,v5,v6"
<< " --grip-speed v1,v2,v3,v4,v5,v6"
<< " --grip-torque v1,v2,v3,v4,v5,v6"
<< " [--live] [--safety-config path]\n"
<< "\n"
<< "Required ranges: sample period 1..1000 ms; each O6 pose, speed, and torque value"
<< " 0..255.\n"
<< "O6 pose, speed, and torque are all required; no execution defaults are used.\n"
<< "Reverse return: --return-from-trajectory-end requires a distinct"
<< " --return-program-id so reverse and forward trajectories use two controller slots.\n"
<< "Safe defaults: DryRun, reverse return disabled, origin/plan speed 5%,"
<< " program 1, right O6 (0x27).\n"
<< "Safety: --live is only one prerequisite; the complete hardware gate must still pass.\n";
}
common::Result<TrajectoryO6TaskCliConfig>
ParseTrajectoryO6TaskCliArguments(int argc, char **argv) {
ParseState state;
state.config = BuildDefaultConfig();
if (argc < 0 || (argc > 0 && argv == nullptr)) {
return common::Result<TrajectoryO6TaskCliConfig>::failure("invalid argc/argv input");
}
if (argc == 2 && argv[1] != nullptr) {
const std::string first_argument = argv[1];
if (first_argument == "-h" || first_argument == "--help") {
state.config.help_requested = true;
return common::Result<TrajectoryO6TaskCliConfig>::success(state.config);
}
}
for (int argument_index = 1; argument_index < argc; ++argument_index) {
const std::string argument = argv[argument_index] == nullptr ? "" : argv[argument_index];
const auto parsed = ParseArgument(argument, argc, argv, &argument_index, &state);
if (!parsed.ok) {
return common::Result<TrajectoryO6TaskCliConfig>::failure(parsed.error_message);
}
}
const auto required = ValidateRequiredArguments(state);
if (!required.ok) {
return common::Result<TrajectoryO6TaskCliConfig>::failure(required.error_message);
}
return common::Result<TrajectoryO6TaskCliConfig>::success(state.config);
}
} // namespace rm_control::config
RM/src/core/arm_o6_trajectory_task.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/core/arm_o6_trajectory_task.h"
#include "rm_control/common/rm75_constants.h"
#include "rm_control/modules/trajectory/sha256_file_checksum.h"
#include <algorithm>
#include <array>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <filesystem>
#include <optional>
#include <sstream>
#include <string>
#include <system_error>
#include <thread>
namespace rm_control::core {
namespace {
constexpr int kMinimumSpeedPercent = 1;
constexpr int kMaximumSpeedPercent = 100;
constexpr int kMinimumControllerProgramId = 1;
constexpr int kMaximumControllerProgramId = 100;
constexpr std::uint16_t kMaximumO6CommandValue = 255U;
struct RobotPreflightSnapshot {
common::RobotJointLimits joint_limits;
common::ArmState arm_state;
};
struct ProgramMonitorResult {
common::ControllerProgramRunState final_state;
ControllerProgramTimingReport timing;
bool observed_running_state = false;
};
struct ProgramTimingAccumulator {
std::chrono::steady_clock::time_point monitor_started_at;
std::optional<std::chrono::steady_clock::time_point> previous_poll_started_at;
ControllerProgramTimingReport report;
double total_poll_period_ms = 0.0;
std::size_t poll_period_count = 0U;
};
struct TaskPreparation {
modules::trajectory::DragTrajectoryValidationReport trajectory_validation;
RobotPreflightSnapshot robot_preflight;
};
struct RoundTripTaskPreparation {
modules::trajectory::DragTrajectoryValidationReport forward_validation;
RobotPreflightSnapshot robot_preflight;
};
struct ControllerProgramLegRequest {
const modules::trajectory::VendorDragTrajectoryParseResult *trajectory = nullptr;
int controller_program_id = 0;
const char *phase_name = nullptr;
};
common::Result<std::uintmax_t> InspectReadOnlyRegularSourceFile(
const std::filesystem::path &path) {
std::error_code filesystem_error;
const std::filesystem::file_status status =
std::filesystem::symlink_status(path, filesystem_error);
if (filesystem_error) {
return common::Result<std::uintmax_t>::failure(
"failed to inspect trajectory source " + path.string() + ": " +
filesystem_error.message());
}
if (std::filesystem::is_symlink(status) || !std::filesystem::is_regular_file(status)) {
return common::Result<std::uintmax_t>::failure(
"trajectory source must be a non-symlink regular file: " + path.string());
}
constexpr std::filesystem::perms kAllWritePermissions =
std::filesystem::perms::owner_write | std::filesystem::perms::group_write |
std::filesystem::perms::others_write;
if (status.permissions() == std::filesystem::perms::unknown ||
(status.permissions() & kAllWritePermissions) != std::filesystem::perms::none) {
return common::Result<std::uintmax_t>::failure(
"trajectory source must be read-only for owner, group, and others: " +
path.string());
}
const std::uintmax_t file_size = std::filesystem::file_size(path, filesystem_error);
if (filesystem_error) {
return common::Result<std::uintmax_t>::failure(
"failed to read trajectory source size " + path.string() + ": " +
filesystem_error.message());
}
return common::Result<std::uintmax_t>::success(file_size);
}
common::Result<void> ValidateO6CommandValues(
const interfaces::O6JointPose &commands,
std::uint16_t configured_maximum,
const std::string &command_name) {
if (configured_maximum > kMaximumO6CommandValue) {
return common::Result<void>::failure(
"configured maximum " + command_name + " exceeds O6 protocol range 0..255");
}
for (std::size_t joint = 0U; joint < commands.size(); ++joint) {
if (commands[joint] > kMaximumO6CommandValue ||
commands[joint] > configured_maximum) {
return common::Result<void>::failure(
"O6 " + command_name + " at joint " + std::to_string(joint) +
" must be in range 0.." + std::to_string(configured_maximum));
}
}
return common::Result<void>::success();
}
common::Result<void> ValidateO6MotionConfig(const ArmO6TrajectoryTaskConfig &config) {
const auto position = ValidateO6CommandValues(
config.grip_pose, kMaximumO6CommandValue, "target position command");
const auto speed = ValidateO6CommandValues(
config.grip_speed_commands, config.maximum_o6_speed_command, "speed command");
const auto torque = ValidateO6CommandValues(
config.grip_torque_commands, config.maximum_o6_torque_command, "torque command");
if (!position.ok) {
return position;
}
if (!speed.ok) {
return speed;
}
if (!torque.ok) {
return torque;
}
if (config.grip_position_tolerance > kMaximumO6CommandValue) {
return common::Result<void>::failure(
"O6 grip position tolerance must be in range 0..255");
}
return common::Result<void>::success();
}
common::Result<void> ValidateTaskConfigImpl(const ArmO6TrajectoryTaskConfig &config) {
const bool speeds_valid = config.origin_speed_percent >= kMinimumSpeedPercent &&
config.origin_speed_percent <= kMaximumSpeedPercent &&
config.plan_speed_percent >= kMinimumSpeedPercent &&
config.plan_speed_percent <= kMaximumSpeedPercent;
const bool program_id_valid = config.controller_program_id >= kMinimumControllerProgramId &&
config.controller_program_id <= kMaximumControllerProgramId;
const bool scalar_limits_valid = std::isfinite(config.maximum_origin_delta_deg) &&
config.maximum_origin_delta_deg > 0.0 &&
std::isfinite(config.origin_position_tolerance_deg) &&
config.origin_position_tolerance_deg > 0.0 &&
config.origin_position_tolerance_deg <= config.maximum_origin_delta_deg &&
std::isfinite(config.maximum_absolute_force_newton) &&
config.maximum_absolute_force_newton > 0.0 &&
std::isfinite(config.maximum_absolute_torque_newton_meter) &&
config.maximum_absolute_torque_newton_meter > 0.0;
const bool timing_valid = config.origin_status_poll_period.count() >= 0 &&
config.origin_motion_timeout.count() > 0 &&
config.program_status_poll_period.count() >= 0 &&
config.program_execution_timeout.count() > 0 &&
config.hand_status_poll_period.count() >= 0 && config.hand_motion_timeout.count() >= 0;
const auto o6_motion_check = ValidateO6MotionConfig(config);
if (!speeds_valid || !program_id_valid || !scalar_limits_valid || !timing_valid ||
!o6_motion_check.ok) {
if (!o6_motion_check.ok) {
return o6_motion_check;
}
return common::Result<void>::failure(
"RM75 O6 task configuration contains an invalid speed, limit, program id, or timeout");
}
return common::Result<void>::success();
}
common::Result<void> ValidateRoundTripTaskConfig(
const ArmO6TrajectoryTaskConfig &config) {
const auto base_config = ValidateTaskConfigImpl(config);
if (!base_config.ok) {
return base_config;
}
if (config.return_controller_program_id < kMinimumControllerProgramId ||
config.return_controller_program_id > kMaximumControllerProgramId) {
return common::Result<void>::failure(
"return controller program id must be in range 1..100");
}
if (config.return_controller_program_id == config.controller_program_id) {
return common::Result<void>::failure(
"return and forward controller program ids must be different");
}
return common::Result<void>::success();
}
common::Result<void> ValidateTrajectoryMetadata(
const modules::trajectory::VendorDragTrajectoryParseResult &trajectory) {
if (trajectory.source_file.empty() || trajectory.samples.empty() ||
trajectory.trajectory_point_count != trajectory.samples.size()) {
return common::Result<void>::failure(
"parsed vendor trajectory path, samples, and point count are inconsistent");
}
if (trajectory.sample_period_ms <= 0 || trajectory.raw_units_per_degree <= 0 ||
!trajectory.timestamp_is_derived || trajectory.source_file_sha256.empty()) {
return common::Result<void>::failure(
"parsed vendor trajectory is missing explicit units, derived timing, or SHA-256");
}
return common::Result<void>::success();
}
common::Result<void> ValidateReversedTrajectoryPair(
const modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
const modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory) {
if (return_trajectory.source_file == forward_trajectory.source_file) {
return common::Result<void>::failure(
"return and forward trajectories must use different read-only files");
}
if (return_trajectory.trajectory_point_count !=
forward_trajectory.trajectory_point_count ||
return_trajectory.sample_period_ms != forward_trajectory.sample_period_ms ||
return_trajectory.raw_units_per_degree !=
forward_trajectory.raw_units_per_degree) {
return common::Result<void>::failure(
"return trajectory metadata does not match the forward trajectory");
}
const std::size_t point_count = forward_trajectory.samples.size();
for (std::size_t return_index = 0U; return_index < point_count; ++return_index) {
const std::size_t forward_index = point_count - 1U - return_index;
if (return_trajectory.samples[return_index].joints_deg !=
forward_trajectory.samples[forward_index].joints_deg) {
return common::Result<void>::failure(
"return trajectory is not the exact point-order reverse at sample " +
std::to_string(return_index));
}
}
return common::Result<void>::success();
}
common::Result<void> ValidateRobotIdentity(const common::RobotInfo &info) {
if (!info.is_rm75 || info.arm_dof != static_cast<int>(common::kRm75JointCount)) {
return common::Result<void>::failure(
"integrated trajectory task requires an RM75 with exactly seven joints");
}
if (!info.six_axis_force_sensor_available) {
return common::Result<void>::failure(
"integrated trajectory task requires an available six-axis force sensor");
}
return common::Result<void>::success();
}
common::Result<void> ValidateRobotSafetyStatus(const common::RobotSafetyStatus &status) {
if (!status.real_mode_enabled || !status.arm_power_on || !status.all_joints_enabled) {
return common::Result<void>::failure(
"robot safety preflight requires real mode, arm power, and all joints enabled");
}
if (!status.arm_error_free || !status.controller_error_free ||
status.controller_error_code != 0) {
return common::Result<void>::failure(
"robot safety preflight found a nonzero arm or controller error");
}
// RM_API2 v1.1.5 明确说明虚拟墙和自碰撞只在仿真模式生效；真机任务改由
// HardwareExecutionGate 的现场工作区确认、实时关节/控制器错误和六维力门控保护。
if (status.emergency_stop_state_available && !status.emergency_stop_normal) {
return common::Result<void>::failure(
"robot safety preflight reports an abnormal emergency-stop state");
}
const bool joint_states_safe = std::all_of(
status.joint_enable_states.begin(), status.joint_enable_states.end(),
[](int state) { return state == 1; }) &&
std::all_of(status.joint_error_codes.begin(), status.joint_error_codes.end(),
[](int code) { return code == 0; }) &&
std::all_of(status.arm_error_codes.begin(), status.arm_error_codes.end(),
[](int code) { return code == 0; });
return joint_states_safe
? common::Result<void>::success()
: common::Result<void>::failure(
"robot safety preflight found a disabled joint or joint error code");
}
common::Result<void> ValidateJointLimitSnapshot(const common::RobotJointLimits &limits) {
for (std::size_t joint = 0; joint < common::kRm75JointCount; ++joint) {
const bool finite = std::isfinite(limits.minimum_position_deg[joint]) &&
std::isfinite(limits.maximum_position_deg[joint]) &&
std::isfinite(limits.maximum_speed_deg_per_second[joint]) &&
std::isfinite(
limits.maximum_acceleration_deg_per_second_squared[joint]);
const bool ordered = limits.minimum_position_deg[joint] <
limits.maximum_position_deg[joint];
const bool positive_motion_limits = limits.maximum_speed_deg_per_second[joint] > 0.0F &&
limits.maximum_acceleration_deg_per_second_squared[joint] > 0.0F;
if (!finite || !ordered || !positive_motion_limits) {
return common::Result<void>::failure(
"invalid controller joint limit snapshot at RM75 joint " +
std::to_string(joint));
}
}
return common::Result<void>::success();
}
common::Result<void> ValidateJointsWithinLimits(
const std::vector<float> &joints_deg,
const common::RobotJointLimits &limits,
const std::string &context) {
if (joints_deg.size() != common::kRm75JointCount) {
return common::Result<void>::failure(context + " must contain exactly seven joints");
}
for (std::size_t joint = 0; joint < common::kRm75JointCount; ++joint) {
if (!std::isfinite(joints_deg[joint]) ||
joints_deg[joint] < limits.minimum_position_deg[joint] ||
joints_deg[joint] > limits.maximum_position_deg[joint]) {
return common::Result<void>::failure(
context + " exceeds controller position limits at RM75 joint " +
std::to_string(joint));
}
}
return common::Result<void>::success();
}
common::Result<void> ValidateEveryTrajectoryPointWithinLimits(
const std::vector<common::TrajectorySample> &samples,
const common::RobotJointLimits &limits) {
for (std::size_t sample = 0; sample < samples.size(); ++sample) {
const auto result = ValidateJointsWithinLimits(
samples[sample].joints_deg, limits, "trajectory sample " + std::to_string(sample));
if (!result.ok) {
return result;
}
}
return common::Result<void>::success();
}
bool PoseIsFinite(const common::Pose &pose) {
return std::isfinite(pose.x) && std::isfinite(pose.y) && std::isfinite(pose.z) &&
std::isfinite(pose.rx) && std::isfinite(pose.ry) && std::isfinite(pose.rz);
}
common::Result<void> ValidateArmState(const common::ArmState &state,
const common::RobotJointLimits &limits) {
const auto joints = ValidateJointsWithinLimits(state.joints_deg, limits, "current arm state");
if (!joints.ok) {
return joints;
}
const bool errors_clear = std::all_of(
state.error_codes.begin(), state.error_codes.end(), [](int code) { return code == 0; });
if (!PoseIsFinite(state.tcp_pose) || !errors_clear) {
return common::Result<void>::failure(
"current arm state contains a non-finite TCP pose or nonzero error code");
}
return common::Result<void>::success();
}
common::Result<void> ValidateOriginDifference(const common::ArmState &state,
const std::vector<float> &origin_deg,
double maximum_delta_deg,
const std::string &context) {
for (std::size_t joint = 0; joint < common::kRm75JointCount; ++joint) {
const double delta = std::fabs(static_cast<double>(state.joints_deg[joint]) -
static_cast<double>(origin_deg[joint]));
if (delta > maximum_delta_deg) {
std::ostringstream message;
message << context << " at RM75 joint " << joint << ": delta=" << delta
<< " degree, allowed=" << maximum_delta_deg << " degree";
return common::Result<void>::failure(message.str());
}
}
return common::Result<void>::success();
}
template <std::size_t Size>
bool AllFinite(const std::array<float, Size> &values) {
return std::all_of(values.begin(), values.end(),
[](float value) { return std::isfinite(value); });
}
common::Result<void> ValidateForceData(const common::ForceData &force,
const ArmO6TrajectoryTaskConfig &config) {
if (!AllFinite(force.raw) || !AllFinite(force.zero) || !AllFinite(force.work_zero) ||
!AllFinite(force.tool_zero)) {
return common::Result<void>::failure(
"six-axis force snapshot contains a non-finite value");
}
for (std::size_t component = 0; component < 3U; ++component) {
if (std::fabs(static_cast<double>(force.zero[component])) >
config.maximum_absolute_force_newton) {
return common::Result<void>::failure(
"external force exceeds the configured newton limit");
}
}
for (std::size_t component = 3U; component < force.zero.size(); ++component) {
if (std::fabs(static_cast<double>(force.zero[component])) >
config.maximum_absolute_torque_newton_meter) {
return common::Result<void>::failure(
"external torque exceeds the configured newton-meter limit");
}
}
return common::Result<void>::success();
}
common::Result<interfaces::DexterousHandReadiness> ReadReadyHand(
interfaces::IDexterousHand &hand,
const std::string &context) {
const auto readiness = hand.ReadReadiness();
if (!readiness.ok) {
return common::Result<interfaces::DexterousHandReadiness>::failure(
context + ": " + readiness.error_message);
}
if (!readiness.value.ready) {
return common::Result<interfaces::DexterousHandReadiness>::failure(
context + ": O6 readiness is false");
}
return readiness;
}
common::Result<RobotPreflightSnapshot> ReadAndValidatePreflight(
interfaces::IRobotArm &robot,
const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
const ArmO6TrajectoryTaskConfig &config) {
if (!robot.isConnected()) {
return common::Result<RobotPreflightSnapshot>::failure(
"integrated trajectory task requires an active RM75 connection");
}
const auto info = robot.getRobotInfo();
const auto safety = robot.getRobotSafetyStatus();
const auto limits = robot.getJointLimits();
const auto state = robot.getCurrentArmState();
const auto force = robot.getForceData();
if (!info.ok || !safety.ok || !limits.ok || !state.ok || !force.ok) {
const std::string error = !info.ok ? info.error_message
: !safety.ok ? safety.error_message
: !limits.ok ? limits.error_message
: !state.ok ? state.error_message : force.error_message;
return common::Result<RobotPreflightSnapshot>::failure(
"RM75 preflight query failed: " + error);
}
const auto identity_check = ValidateRobotIdentity(info.value);
const auto safety_check = ValidateRobotSafetyStatus(safety.value);
const auto limits_check = ValidateJointLimitSnapshot(limits.value);
const auto state_check = ValidateArmState(state.value, limits.value);
const auto force_check = ValidateForceData(force.value, config);
const auto trajectory_check = ValidateEveryTrajectoryPointWithinLimits(
trajectory.samples, limits.value);
if (!identity_check.ok || !safety_check.ok || !limits_check.ok || !state_check.ok ||
!force_check.ok || !trajectory_check.ok) {
const auto &failed = !identity_check.ok ? identity_check
: !safety_check.ok ? safety_check : !limits_check.ok ? limits_check
: !state_check.ok ? state_check : !force_check.ok ? force_check : trajectory_check;
return common::Result<RobotPreflightSnapshot>::failure(failed.error_message);
}
return common::Result<RobotPreflightSnapshot>::success({limits.value, state.value});
}
void WaitForNextPoll(std::chrono::milliseconds poll_period) {
if (poll_period.count() > 0) {
std::this_thread::sleep_for(poll_period);
} else {
std::this_thread::yield();
}
}
bool StopWasRequested(const ArmO6TrajectoryTaskConfig &config) {
return config.stop_requested && config.stop_requested();
}
common::Result<void> WaitForTrajectoryOrigin(
interfaces::IRobotArm &robot,
const common::RobotJointLimits &joint_limits,
const std::vector<float> &origin_deg,
const ArmO6TrajectoryTaskConfig &config) {
const auto deadline = std::chrono::steady_clock::now() + config.origin_motion_timeout;
while (true) {
if (StopWasRequested(config)) {
return common::Result<void>::failure(
"stop requested while RM75 was moving to the trajectory origin");
}
const auto safety = robot.getRobotSafetyStatus();
const auto state = robot.getCurrentArmState();
if (!safety.ok || !state.ok) {
return common::Result<void>::failure(
"trajectory-origin RM75 query failed: " +
(!safety.ok ? safety.error_message : state.error_message));
}
const auto safety_check = ValidateRobotSafetyStatus(safety.value);
const auto state_check = ValidateArmState(state.value, joint_limits);
if (!safety_check.ok || !state_check.ok) {
return !safety_check.ok ? safety_check : state_check;
}
const auto origin_check = ValidateOriginDifference(
state.value, origin_deg, config.origin_position_tolerance_deg,
"RM75 has not reached the trajectory origin");
if (origin_check.ok) {
return common::Result<void>::success();
}
if (std::chrono::steady_clock::now() >= deadline) {
return common::Result<void>::failure(
origin_check.error_message + " before the configured timeout");
}
WaitForNextPoll(config.origin_status_poll_period);
}
}
void RecordProgramPollStarted(
ProgramTimingAccumulator *timing,
std::chrono::steady_clock::time_point poll_started_at,
std::chrono::milliseconds target_poll_period) {
++timing->report.poll_count;
if (!timing->previous_poll_started_at.has_value()) {
timing->previous_poll_started_at = poll_started_at;
return;
}
const double actual_period_ms = std::chrono::duration<double, std::milli>(
poll_started_at - *timing->previous_poll_started_at).count();
if (timing->poll_period_count == 0U) {
timing->report.minimum_poll_period_ms = actual_period_ms;
} else {
timing->report.minimum_poll_period_ms = std::min(
timing->report.minimum_poll_period_ms, actual_period_ms);
}
timing->report.maximum_poll_period_ms = std::max(
timing->report.maximum_poll_period_ms, actual_period_ms);
timing->report.maximum_absolute_poll_jitter_ms = std::max(
timing->report.maximum_absolute_poll_jitter_ms,
std::fabs(actual_period_ms - static_cast<double>(target_poll_period.count())));
timing->total_poll_period_ms += actual_period_ms;
++timing->poll_period_count;
timing->previous_poll_started_at = poll_started_at;
}
ControllerProgramTimingReport FinishProgramTiming(
const ProgramTimingAccumulator &timing,
std::chrono::steady_clock::time_point monitor_finished_at) {
ControllerProgramTimingReport report = timing.report;
if (timing.poll_period_count > 0U) {
report.average_poll_period_ms =
timing.total_poll_period_ms / static_cast<double>(timing.poll_period_count);
}
report.observed_execution_duration_ms = std::chrono::duration<double, std::milli>(
monitor_finished_at - timing.monitor_started_at).count();
return report;
}
common::Result<void> ValidateObservedProgramIdentityAndSpeed(
const common::ControllerProgramRunState &state,
int expected_program_id,
int maximum_speed_percent) {
if (state.program_id != expected_program_id) {
return common::Result<void>::failure(
"controller reported program id " + std::to_string(state.program_id) +
" while monitoring requested id " +
std::to_string(expected_program_id));
}
if (state.plan_speed_percent < kMinimumSpeedPercent ||
state.plan_speed_percent > maximum_speed_percent) {
return common::Result<void>::failure(
"controller reported plan speed " +
std::to_string(state.plan_speed_percent) +
"% outside the requested safe range 1.." +
std::to_string(maximum_speed_percent) + "%");
}
return common::Result<void>::success();
}
common::Result<ProgramMonitorResult> MonitorControllerProgram(
interfaces::IRobotArm &robot,
int expected_program_id,
int maximum_speed_percent,
const std::string &phase_name,
const ArmO6TrajectoryTaskConfig &config) {
const auto monitor_started_at = std::chrono::steady_clock::now();
const auto deadline = monitor_started_at + config.program_execution_timeout;
ProgramTimingAccumulator timing;
timing.monitor_started_at = monitor_started_at;
ProgramMonitorResult monitor;
while (true) {
if (StopWasRequested(config)) {
return common::Result<ProgramMonitorResult>::failure(
"stop requested while monitoring the RM75 controller program");
}
RecordProgramPollStarted(
&timing, std::chrono::steady_clock::now(), config.program_status_poll_period);
const auto state = robot.getControllerProgramRunState();
if (!state.ok) {
return common::Result<ProgramMonitorResult>::failure(
phase_name + " failed to query controller program state: " +
state.error_message);
}
if (state.value.raw_state == 1) {
const auto identity = ValidateObservedProgramIdentityAndSpeed(
state.value, expected_program_id, maximum_speed_percent);
if (!identity.ok) {
return common::Result<ProgramMonitorResult>::failure(identity.error_message);
}
monitor.observed_running_state = true;
} else if (state.value.raw_state == 0 && monitor.observed_running_state) {
monitor.final_state = state.value;
monitor.timing = FinishProgramTiming(timing, std::chrono::steady_clock::now());
return common::Result<ProgramMonitorResult>::success(monitor);
} else if (state.value.raw_state == 2) {
return common::Result<ProgramMonitorResult>::failure(
phase_name + " controller program unexpectedly entered paused state");
} else if (state.value.raw_state != 0) {
return common::Result<ProgramMonitorResult>::failure(
phase_name + " controller program returned unknown raw state " +
std::to_string(state.value.raw_state));
}
if (std::chrono::steady_clock::now() >= deadline) {
return common::Result<ProgramMonitorResult>::failure(
phase_name +
" controller program did not show running followed by completed before timeout");
}
WaitForNextPoll(config.program_status_poll_period);
}
}
bool GripPoseReached(const interfaces::O6JointPose &actual,
const interfaces::O6JointPose &target,
std::uint16_t tolerance) {
for (std::size_t joint = 0; joint < interfaces::kO6JointCount; ++joint) {
const int delta = std::abs(static_cast<int>(actual[joint]) -
static_cast<int>(target[joint]));
if (delta > static_cast<int>(tolerance)) {
return false;
}
}
return true;
}
common::Result<interfaces::DexterousHandReadiness> WaitForGripPose(
interfaces::IDexterousHand &hand,
const ArmO6TrajectoryTaskConfig &config) {
const auto deadline = std::chrono::steady_clock::now() + config.hand_motion_timeout;
while (true) {
if (StopWasRequested(config)) {
return common::Result<interfaces::DexterousHandReadiness>::failure(
"stop requested while waiting for O6 grip readback");
}
const auto readiness = ReadReadyHand(hand, "O6 grip readback failed");
if (!readiness.ok) {
return readiness;
}
if (GripPoseReached(readiness.value.joint_positions, config.grip_pose,
config.grip_position_tolerance)) {
return readiness;
}
if (std::chrono::steady_clock::now() >= deadline) {
return common::Result<interfaces::DexterousHandReadiness>::failure(
"O6 joint positions did not reach the commanded grip pose before timeout");
}
WaitForNextPoll(config.hand_status_poll_period);
}
}
common::Result<ArmO6TrajectoryTaskReport> FailAfterMotionAttempt(
interfaces::IRobotArm &robot,
const std::string &error_message) {
const auto stop = robot.slowStopMotion();
if (stop.ok) {
return common::Result<ArmO6TrajectoryTaskReport>::failure(error_message);
}
return common::Result<ArmO6TrajectoryTaskReport>::failure(
error_message + "; slowStopMotion also failed: " + stop.error_message);
}
common::Result<void> RequirePermissionForSpeed(const HardwareExecutionGateInput &gate_input,
int required_speed_percent,
const std::string &context) {
HardwareExecutionGateRequirements requirements;
requirements.required_speed_percent = required_speed_percent;
requirements.require_confirmed_trajectory_sample_period = true;
requirements.require_confirmed_vendor_raw_units_per_degree = true;
const auto permission = RequireHardwareExecutionPermission(gate_input, requirements);
return permission.ok ? permission : common::Result<void>::failure(
context + ": " + permission.error_message);
}
common::Result<TaskPreparation> PrepareTask(
interfaces::IRobotArm &robot,
interfaces::IDexterousHand &hand,
const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
const ArmO6TrajectoryTaskConfig &config,
const HardwareExecutionGateInput &gate_input) {
if (StopWasRequested(config)) {
return common::Result<TaskPreparation>::failure(
"stop requested before RM75/O6 task preparation");
}
const auto config_check = ValidateArmO6TrajectoryTaskConfig(config);
const auto metadata_check = ValidateTrajectoryMetadata(trajectory);
const auto validation = modules::trajectory::ValidateDragTrajectory(
trajectory.samples, config.validation_limits);
if (!config_check.ok || !metadata_check.ok || !validation.ok) {
const std::string error = !config_check.ok ? config_check.error_message
: !metadata_check.ok ? metadata_check.error_message : validation.error_message;
return common::Result<TaskPreparation>::failure(error);
}
const auto source_integrity = VerifySourceFileIntegrity(trajectory);
if (!source_integrity.ok) {
return common::Result<TaskPreparation>::failure(
"pre-motion trajectory integrity check failed: " +
source_integrity.error_message);
}
const auto permission = RequirePermissionForSpeed(
gate_input, std::max(config.origin_speed_percent, config.plan_speed_percent),
"initial RM75 motion permission refused");
if (!permission.ok) {
return common::Result<TaskPreparation>::failure(permission.error_message);
}
const auto preflight = ReadAndValidatePreflight(robot, trajectory, config);
if (!preflight.ok) {
return common::Result<TaskPreparation>::failure(preflight.error_message);
}
const auto origin = ValidateOriginDifference(
preflight.value.arm_state, trajectory.samples.front().joints_deg,
config.maximum_origin_delta_deg, "trajectory origin is too far from current RM75 state");
const auto hand_ready = ReadReadyHand(hand, "O6 preflight failed");
if (!origin.ok || !hand_ready.ok) {
return common::Result<TaskPreparation>::failure(
!origin.ok ? origin.error_message : hand_ready.error_message);
}
const auto slot = robot.isControllerProgramSlotAvailable(config.controller_program_id);
if (!slot.ok || !slot.value) {
return common::Result<TaskPreparation>::failure(
!slot.ok ? "controller program slot query failed: " + slot.error_message
: "controller program slot is occupied; refusing to overwrite it");
}
return common::Result<TaskPreparation>::success({validation.value, preflight.value});
}
common::Result<void> ValidateRobotAtTrajectoryEndpoint(
interfaces::IRobotArm &robot,
const common::RobotJointLimits &joint_limits,
const std::vector<float> &expected_joints_deg,
const ArmO6TrajectoryTaskConfig &config,
const std::string &context) {
const auto safety = robot.getRobotSafetyStatus();
const auto state = robot.getCurrentArmState();
const auto force = robot.getForceData();
if (!safety.ok || !state.ok || !force.ok) {
const std::string error = !safety.ok ? safety.error_message
: !state.ok ? state.error_message : force.error_message;
return common::Result<void>::failure(context + " query failed: " + error);
}
const auto safety_check = ValidateRobotSafetyStatus(safety.value);
const auto state_check = ValidateArmState(state.value, joint_limits);
const auto force_check = ValidateForceData(force.value, config);
const auto endpoint_check = ValidateOriginDifference(
state.value, expected_joints_deg, config.origin_position_tolerance_deg,
context);
if (!safety_check.ok || !state_check.ok || !force_check.ok || !endpoint_check.ok) {
return !safety_check.ok ? safety_check
: !state_check.ok ? state_check
: !force_check.ok ? force_check : endpoint_check;
}
return common::Result<void>::success();
}
common::Result<RoundTripTaskPreparation> PrepareRoundTripTask(
interfaces::IRobotArm &robot,
interfaces::IDexterousHand &hand,
const modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
const modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory,
const ArmO6TrajectoryTaskConfig &config,
const HardwareExecutionGateInput &gate_input) {
if (StopWasRequested(config)) {
return common::Result<RoundTripTaskPreparation>::failure(
"stop requested before RM75 round-trip task preparation");
}
const auto config_check = ValidateRoundTripTaskConfig(config);
const auto return_metadata = ValidateTrajectoryMetadata(return_trajectory);
const auto forward_metadata = ValidateTrajectoryMetadata(forward_trajectory);
const auto pair_check = ValidateReversedTrajectoryPair(
return_trajectory, forward_trajectory);
const auto return_validation = modules::trajectory::ValidateDragTrajectory(
return_trajectory.samples, config.validation_limits);
const auto forward_validation = modules::trajectory::ValidateDragTrajectory(
forward_trajectory.samples, config.validation_limits);
if (!config_check.ok || !return_metadata.ok || !forward_metadata.ok ||
!pair_check.ok || !return_validation.ok || !forward_validation.ok) {
const std::string error = !config_check.ok ? config_check.error_message
: !return_metadata.ok ? return_metadata.error_message
: !forward_metadata.ok ? forward_metadata.error_message
: !pair_check.ok ? pair_check.error_message
: !return_validation.ok ? return_validation.error_message
: forward_validation.error_message;
return common::Result<RoundTripTaskPreparation>::failure(error);
}
const auto return_integrity = VerifySourceFileIntegrity(return_trajectory);
const auto forward_integrity = VerifySourceFileIntegrity(forward_trajectory);
if (!return_integrity.ok || !forward_integrity.ok) {
return common::Result<RoundTripTaskPreparation>::failure(
"pre-motion round-trip trajectory integrity check failed: " +
(!return_integrity.ok ? return_integrity.error_message
: forward_integrity.error_message));
}
const auto permission = RequirePermissionForSpeed(
gate_input, config.plan_speed_percent,
"initial RM75 round-trip permission refused");
if (!permission.ok) {
return common::Result<RoundTripTaskPreparation>::failure(
permission.error_message);
}
const auto preflight = ReadAndValidatePreflight(
robot, forward_trajectory, config);
if (!preflight.ok) {
return common::Result<RoundTripTaskPreparation>::failure(
preflight.error_message);
}
const auto endpoint = ValidateOriginDifference(
preflight.value.arm_state, return_trajectory.samples.front().joints_deg,
config.origin_position_tolerance_deg,
"RM75 current state must already match the forward trajectory end point");
const auto hand_ready = ReadReadyHand(hand, "O6 round-trip preflight failed");
if (!endpoint.ok || !hand_ready.ok) {
return common::Result<RoundTripTaskPreparation>::failure(
!endpoint.ok ? endpoint.error_message : hand_ready.error_message);
}
const auto return_slot = robot.isControllerProgramSlotAvailable(
config.return_controller_program_id);
const auto forward_slot = robot.isControllerProgramSlotAvailable(
config.controller_program_id);
if (!return_slot.ok || !forward_slot.ok || !return_slot.value ||
!forward_slot.value) {
const std::string error = !return_slot.ok ? return_slot.error_message
: !forward_slot.ok ? forward_slot.error_message
: !return_slot.value ? "return controller program slot is occupied"
: "forward controller program slot is occupied";
return common::Result<RoundTripTaskPreparation>::failure(
"round-trip controller slot preflight failed: " + error);
}
return common::Result<RoundTripTaskPreparation>::success(
{forward_validation.value, preflight.value});
}
common::Result<void> SaveRoundTripControllerPrograms(
interfaces::IRobotArm &robot,
const modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
const modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory,
const ArmO6TrajectoryTaskConfig &config) {
const std::array<ControllerProgramLegRequest, 2> legs{{
{&return_trajectory, config.return_controller_program_id, "return trajectory"},
{&forward_trajectory, config.controller_program_id, "forward trajectory"},
}};
for (const auto &leg : legs) {
if (StopWasRequested(config)) {
return common::Result<void>::failure(
std::string("stop requested before saving ") + leg.phase_name);
}
const auto integrity = VerifySourceFileIntegrity(*leg.trajectory);
if (!integrity.ok) {
return common::Result<void>::failure(
std::string(leg.phase_name) +
" integrity check failed immediately before upload: " +
integrity.error_message);
}
common::VendorDragTrajectoryExecutionRequest request;
request.trajectory_path = leg.trajectory->source_file;
request.plan_speed_percent = config.plan_speed_percent;
request.controller_program_id = leg.controller_program_id;
const auto saved = robot.saveVendorDragTrajectoryProgram(request);
if (!saved.ok) {
return common::Result<void>::failure(
std::string("failed to save ") + leg.phase_name +
" without motion: " + saved.error_message);
}
}
return common::Result<void>::success();
}
common::Result<ProgramMonitorResult> ExecuteSavedControllerProgramLeg(
interfaces::IRobotArm &robot,
const ControllerProgramLegRequest &leg,
const ArmO6TrajectoryTaskConfig &config,
const HardwareExecutionGateInput &gate_input,
const common::RobotJointLimits &joint_limits) {
if (leg.trajectory == nullptr || leg.phase_name == nullptr) {
return common::Result<ProgramMonitorResult>::failure(
"internal round-trip controller program leg is invalid");
}
if (StopWasRequested(config)) {
return common::Result<ProgramMonitorResult>::failure(
std::string("stop requested before running ") + leg.phase_name);
}
const auto permission = RequirePermissionForSpeed(
gate_input, config.plan_speed_percent,
std::string(leg.phase_name) + " permission refused");
if (!permission.ok) {
return common::Result<ProgramMonitorResult>::failure(
permission.error_message);
}
const auto start_state = ValidateRobotAtTrajectoryEndpoint(
robot, joint_limits, leg.trajectory->samples.front().joints_deg,
config, std::string(leg.phase_name) + " start point mismatch");
if (!start_state.ok) {
return common::Result<ProgramMonitorResult>::failure(
start_state.error_message);
}
common::ControllerProgramRunRequest run_request;
run_request.controller_program_id = leg.controller_program_id;
run_request.speed_percent = config.plan_speed_percent;
run_request.blocking = false;
const auto run = robot.runControllerProgram(run_request);
if (!run.ok) {
return common::Result<ProgramMonitorResult>::failure(
std::string("failed to run ") + leg.phase_name + ": " +
run.error_message);
}
const auto monitor = MonitorControllerProgram(
robot, leg.controller_program_id, config.plan_speed_percent,
leg.phase_name, config);
if (!monitor.ok) {
return monitor;
}
const auto end_state = ValidateRobotAtTrajectoryEndpoint(
robot, joint_limits, leg.trajectory->samples.back().joints_deg,
config, std::string(leg.phase_name) + " end point mismatch");
if (!end_state.ok) {
return common::Result<ProgramMonitorResult>::failure(
end_state.error_message);
}
return monitor;
}
common::Result<ProgramMonitorResult> ExecuteArmTrajectory(
interfaces::IRobotArm &robot,
const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
const ArmO6TrajectoryTaskConfig &config,
const HardwareExecutionGateInput &gate_input,
const common::RobotJointLimits &joint_limits) {
if (StopWasRequested(config)) {
return common::Result<ProgramMonitorResult>::failure(
"stop requested before moving RM75 to the trajectory origin");
}
const auto move = robot.moveJ(
trajectory.samples.front().joints_deg, config.origin_speed_percent, 0, 0, false);
if (!move.ok) {
return common::Result<ProgramMonitorResult>::failure(
"failed to move RM75 to trajectory origin: " + move.error_message);
}
const auto origin = WaitForTrajectoryOrigin(
robot, joint_limits, trajectory.samples.front().joints_deg, config);
if (!origin.ok) {
return common::Result<ProgramMonitorResult>::failure(origin.error_message);
}
const auto permission = RequirePermissionForSpeed(
gate_input, config.plan_speed_percent, "RM75 trajectory permission refused");
if (!permission.ok) {
return common::Result<ProgramMonitorResult>::failure(permission.error_message);
}
common::VendorDragTrajectoryExecutionRequest request;
request.trajectory_path = trajectory.source_file;
request.plan_speed_percent = config.plan_speed_percent;
request.controller_program_id = config.controller_program_id;
const auto source_integrity = VerifySourceFileIntegrity(trajectory);
if (!source_integrity.ok) {
return common::Result<ProgramMonitorResult>::failure(
"pre-upload trajectory integrity check failed: " +
source_integrity.error_message);
}
if (StopWasRequested(config)) {
return common::Result<ProgramMonitorResult>::failure(
"stop requested before uploading the RM75 controller program");
}
const auto execute = robot.executeVendorDragTrajectoryFile(request);
if (!execute.ok) {
return common::Result<ProgramMonitorResult>::failure(
"failed to start RM75 vendor trajectory: " + execute.error_message);
}
return MonitorControllerProgram(
robot, config.controller_program_id, config.plan_speed_percent,
"forward trajectory", config);
}
common::Result<void> RecheckRobotBeforeGrip(
interfaces::IRobotArm &robot,
const common::RobotJointLimits &joint_limits,
const ArmO6TrajectoryTaskConfig &config) {
const auto safety = robot.getRobotSafetyStatus();
const auto state = robot.getCurrentArmState();
const auto force = robot.getForceData();
if (!safety.ok || !state.ok || !force.ok) {
const std::string error = !safety.ok ? safety.error_message
: !state.ok ? state.error_message : force.error_message;
return common::Result<void>::failure(
"pre-grip RM75 safety query failed: " + error);
}
const auto safety_check = ValidateRobotSafetyStatus(safety.value);
const auto state_check = ValidateArmState(state.value, joint_limits);
const auto force_check = ValidateForceData(force.value, config);
if (!safety_check.ok || !state_check.ok || !force_check.ok) {
return !safety_check.ok ? safety_check : !state_check.ok ? state_check : force_check;
}
return common::Result<void>::success();
}
common::Result<interfaces::DexterousHandReadiness> ExecuteO6Grip(
interfaces::IRobotArm &robot,
interfaces::IDexterousHand &hand,
const ArmO6TrajectoryTaskConfig &config,
const HardwareExecutionGateInput &gate_input,
const common::RobotJointLimits &joint_limits) {
const auto robot_safety = RecheckRobotBeforeGrip(robot, joint_limits, config);
if (!robot_safety.ok) {
return common::Result<interfaces::DexterousHandReadiness>::failure(
robot_safety.error_message);
}
const auto readiness = ReadReadyHand(hand, "O6 readiness after trajectory failed");
if (!readiness.ok) {
return readiness;
}
const auto permission = RequirePermissionForSpeed(
gate_input, 0, "O6 grip permission refused");
if (!permission.ok) {
return common::Result<interfaces::DexterousHandReadiness>::failure(
permission.error_message);
}
if (StopWasRequested(config)) {
return common::Result<interfaces::DexterousHandReadiness>::failure(
"stop requested before commanding the O6 grip");
}
interfaces::O6MotionCommand motion_command;
motion_command.target_positions = config.grip_pose;
motion_command.joint_speed_commands = config.grip_speed_commands;
motion_command.joint_torque_commands = config.grip_torque_commands;
const auto command = hand.CommandMotion(motion_command);
if (!command.ok) {
return common::Result<interfaces::DexterousHandReadiness>::failure(
"failed to command O6 grip motion: " + command.error_message);
}
return WaitForGripPose(hand, config);
}
} // namespace
common::Result<void> ValidateArmO6TrajectoryTaskConfig(
const ArmO6TrajectoryTaskConfig &config) {
return ValidateTaskConfigImpl(config);
}
common::Result<void> ValidateArmO6RoundTripTaskConfig(
const ArmO6TrajectoryTaskConfig &config) {
return ValidateRoundTripTaskConfig(config);
}
common::Result<void> VerifySourceFileIntegrity(
const modules::trajectory::VendorDragTrajectoryParseResult &trajectory) {
if (trajectory.source_file.empty() || trajectory.source_file_sha256.empty()) {
return common::Result<void>::failure(
"trajectory source path and expected SHA-256 must not be empty");
}
const auto size_before_digest =
InspectReadOnlyRegularSourceFile(trajectory.source_file);
if (!size_before_digest.ok ||
size_before_digest.value != trajectory.source_file_size_bytes) {
return common::Result<void>::failure(
!size_before_digest.ok ? size_before_digest.error_message
: "trajectory source size differs from parsed metadata");
}
const auto digest = modules::trajectory::CalculateFileSha256(trajectory.source_file);
if (!digest.ok) {
return common::Result<void>::failure(digest.error_message);
}
const auto size_after_digest =
InspectReadOnlyRegularSourceFile(trajectory.source_file);
if (!size_after_digest.ok ||
size_after_digest.value != trajectory.source_file_size_bytes) {
return common::Result<void>::failure(
!size_after_digest.ok ? size_after_digest.error_message
: "trajectory source changed while calculating SHA-256");
}
if (digest.value != trajectory.source_file_sha256) {
return common::Result<void>::failure(
"trajectory source SHA-256 differs from parsed metadata");
}
return common::Result<void>::success();
}
ArmO6TrajectoryTask::ArmO6TrajectoryTask(interfaces::IRobotArm &robot,
interfaces::IDexterousHand &hand) noexcept
: robot_(robot), hand_(hand) {}
common::Result<ArmO6TrajectoryTaskReport> ArmO6TrajectoryTask::Run(
const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
const ArmO6TrajectoryTaskConfig &config,
const HardwareExecutionGateInput &gate_input) {
const auto preparation = PrepareTask(robot_, hand_, trajectory, config, gate_input);
if (!preparation.ok) {
return common::Result<ArmO6TrajectoryTaskReport>::failure(
preparation.error_message);
}
const auto monitor = ExecuteArmTrajectory(
robot_, trajectory, config, gate_input, preparation.value.robot_preflight.joint_limits);
if (!monitor.ok) {
return FailAfterMotionAttempt(robot_, monitor.error_message);
}
const auto grip_readback = ExecuteO6Grip(
robot_, hand_, config, gate_input, preparation.value.robot_preflight.joint_limits);
if (!grip_readback.ok) {
return FailAfterMotionAttempt(robot_, grip_readback.error_message);
}
ArmO6TrajectoryTaskReport report;
report.trajectory_validation = preparation.value.trajectory_validation;
report.final_program_state = monitor.value.final_state;
report.final_hand_readiness = grip_readback.value;
report.controller_timing = monitor.value.timing;
report.trajectory_point_count = trajectory.trajectory_point_count;
report.observed_running_state = monitor.value.observed_running_state;
return common::Result<ArmO6TrajectoryTaskReport>::success(report);
}
common::Result<ArmO6TrajectoryTaskReport>
ArmO6TrajectoryTask::RunFromTrajectoryEnd(
const modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
const modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory,
const ArmO6TrajectoryTaskConfig &config,
const HardwareExecutionGateInput &gate_input) {
const auto preparation = PrepareRoundTripTask(
robot_, hand_, return_trajectory, forward_trajectory, config, gate_input);
if (!preparation.ok) {
return common::Result<ArmO6TrajectoryTaskReport>::failure(
preparation.error_message);
}
const auto saved = SaveRoundTripControllerPrograms(
robot_, return_trajectory, forward_trajectory, config);
if (!saved.ok) {
return common::Result<ArmO6TrajectoryTaskReport>::failure(
saved.error_message);
}
const ControllerProgramLegRequest return_leg{
&return_trajectory, config.return_controller_program_id,
"return trajectory"};
const auto return_monitor = ExecuteSavedControllerProgramLeg(
robot_, return_leg, config, gate_input,
preparation.value.robot_preflight.joint_limits);
if (!return_monitor.ok) {
return FailAfterMotionAttempt(robot_, return_monitor.error_message);
}
const ControllerProgramLegRequest forward_leg{
&forward_trajectory, config.controller_program_id,
"forward trajectory"};
const auto forward_monitor = ExecuteSavedControllerProgramLeg(
robot_, forward_leg, config, gate_input,
preparation.value.robot_preflight.joint_limits);
if (!forward_monitor.ok) {
return FailAfterMotionAttempt(robot_, forward_monitor.error_message);
}
const auto grip_readback = ExecuteO6Grip(
robot_, hand_, config, gate_input,
preparation.value.robot_preflight.joint_limits);
if (!grip_readback.ok) {
return FailAfterMotionAttempt(robot_, grip_readback.error_message);
}
ArmO6TrajectoryTaskReport report;
report.trajectory_validation = preparation.value.forward_validation;
report.return_final_program_state = return_monitor.value.final_state;
report.final_program_state = forward_monitor.value.final_state;
report.final_hand_readiness = grip_readback.value;
report.return_controller_timing = return_monitor.value.timing;
report.controller_timing = forward_monitor.value.timing;
report.trajectory_point_count = forward_trajectory.trajectory_point_count;
report.returned_from_trajectory_end = true;
report.return_observed_running_state =
return_monitor.value.observed_running_state;
report.observed_running_state =
forward_monitor.value.observed_running_state;
return common::Result<ArmO6TrajectoryTaskReport>::success(report);
}
} // namespace rm_control::core
RM/src/core/drag_teach_recorder.cpp
View file @ 8275ee2e
/**
* @file drag_teach_recorder.cpp
* @brief RM75 拖动示教轨迹录制器实现文件。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件实现拖动示教录制流程。流程会访问真实机械臂，因此实现中保持显式安全倒计时、
* 配置边界检查、采样失败处理和停止拖动示教保护。CSV 和路径管理委托给 trajectory
* 模块，真实硬件调用委托给 `IRobotArm`。
*
* 设计原则：
* - 启动拖动示教前完成配置和文件路径校验；
* - 启动后无论采样是否失败都尝试停止拖动；
* - 采样循环只负责读取状态和生成样本；
* - 录制摘要和厂商轨迹保存结果分开处理。
*
* @dependencies
* - rm_control/core/drag_teach_recorder.h
*/
#include "rm_control/core/drag_teach_recorder.h" #include "rm_control/core/drag_teach_recorder.h"
#include <chrono> #include <chrono>
...@@ -12,14 +34,83 @@ constexpr int kMinimumSamplePeriodMs = 50; ...@@ -12,14 +34,83 @@ constexpr int kMinimumSamplePeriodMs = 50;
constexpr int kMaximumDurationSeconds = 3600; constexpr int kMaximumDurationSeconds = 3600;
constexpr int kSafetyCountdownSeconds = 5; constexpr int kSafetyCountdownSeconds = 5;
common::TrajectorySample makeSample(long long elapsed_ms, const common::ArmState &state) { /**
* @brief 将机械臂状态转换为轨迹采样点。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 拖动示教录制需要把当前机械臂状态保存为 CSV。该函数只做结构转换：
* 设置相对起始时间、复制关节角和 TCP 位姿，不访问文件系统或硬件。
*
* @param[in] elapsed_ms 从录制开始到当前采样的毫秒数。
* @param[in] state 当前机械臂状态。
*
* @return 轨迹采样点。
*
* @throws 本函数不主动抛出异常；向量复制失败由标准库报告。
*
* @note
* 错误码当前不写入轨迹 CSV，只用于状态读取阶段诊断。
*
* @warning
* 如果后续 CSV 需要记录错误码，应扩展 `TrajectorySample`，不要在此临时拼接文本。
*
* @thread_safety
* 函数只使用局部对象，线程安全。
*
* @par 边界条件
* 空关节列表会被原样保存，后续 CSV 写入阶段会按 7 轴要求校验。
*
* @par 后续维护
* 新增采样字段时优先修改公共数据结构和 trajectory 模块。
*/
common::TrajectorySample makeSample(std::size_t sample_index,
long long elapsed_ms,
const common::ArmState &state) {
common::TrajectorySample sample; common::TrajectorySample sample;
sample.sample_index = sample_index;
sample.elapsed_ms = elapsed_ms; sample.elapsed_ms = elapsed_ms;
sample.joints_deg = state.joints_deg; sample.joints_deg = state.joints_deg;
sample.tcp_pose = state.tcp_pose; sample.tcp_pose = state.tcp_pose;
sample.source = "realman_rm75_drag_teach_legacy_recorder";
return sample; return sample;
} }
/**
* @brief 打印拖动示教启动前安全倒计时。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 拖动示教是需要人工参与的真实硬件流程。该函数在启动前输出安全提示并等待固定秒数，
* 给操作者最后确认工作空间和急停状态的时间。
*
* @param[out] out 过程日志输出流。
*
* @return 无返回值。
*
* @throws 本函数不主动抛出异常；输出流异常由调用方处理。
*
* @note
* 倒计时长度由 `kSafetyCountdownSeconds` 控制。
*
* @warning
* 不要删除倒计时；如果需要自动化测试，应新增 dry-run 模式而不是绕过此函数。
*
* @thread_safety
* 函数会阻塞当前线程，不适合在实时控制线程中调用。
*
* @par 边界条件
* 倒计时为 0 时不会进入循环；当前常量固定为正数。
*
* @par 后续维护
* 如需人工确认输入，应在 CLI 或 app 层增加显式确认，不在本函数读取 stdin。
*/
void printCountdown(std::ostream &out) { void printCountdown(std::ostream &out) {
out << "Prepare for six-dof force drag teach. Clear workspace and keep E-stop reachable.\n"; out << "Prepare for six-dof force drag teach. Clear workspace and keep E-stop reachable.\n";
for (int second = kSafetyCountdownSeconds; second > 0; --second) { for (int second = kSafetyCountdownSeconds; second > 0; --second) {
...@@ -33,79 +124,42 @@ void printCountdown(std::ostream &out) { ...@@ -33,79 +124,42 @@ void printCountdown(std::ostream &out) {
DragTeachRecorder::DragTeachRecorder(interfaces::IRobotArm &robot) DragTeachRecorder::DragTeachRecorder(interfaces::IRobotArm &robot)
: robot_(robot) {} : robot_(robot) {}
common::Result<DragTeachRecordSummary> DragTeachRecorder::record(const DragTeachRecordConfig &config, common::Result<DragTeachRecordSummary> DragTeachRecorder::record(
std::ostream &out) { const DragTeachRecordConfig &config,
auto validation = validateConfig(config); std::ostream &out) {
const auto validation = validateConfig(config);
if (!validation.ok) { if (!validation.ok) {
return common::Result<DragTeachRecordSummary>::failure(validation.error_message); return common::Result<DragTeachRecordSummary>::failure(validation.error_message);
} }
auto files = modules::trajectory::makeTrajectoryFileSet(config.storage_root, config.session_name); const auto files = createTrajectoryFileSet(config);
if (!files.ok) { if (!files.ok) {
return common::Result<DragTeachRecordSummary>::failure(files.error_message); return common::Result<DragTeachRecordSummary>::failure(files.error_message);
} }
printCountdown(out); printCountdown(out);
const auto start_result = robot_.startDragTeach(config.drag_options);
auto start_result = robot_.startDragTeach(config.drag_options);
if (!start_result.ok) { if (!start_result.ok) {
return common::Result<DragTeachRecordSummary>::failure(start_result.error_message); return common::Result<DragTeachRecordSummary>::failure(start_result.error_message);
} }
std::vector<common::TrajectorySample> samples; const auto samples = collectTrajectorySamples(config, out);
const auto start_time = std::chrono::steady_clock::now(); const auto stop_result = robot_.stopDragTeach();
const int total_samples = (config.duration_seconds * 1000) / config.sample_period_ms + 1; if (!samples.ok) {
common::Result<void> sample_result = common::Result<void>::success(); return common::Result<DragTeachRecordSummary>::failure(samples.error_message);
for (int sample_index = 0; sample_index < total_samples; ++sample_index) {
const auto target_time = start_time + std::chrono::milliseconds(sample_index * config.sample_period_ms);
std::this_thread::sleep_until(target_time);
auto state = robot_.getCurrentArmState();
if (!state.ok) {
sample_result = common::Result<void>::failure(state.error_message);
break;
}
const auto now = std::chrono::steady_clock::now();
const long long elapsed_ms =
std::chrono::duration_cast<std::chrono::milliseconds>(now - start_time).count();
samples.push_back(makeSample(elapsed_ms, state.value));
if (sample_index == 0 || sample_index == total_samples - 1) {
out << "sample " << sample_index << "/" << (total_samples - 1)
<< " elapsed_ms=" << elapsed_ms << '\n';
}
}
auto stop_result = robot_.stopDragTeach();
if (!sample_result.ok) {
return common::Result<DragTeachRecordSummary>::failure(sample_result.error_message);
} }
if (!stop_result.ok) { if (!stop_result.ok) {
return common::Result<DragTeachRecordSummary>::failure(stop_result.error_message); return common::Result<DragTeachRecordSummary>::failure(stop_result.error_message);
} }
auto write_result = modules::trajectory::writeTrajectoryCsv(files.value.csv_path, samples); const auto write_result =
modules::trajectory::writeTrajectoryCsv(files.value.csv_path, samples.value);
if (!write_result.ok) { if (!write_result.ok) {
return common::Result<DragTeachRecordSummary>::failure(write_result.error_message); return common::Result<DragTeachRecordSummary>::failure(write_result.error_message);
} }
DragTeachRecordSummary summary; DragTeachRecordSummary summary = buildSummary(files.value, samples.value);
summary.files = files.value; saveVendorTrajectoryIfRequested(config, &summary);
summary.sample_count = static_cast<int>(samples.size());
if (config.save_vendor_trajectory) {
auto vendor_result = robot_.saveDragTeachTrajectory(files.value.vendor_path);
if (vendor_result.ok) {
summary.vendor_trajectory_saved = true;
summary.vendor_save_message = "saved " + std::to_string(vendor_result.value.point_count) +
" vendor points";
} else {
summary.vendor_save_message = vendor_result.error_message;
}
} else {
summary.vendor_save_message = "vendor trajectory save disabled";
}
out << "CSV saved: " << summary.files.csv_path << '\n'; out << "CSV saved: " << summary.files.csv_path << '\n';
out << "Vendor trajectory: " << summary.vendor_save_message << '\n'; out << "Vendor trajectory: " << summary.vendor_save_message << '\n';
...@@ -130,4 +184,83 @@ common::Result<void> DragTeachRecorder::validateConfig(const DragTeachRecordConf ...@@ -130,4 +184,83 @@ common::Result<void> DragTeachRecorder::validateConfig(const DragTeachRecordConf
return common::Result<void>::success(); return common::Result<void>::success();
} }
common::Result<modules::trajectory::TrajectoryFileSet>
DragTeachRecorder::createTrajectoryFileSet(const DragTeachRecordConfig &config) const {
return modules::trajectory::makeTrajectoryFileSet(config.storage_root, config.session_name);
}
common::Result<std::vector<common::TrajectorySample>>
DragTeachRecorder::collectTrajectorySamples(const DragTeachRecordConfig &config,
std::ostream &out) {
std::vector<common::TrajectorySample> samples;
const auto start_time = std::chrono::steady_clock::now();
const int total_samples = (config.duration_seconds * 1000) / config.sample_period_ms + 1;
for (int sample_index = 0; sample_index < total_samples; ++sample_index) {
const auto target_time =
start_time + std::chrono::milliseconds(sample_index * config.sample_period_ms);
std::this_thread::sleep_until(target_time);
const auto state = robot_.getCurrentArmState();
if (!state.ok) {
return common::Result<std::vector<common::TrajectorySample>>::failure(
state.error_message);
}
const auto now = std::chrono::steady_clock::now();
const long long elapsed_ms =
std::chrono::duration_cast<std::chrono::milliseconds>(now - start_time).count();
samples.push_back(makeSample(static_cast<std::size_t>(sample_index),
elapsed_ms,
state.value));
if (sample_index == 0 || sample_index == total_samples - 1) {
out << "sample " << sample_index << "/" << (total_samples - 1)
<< " elapsed_ms=" << elapsed_ms << '\n';
}
}
return common::Result<std::vector<common::TrajectorySample>>::success(samples);
}
DragTeachRecordSummary DragTeachRecorder::buildSummary(
const modules::trajectory::TrajectoryFileSet &files,
const std::vector<common::TrajectorySample> &samples) const {
DragTeachRecordSummary summary;
summary.files = files;
summary.sample_count = static_cast<int>(samples.size());
return summary;
}
void DragTeachRecorder::saveVendorTrajectoryIfRequested(
const DragTeachRecordConfig &config,
DragTeachRecordSummary *summary) {
if (summary == nullptr) {
return;
}
if (!config.save_vendor_trajectory) {
summary->vendor_save_message = "vendor trajectory save disabled";
return;
}
const common::VendorTrajectorySaveAttempt vendor_attempt =
robot_.saveDragTeachTrajectory(summary->files.vendor_path);
if (vendor_attempt.api_called && vendor_attempt.return_code == 0) {
summary->vendor_trajectory_saved = true;
summary->vendor_save_message =
"saved " + std::to_string(vendor_attempt.point_count) + " vendor points";
return;
}
summary->vendor_save_message = "saveDragTeachTrajectory failed";
if (vendor_attempt.api_called) {
summary->vendor_save_message +=
", return_code=" + std::to_string(vendor_attempt.return_code);
} else {
summary->vendor_save_message += " before vendor API call";
}
if (!vendor_attempt.error_message.empty()) {
summary->vendor_save_message += ": " + vendor_attempt.error_message;
}
}
} // namespace rm_control::core } // namespace rm_control::core
RM/src/core/drag_teach_service.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/core/drag_teach_service.h"
#include "rm_control/common/rm75_constants.h"
#include "rm_control/modules/trajectory/trajectory_file_store.h"
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <exception>
namespace rm_control::core {
namespace {
constexpr std::chrono::milliseconds kMinimumSamplePeriod{10};
constexpr std::chrono::milliseconds kMaximumSamplePeriod{1000};
bool AllFinite(const std::vector<float> &values) {
return std::all_of(values.begin(), values.end(), [](float value) {
return std::isfinite(static_cast<double>(value));
});
}
bool AllFinite(const common::Pose &pose) {
return std::isfinite(static_cast<double>(pose.x)) &&
std::isfinite(static_cast<double>(pose.y)) &&
std::isfinite(static_cast<double>(pose.z)) &&
std::isfinite(static_cast<double>(pose.rx)) &&
std::isfinite(static_cast<double>(pose.ry)) &&
std::isfinite(static_cast<double>(pose.rz));
}
template <std::size_t Size>
bool AllFinite(const std::array<float, Size> &values) {
return std::all_of(values.begin(), values.end(), [](float value) {
return std::isfinite(static_cast<double>(value));
});
}
common::TrajectorySample MakeSample(
std::size_t sample_index,
long long elapsed_ms,
const common::ArmState &state) {
common::TrajectorySample sample;
sample.sample_index = sample_index;
sample.elapsed_ms = elapsed_ms;
sample.joints_deg = state.joints_deg;
sample.tcp_pose = state.tcp_pose;
return sample;
}
std::string StateName(DragTeachServiceState state) {
switch (state) {
case DragTeachServiceState::Idle: return "Idle";
case DragTeachServiceState::Recording: return "Recording";
case DragTeachServiceState::Recorded: return "Recorded";
case DragTeachServiceState::Saved: return "Saved";
case DragTeachServiceState::AtOrigin: return "AtOrigin";
case DragTeachServiceState::Replaying: return "Replaying";
case DragTeachServiceState::Paused: return "Paused";
case DragTeachServiceState::Stopped: return "Stopped";
case DragTeachServiceState::Faulted: return "Faulted";
}
return "Unknown";
}
common::Result<void> ValidateJointLimitSnapshot(
const common::RobotJointLimits &limits,
const DragTeachServiceConfig &config) {
for (std::size_t joint_index = 0;
joint_index < common::kRm75JointCount;
++joint_index) {
const double minimum = limits.minimum_position_deg[joint_index];
const double maximum = limits.maximum_position_deg[joint_index];
const double maximum_speed = limits.maximum_speed_deg_per_second[joint_index];
const double maximum_acceleration =
limits.maximum_acceleration_deg_per_second_squared[joint_index];
if (!std::isfinite(minimum) || !std::isfinite(maximum) ||
!std::isfinite(maximum_speed) || !std::isfinite(maximum_acceleration) ||
minimum >= maximum || maximum_speed <= 0.0 || maximum_acceleration <= 0.0) {
return common::Result<void>::failure(
"invalid controller limits for RM75 joint " +
std::to_string(joint_index + 1));
}
if (config.validation_limits.maximum_joint_velocity_deg_per_second >
maximum_speed ||
config.validation_limits.maximum_joint_acceleration_deg_per_second_squared >
maximum_acceleration) {
return common::Result<void>::failure(
"configured trajectory limits exceed controller limits for RM75 joint " +
std::to_string(joint_index + 1));
}
}
return common::Result<void>::success();
}
} // namespace
DragTeachService::DragTeachService(interfaces::IRobotArm &robot)
: robot_(robot) {}
DragTeachService::~DragTeachService() noexcept {
try {
RequestSamplingStop();
(void)JoinSamplingThread();
(void)StopDragTeachIfActive();
bool native_motion_possibly_active = false;
{
std::lock_guard<std::mutex> lock(mutex_);
native_motion_possibly_active = native_motion_possibly_active_;
}
const DragTeachServiceState current_state = state();
if (native_motion_possibly_active ||
current_state == DragTeachServiceState::Replaying ||
current_state == DragTeachServiceState::Paused) {
(void)robot_.stopDragTrajectoryReplay();
}
} catch (...) {
// 析构只能 best-effort 执行减险停止，禁止异常越过析构边界。
}
}
common::Result<void> DragTeachService::StartRecording(
const DragTeachServiceConfig &config,
const HardwareExecutionGateInput &gate_input) {
std::lock_guard<std::mutex> operation_lock(operation_mutex_);
const auto state_result = RequireState(DragTeachServiceState::Idle, "StartRecording");
if (!state_result.ok) {
return state_result;
}
const auto config_result = ValidateRecordingConfig(config);
if (!config_result.ok) {
return config_result;
}
config_ = config;
const auto preflight_result = ReadAndValidateRobotPreflight(config);
if (!preflight_result.ok) {
return preflight_result;
}
const auto permission = RequireHardwareExecutionPermission(gate_input);
if (!permission.ok) {
return common::Result<void>::failure(
"StartRecording refused: " + permission.error_message);
}
{
std::lock_guard<std::mutex> lock(mutex_);
samples_.clear();
sampling_error_.clear();
sampling_failed_.store(false);
validation_report_available_ = false;
artifact_available_ = false;
}
return StartHardwareAndSamplingThread();
}
common::Result<StopRecordingSummary> DragTeachService::StopRecording() {
std::lock_guard<std::mutex> operation_lock(operation_mutex_);
const DragTeachServiceState current_state = state();
if (current_state != DragTeachServiceState::Recording &&
current_state != DragTeachServiceState::Faulted) {
return common::Result<StopRecordingSummary>::failure(
"StopRecording requires Recording or Faulted state, actual=" +
StateName(current_state));
}
RequestSamplingStop();
const auto join_result = JoinSamplingThread();
if (!join_result.ok) {
state_ = DragTeachServiceState::Faulted;
return common::Result<StopRecordingSummary>::failure(
join_result.error_message);
}
const auto stop_result = StopDragTeachIfActive();
if (!stop_result.ok) {
std::lock_guard<std::mutex> lock(mutex_);
state_ = DragTeachServiceState::Faulted;
return common::Result<StopRecordingSummary>::failure(stop_result.error_message);
}
{
std::lock_guard<std::mutex> lock(mutex_);
if (sampling_failed_.load()) {
state_ = DragTeachServiceState::Faulted;
return common::Result<StopRecordingSummary>::failure(
sampling_error_.empty() ? "drag teach sampling failed without details"
: sampling_error_);
}
}
return ValidateStoppedRecording();
}
common::Result<modules::trajectory::RawTrajectoryArtifact>
DragTeachService::SaveRawTrajectory() {
std::lock_guard<std::mutex> operation_lock(operation_mutex_);
const auto state_result = RequireState(DragTeachServiceState::Recorded,
"SaveRawTrajectory");
if (!state_result.ok) {
return common::Result<modules::trajectory::RawTrajectoryArtifact>::failure(
state_result.error_message);
}
const auto paths = modules::trajectory::CreateUniqueRawTrajectorySession(
config_.storage_root, config_.session_name);
if (!paths.ok) {
return common::Result<modules::trajectory::RawTrajectoryArtifact>::failure(
paths.error_message);
}
const common::VendorTrajectorySaveAttempt save_attempt =
robot_.saveDragTeachTrajectory(paths.value.raw_trajectory);
const auto csv_result = modules::trajectory::writeTrajectoryCsvWithoutOverwrite(
paths.value.synchronized_csv, samples_);
modules::trajectory::RawTrajectoryManifestMetadata metadata;
metadata.robot_model = "RM75-6F (arm_model=" +
std::to_string(robot_info_.arm_model) + ")";
metadata.sdk_version = software_info_.sdk_version;
metadata.firmware_version = software_info_.firmware_version;
metadata.trajectory_point_count = save_attempt.point_count > 0
? static_cast<std::uint64_t>(save_attempt.point_count)
: 0U;
metadata.recorded_sample_count = static_cast<std::uint64_t>(samples_.size());
metadata.vendor_save_api_called = save_attempt.api_called;
metadata.vendor_save_return_code = save_attempt.return_code;
metadata.vendor_save_error_message = save_attempt.error_message;
metadata.synchronized_csv_write_succeeded = csv_result.ok;
const auto artifact = modules::trajectory::FinalizeRawTrajectorySession(
paths.value, metadata);
if (!artifact.ok) {
return artifact;
}
{
std::lock_guard<std::mutex> lock(mutex_);
last_artifact_ = artifact.value;
artifact_available_ = true;
state_ = save_attempt.api_called && save_attempt.return_code == 0 &&
artifact.value.raw_file_present && csv_result.ok
? DragTeachServiceState::Saved
: DragTeachServiceState::Recorded;
}
if (!csv_result.ok) {
return common::Result<modules::trajectory::RawTrajectoryArtifact>::failure(
csv_result.error_message + "; manifest=" +
artifact.value.paths.manifest.string());
}
return artifact;
}
common::Result<void> DragTeachService::MoveToTrajectoryOrigin(
const HardwareExecutionGateInput &gate_input) {
std::lock_guard<std::mutex> operation_lock(operation_mutex_);
const DragTeachServiceState current_state = state();
if (current_state != DragTeachServiceState::Saved &&
current_state != DragTeachServiceState::Stopped) {
return common::Result<void>::failure(
"MoveToTrajectoryOrigin requires Saved or Stopped state, actual=" +
StateName(current_state));
}
const auto permission = RequirePermission(gate_input, OriginRequirements());
if (!permission.ok) {
return permission;
}
const auto preflight = ValidateRuntimeMotionPreflight();
if (!preflight.ok) {
return preflight;
}
{
std::lock_guard<std::mutex> lock(mutex_);
native_motion_possibly_active_ = true;
}
const auto result = robot_.moveToDragTrajectoryOrigin(true);
if (result.ok) {
std::lock_guard<std::mutex> lock(mutex_);
native_motion_possibly_active_ = false;
state_ = DragTeachServiceState::AtOrigin;
}
return result;
}
common::Result<void> DragTeachService::ReplayLastDragTrajectory(
const HardwareExecutionGateInput &gate_input) {
std::lock_guard<std::mutex> operation_lock(operation_mutex_);
const auto state_result = RequireState(DragTeachServiceState::AtOrigin,
"ReplayLastDragTrajectory");
if (!state_result.ok) {
return state_result;
}
const auto permission = RequirePermission(gate_input, ReplayRequirements());
if (!permission.ok) {
return permission;
}
const auto preflight = ValidateRuntimeMotionPreflight();
if (!preflight.ok) {
return preflight;
}
const auto origin_match = ValidateCurrentStateMatchesTrajectoryOrigin();
if (!origin_match.ok) {
return origin_match;
}
{
std::lock_guard<std::mutex> lock(mutex_);
native_motion_possibly_active_ = true;
}
const auto result = robot_.replayLastDragTrajectory(false);
if (result.ok) {
std::lock_guard<std::mutex> lock(mutex_);
state_ = DragTeachServiceState::Replaying;
}
return result;
}
common::Result<void> DragTeachService::PauseReplay() {
std::lock_guard<std::mutex> operation_lock(operation_mutex_);
const auto state_result = RequireState(DragTeachServiceState::Replaying,
"PauseReplay");
if (!state_result.ok) {
return state_result;
}
const auto result = robot_.pauseDragTrajectoryReplay();
if (result.ok) {
std::lock_guard<std::mutex> lock(mutex_);
state_ = DragTeachServiceState::Paused;
}
return result;
}
common::Result<void> DragTeachService::ContinueReplay(
const HardwareExecutionGateInput &gate_input) {
std::lock_guard<std::mutex> operation_lock(operation_mutex_);
const auto state_result = RequireState(DragTeachServiceState::Paused,
"ContinueReplay");
if (!state_result.ok) {
return state_result;
}
const auto permission = RequirePermission(gate_input, ReplayRequirements());
if (!permission.ok) {
return permission;
}
const auto preflight = ValidateRuntimeMotionPreflight();
if (!preflight.ok) {
return preflight;
}
const auto result = robot_.continueDragTrajectoryReplay();
if (result.ok) {
std::lock_guard<std::mutex> lock(mutex_);
state_ = DragTeachServiceState::Replaying;
}
return result;
}
common::Result<void> DragTeachService::StopReplay() {
std::lock_guard<std::mutex> operation_lock(operation_mutex_);
const DragTeachServiceState current_state = state();
bool native_motion_possibly_active = false;
{
std::lock_guard<std::mutex> lock(mutex_);
native_motion_possibly_active = native_motion_possibly_active_;
}
if (!native_motion_possibly_active &&
current_state != DragTeachServiceState::Replaying &&
current_state != DragTeachServiceState::Paused) {
return common::Result<void>::failure(
"StopReplay requires Replaying or Paused state, actual=" +
StateName(current_state));
}
const auto result = robot_.stopDragTrajectoryReplay();
if (result.ok) {
std::lock_guard<std::mutex> lock(mutex_);
native_motion_possibly_active_ = false;
state_ = DragTeachServiceState::Stopped;
}
return result;
}
DragTeachServiceState DragTeachService::state() const noexcept {
return state_.load();
}
std::size_t DragTeachService::recordedSampleCount() const {
std::lock_guard<std::mutex> lock(mutex_);
return samples_.size();
}
common::Result<void> DragTeachService::ValidateRecordingConfig(
const DragTeachServiceConfig &config) const {
if (config.storage_root.empty() || config.session_name.empty()) {
return common::Result<void>::failure(
"drag teach storage_root and session_name must not be empty");
}
if (config.sample_period < kMinimumSamplePeriod ||
config.sample_period > kMaximumSamplePeriod) {
return common::Result<void>::failure(
"drag teach sample_period must be in range 10..1000 ms");
}
const auto &limits = config.validation_limits;
if (!std::isfinite(limits.maximum_joint_jump_deg) ||
!std::isfinite(limits.maximum_joint_velocity_deg_per_second) ||
!std::isfinite(limits.maximum_joint_acceleration_deg_per_second_squared) ||
limits.maximum_joint_jump_deg <= 0.0 ||
limits.maximum_joint_velocity_deg_per_second <= 0.0 ||
limits.maximum_joint_acceleration_deg_per_second_squared <= 0.0) {
return common::Result<void>::failure(
"drag trajectory validation limits must be finite and greater than zero");
}
if (!std::isfinite(config.maximum_absolute_force_newton) ||
!std::isfinite(config.maximum_absolute_torque_newton_meter) ||
config.maximum_absolute_force_newton <= 0.0 ||
config.maximum_absolute_torque_newton_meter <= 0.0) {
return common::Result<void>::failure(
"force and torque safety thresholds must be finite and greater than zero");
}
if (config.maximum_sample_count < 2U) {
return common::Result<void>::failure(
"maximum_sample_count must allow at least two samples");
}
return common::Result<void>::success();
}
common::Result<void> DragTeachService::ReadAndValidateRobotPreflight(
const DragTeachServiceConfig &config) {
if (!robot_.isConnected()) {
return common::Result<void>::failure(
"DragTeachService requires an active robot connection");
}
const auto info = robot_.getRobotInfo();
if (!info.ok) {
return common::Result<void>::failure(info.error_message);
}
const auto identity = ValidateRobotIdentity(info.value, config.drag_options);
if (!identity.ok) {
return identity;
}
const auto software = robot_.getRobotSoftwareInfo();
const auto safety = robot_.getRobotSafetyStatus();
const auto joint_limits = robot_.getJointLimits();
if (!software.ok || !safety.ok || !joint_limits.ok) {
return common::Result<void>::failure(
!software.ok ? software.error_message
: !safety.ok ? safety.error_message
: joint_limits.error_message);
}
const auto safety_validation = ValidateRobotSafetyStatus(safety.value);
if (!safety_validation.ok) {
return safety_validation;
}
const auto limit_validation = ValidateJointLimitSnapshot(joint_limits.value, config);
if (!limit_validation.ok) {
return limit_validation;
}
robot_info_ = info.value;
software_info_ = software.value;
joint_limits_ = joint_limits.value;
return ValidateCurrentStateAndForce();
}
common::Result<void> DragTeachService::ValidateRobotIdentity(
const common::RobotInfo &info,
const common::DragTeachStartOptions &options) const {
if (!info.is_rm75 ||
info.arm_dof != static_cast<int>(common::kRm75JointCount)) {
return common::Result<void>::failure(
"drag teach requires an RM75 with exactly seven joints");
}
if (options.mode == common::DragTeachMode::SixDofForcePositionAndOrientation &&
!info.six_axis_force_sensor_available) {
return common::Result<void>::failure(
"six-dof force drag teach requires a six-axis force sensor");
}
if (options.mode == common::DragTeachMode::SixDofForcePositionAndOrientation &&
info.controller_version != 3) {
return common::Result<void>::failure(
"rm_start_multi_drag_teach requires a generation-3 controller");
}
return common::Result<void>::success();
}
common::Result<void> DragTeachService::ValidateRobotSafetyStatus(
const common::RobotSafetyStatus &status) const {
if (!status.real_mode_enabled || !status.arm_power_on) {
return common::Result<void>::failure(
"robot preflight requires real mode and arm power on");
}
if (!status.all_joints_enabled || !status.arm_error_free ||
!status.controller_error_free) {
return common::Result<void>::failure(
"robot preflight found disabled joints or nonzero errors");
}
if (!status.virtual_wall_enabled) {
return common::Result<void>::failure(
"robot preflight requires the configured virtual wall to be enabled");
}
if (!status.self_collision_enabled) {
return common::Result<void>::failure(
"robot preflight requires self-collision detection to be enabled");
}
if (status.emergency_stop_state_available && !status.emergency_stop_normal) {
return common::Result<void>::failure(
"robot preflight reports an abnormal emergency-stop state");
}
return common::Result<void>::success();
}
common::Result<void> DragTeachService::ValidateCurrentStateAndForce() {
const auto state_result = robot_.getCurrentArmState();
const auto force_result = robot_.getForceData();
if (!state_result.ok || !force_result.ok) {
return common::Result<void>::failure(
!state_result.ok ? state_result.error_message : force_result.error_message);
}
const auto state_validation = ValidateSampledArmState(state_result.value);
if (!state_validation.ok) {
return state_validation;
}
const common::ForceData &force = force_result.value;
if (!AllFinite(force.raw) || !AllFinite(force.zero) ||
!AllFinite(force.work_zero) || !AllFinite(force.tool_zero)) {
return common::Result<void>::failure(
"robot preflight force data contains a non-finite value");
}
for (std::size_t component_index = 0; component_index < 3U; ++component_index) {
if (std::fabs(static_cast<double>(force.zero[component_index])) >
config_.maximum_absolute_force_newton) {
return common::Result<void>::failure(
"robot preflight external force exceeds configured newton limit");
}
}
for (std::size_t component_index = 3U; component_index < force.zero.size();
++component_index) {
if (std::fabs(static_cast<double>(force.zero[component_index])) >
config_.maximum_absolute_torque_newton_meter) {
return common::Result<void>::failure(
"robot preflight external torque exceeds configured newton-meter limit");
}
}
return common::Result<void>::success();
}
common::Result<void> DragTeachService::ValidateRuntimeMotionPreflight() {
if (!robot_.isConnected()) {
return common::Result<void>::failure(
"runtime motion preflight requires an active robot connection");
}
const auto info = robot_.getRobotInfo();
const auto safety = robot_.getRobotSafetyStatus();
const auto joint_limits = robot_.getJointLimits();
if (!info.ok || !safety.ok || !joint_limits.ok) {
return common::Result<void>::failure(
!info.ok ? info.error_message
: !safety.ok ? safety.error_message
: joint_limits.error_message);
}
const auto identity = ValidateRobotIdentity(info.value, config_.drag_options);
const auto safety_validation = ValidateRobotSafetyStatus(safety.value);
const auto limit_validation = ValidateJointLimitSnapshot(joint_limits.value, config_);
if (!identity.ok || !safety_validation.ok || !limit_validation.ok) {
return !identity.ok ? identity
: !safety_validation.ok ? safety_validation
: limit_validation;
}
joint_limits_ = joint_limits.value;
return ValidateCurrentStateAndForce();
}
common::Result<void> DragTeachService::ValidateSampledArmState(
const common::ArmState &state) const {
if (state.joints_deg.size() != common::kRm75JointCount ||
!AllFinite(state.joints_deg) || !AllFinite(state.tcp_pose) ||
!state.error_codes.empty()) {
return common::Result<void>::failure(
"RM75 state requires seven finite joints, a finite TCP pose, and no arm errors");
}
for (std::size_t joint_index = 0;
joint_index < common::kRm75JointCount;
++joint_index) {
const float position = state.joints_deg[joint_index];
if (position < joint_limits_.minimum_position_deg[joint_index] ||
position > joint_limits_.maximum_position_deg[joint_index]) {
return common::Result<void>::failure(
"RM75 joint " + std::to_string(joint_index + 1) +
" is outside the controller position limits");
}
}
return common::Result<void>::success();
}
common::Result<void>
DragTeachService::ValidateCurrentStateMatchesTrajectoryOrigin() {
const auto current = robot_.getCurrentArmState();
if (!current.ok) {
return common::Result<void>::failure(current.error_message);
}
const auto state_validation = ValidateSampledArmState(current.value);
if (!state_validation.ok) {
return state_validation;
}
if (samples_.empty()) {
return common::Result<void>::failure(
"cannot verify trajectory origin without recorded samples");
}
for (std::size_t joint_index = 0;
joint_index < common::kRm75JointCount;
++joint_index) {
const double difference = std::fabs(
static_cast<double>(current.value.joints_deg[joint_index]) -
static_cast<double>(samples_.front().joints_deg[joint_index]));
if (difference > config_.validation_limits.maximum_joint_jump_deg) {
return common::Result<void>::failure(
"current RM75 state differs from trajectory origin at joint " +
std::to_string(joint_index + 1));
}
}
return common::Result<void>::success();
}
common::Result<void> DragTeachService::StartHardwareAndSamplingThread() {
{
std::lock_guard<std::mutex> lock(mutex_);
// 通信超时不能证明控制器未执行，因此发送前就标记为“可能仍在拖动”。
hardware_drag_active_ = true;
}
common::Result<void> start_result;
try {
start_result = robot_.startDragTeach(config_.drag_options);
} catch (const std::exception &error) {
start_result = common::Result<void>::failure(
std::string("startDragTeach threw an exception: ") + error.what());
} catch (...) {
start_result = common::Result<void>::failure(
"startDragTeach threw a non-standard exception");
}
if (!start_result.ok) {
const auto stop_result = StopDragTeachIfActive();
state_ = DragTeachServiceState::Faulted;
return common::Result<void>::failure(
start_result.error_message +
(stop_result.ok ? "; stopDragTeach completed after uncertain start"
: "; stopDragTeach also failed: " +
stop_result.error_message));
}
{
std::lock_guard<std::mutex> lock(mutex_);
state_ = DragTeachServiceState::Recording;
}
sampling_stop_requested_.store(false);
try {
sampling_thread_ = std::thread(&DragTeachService::SamplingLoop, this);
} catch (const std::exception &error) {
const auto stop_result = StopDragTeachIfActive();
std::lock_guard<std::mutex> lock(mutex_);
state_ = DragTeachServiceState::Faulted;
return common::Result<void>::failure(
std::string("failed to start drag sampling thread: ") + error.what() +
(stop_result.ok ? "" : "; " + stop_result.error_message));
}
return common::Result<void>::success();
}
void DragTeachService::SamplingLoop() noexcept {
try {
RunSamplingLoop();
} catch (const std::exception &error) {
RecordSamplingFailure(error.what());
} catch (...) {
RecordSamplingFailure("unhandled non-standard sampling exception");
}
}
void DragTeachService::RunSamplingLoop() {
const auto start_time = std::chrono::steady_clock::now();
std::size_t sample_index = 0;
while (!sampling_stop_requested_.load()) {
if (sample_index >= config_.maximum_sample_count) {
RecordSamplingFailure("maximum drag teach sample count reached");
return;
}
const auto state_result = robot_.getCurrentArmState();
if (!state_result.ok) {
RecordSamplingFailure(state_result.error_message);
return;
}
const auto state_validation = ValidateSampledArmState(state_result.value);
if (!state_validation.ok) {
RecordSamplingFailure(state_validation.error_message);
return;
}
const auto now = std::chrono::steady_clock::now();
const long long elapsed_ms =
std::chrono::duration_cast<std::chrono::milliseconds>(now - start_time).count();
{
std::lock_guard<std::mutex> lock(mutex_);
samples_.push_back(MakeSample(sample_index, elapsed_ms, state_result.value));
}
++sample_index;
const auto next_time = start_time + config_.sample_period * sample_index;
if (now > next_time + config_.sample_period) {
RecordSamplingFailure(
"drag teach sampling missed more than one configured period");
return;
}
std::unique_lock<std::mutex> lock(mutex_);
sampling_condition_.wait_until(lock, next_time, [this] {
return sampling_stop_requested_.load();
});
}
}
void DragTeachService::RecordSamplingFailure(std::string_view message) noexcept {
sampling_failed_.store(true);
try {
bool drag_stopped = false;
std::string stop_error;
try {
const auto stop_result = robot_.stopDragTeach();
drag_stopped = stop_result.ok;
if (!stop_result.ok) {
stop_error = stop_result.error_message;
}
} catch (const std::exception &error) {
stop_error = error.what();
} catch (...) {
stop_error = "unknown stopDragTeach exception";
}
std::lock_guard<std::mutex> lock(mutex_);
if (drag_stopped) {
hardware_drag_active_ = false;
}
state_ = DragTeachServiceState::Faulted;
sampling_error_ = "drag teach sampling failed: " + std::string(message);
if (!drag_stopped) {
sampling_error_ += "; stopDragTeach failed and will be retried: " +
stop_error;
}
} catch (...) {
state_.store(DragTeachServiceState::Faulted);
}
}
void DragTeachService::RequestSamplingStop() noexcept {
sampling_stop_requested_.store(true);
sampling_condition_.notify_all();
}
common::Result<void> DragTeachService::JoinSamplingThread() {
try {
if (sampling_thread_.joinable()) {
sampling_thread_.join();
}
return common::Result<void>::success();
} catch (const std::exception &error) {
return common::Result<void>::failure(
std::string("failed to join drag sampling thread: ") + error.what());
} catch (...) {
return common::Result<void>::failure(
"failed to join drag sampling thread: unknown exception");
}
}
common::Result<void> DragTeachService::StopDragTeachIfActive() {
{
std::lock_guard<std::mutex> lock(mutex_);
if (!hardware_drag_active_) {
return common::Result<void>::success();
}
}
const auto result = robot_.stopDragTeach();
if (result.ok) {
std::lock_guard<std::mutex> lock(mutex_);
hardware_drag_active_ = false;
}
return result;
}
common::Result<StopRecordingSummary>
DragTeachService::ValidateStoppedRecording() {
const auto validation = modules::trajectory::ValidateDragTrajectory(
samples_, config_.validation_limits);
if (!validation.ok) {
std::lock_guard<std::mutex> lock(mutex_);
state_ = DragTeachServiceState::Faulted;
return common::Result<StopRecordingSummary>::failure(
validation.error_message);
}
StopRecordingSummary summary;
summary.recorded_sample_count = samples_.size();
summary.validation_report = validation.value;
{
std::lock_guard<std::mutex> lock(mutex_);
validation_report_ = validation.value;
validation_report_available_ = true;
state_ = DragTeachServiceState::Recorded;
}
return common::Result<StopRecordingSummary>::success(summary);
}
HardwareExecutionGateRequirements DragTeachService::OriginRequirements() const {
HardwareExecutionGateRequirements requirements;
requirements.required_speed_percent =
kNativeTrajectoryOriginRequiredSpeedPercent;
return requirements;
}
HardwareExecutionGateRequirements DragTeachService::ReplayRequirements() const {
HardwareExecutionGateRequirements requirements;
requirements.require_native_replay_without_speed_control_acceptance = true;
requirements.require_native_replay_speed_verification = true;
return requirements;
}
common::Result<void> DragTeachService::RequirePermission(
const HardwareExecutionGateInput &gate_input,
const HardwareExecutionGateRequirements &requirements) const {
const auto permission =
RequireHardwareExecutionPermission(gate_input, requirements);
if (!permission.ok) {
return common::Result<void>::failure(
"native drag trajectory action refused: " +
permission.error_message);
}
return common::Result<void>::success();
}
common::Result<void> DragTeachService::RequireState(
DragTeachServiceState expected,
const std::string &operation_name) const {
const DragTeachServiceState actual = state();
if (actual != expected) {
return common::Result<void>::failure(
operation_name + " requires " + StateName(expected) +
" state, actual=" + StateName(actual));
}
return common::Result<void>::success();
}
} // namespace rm_control::core
RM/src/core/hardware_execution_gate.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/core/hardware_execution_gate.h"
#include <sstream>
#include <string>
namespace rm_control::core {
namespace {
constexpr int kMinimumSpeedPercent = 1;
constexpr int kMaximumSpeedPercent = 100;
void AddMissingCondition(bool condition,
const std::string &rejection_reason,
HardwareExecutionGateDecision *decision) {
if (!condition) {
decision->rejection_reasons.push_back(rejection_reason);
}
}
void ValidateConfiguredMaximumSpeed(const HardwareExecutionGateInput &input,
HardwareExecutionGateDecision *decision) {
const bool speed_is_valid = input.configured_max_speed_percent >= kMinimumSpeedPercent &&
input.configured_max_speed_percent <= kMaximumSpeedPercent;
AddMissingCondition(speed_is_valid,
"configured maximum speed percent must be in the range 1..100",
decision);
}
void ValidateRequiredSpeed(const HardwareExecutionGateInput &input,
const HardwareExecutionGateRequirements &requirements,
HardwareExecutionGateDecision *decision) {
if (requirements.required_speed_percent < 0 ||
requirements.required_speed_percent > kMaximumSpeedPercent) {
decision->rejection_reasons.emplace_back(
"required hardware speed percent must be in the range 0..100");
return;
}
if (requirements.required_speed_percent > input.configured_max_speed_percent) {
decision->rejection_reasons.emplace_back(
"required hardware speed percent exceeds configured maximum speed percent");
}
}
std::string JoinRejectionReasons(const std::vector<std::string> &rejection_reasons) {
std::ostringstream message;
message << "hardware execution refused";
for (const auto &reason : rejection_reasons) {
message << "; " << reason;
}
return message.str();
}
} // namespace
HardwareExecutionGateDecision EvaluateHardwareExecutionGate(
const HardwareExecutionGateInput &input,
const HardwareExecutionGateRequirements &requirements) {
HardwareExecutionGateDecision decision;
AddMissingCondition(input.live_requested, "--live was not explicitly requested", &decision);
AddMissingCondition(input.hardware_execution_enabled,
"hardware_execution_enabled is false",
&decision);
AddMissingCondition(input.arm_connected, "robot arm is not connected", &decision);
AddMissingCondition(input.hand_connected, "dexterous hand is not connected", &decision);
AddMissingCondition(input.emergency_stop_verified,
"emergency stop state has not been verified as safe",
&decision);
AddMissingCondition(input.safety_limits_loaded,
"safety limits have not been loaded",
&decision);
AddMissingCondition(input.operator_confirmed,
"operator second confirmation is missing",
&decision);
AddMissingCondition(input.trajectory_validated,
"trajectory has not passed validation",
&decision);
AddMissingCondition(input.workspace_and_virtual_wall_verified,
"workspace and virtual wall have not been verified",
&decision);
ValidateConfiguredMaximumSpeed(input, &decision);
ValidateRequiredSpeed(input, requirements, &decision);
if (requirements.require_native_replay_without_speed_control_acceptance) {
AddMissingCondition(input.native_replay_without_speed_control_accepted,
"native replay without speed control was not explicitly accepted",
&decision);
}
if (requirements.require_native_replay_speed_verification) {
AddMissingCondition(input.native_replay_speed_within_config_verified,
"native replay speed has not been independently verified "
"within the configured limit",
&decision);
}
if (requirements.require_confirmed_trajectory_sample_period) {
AddMissingCondition(input.trajectory_sample_period_confirmed,
"trajectory sample period has not been confirmed",
&decision);
}
if (requirements.require_confirmed_vendor_raw_units_per_degree) {
AddMissingCondition(input.vendor_raw_units_per_degree_confirmed,
"vendor raw-units-per-degree ratio has not been confirmed",
&decision);
}
decision.execution_allowed = decision.rejection_reasons.empty();
return decision;
}
common::Result<void> RequireHardwareExecutionPermission(
const HardwareExecutionGateInput &input,
const HardwareExecutionGateRequirements &requirements) {
const auto decision = EvaluateHardwareExecutionGate(input, requirements);
if (!decision.execution_allowed) {
return common::Result<void>::failure(JoinRejectionReasons(decision.rejection_reasons));
}
return common::Result<void>::success();
}
} // namespace rm_control::core
RM/src/core/json_command_runner.cpp
View file @ 8275ee2e
/**
* @file json_command_runner.cpp
* @brief RM JSON 指令执行器实现文件。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件实现 JSON 指令执行流程。执行器只依赖 `IRobotArm` 抽象接口，不包含 RealMan
* SDK 头文件。真实运动命令会先读取当前机械臂状态，再执行速度、关节增量、笛卡尔位移
* 和欧拉角变化限幅；`dry_run=true` 时只校验不发送运动。
*
* 设计原则：
* - 顶层 `run()` 只负责连接和命令循环；
* - 只读命令、运动命令和各运动类型拆成独立函数；
* - 所有运动命令必须经过同一个安全入口；
* - 错误通过 `Result` 返回，不抛异常给入口文件。
*
* @dependencies
* - rm_control/core/json_command_runner.h
*/
#include "rm_control/core/json_command_runner.h" #include "rm_control/core/json_command_runner.h"
#include <algorithm>
#include <chrono> #include <chrono>
#include <cmath> #include <cmath>
#include <iomanip> #include <iomanip>
#include <iostream> #include <ostream>
#include <thread> #include <thread>
#include <vector>
namespace rm_control::core { namespace rm_control::core {
namespace { namespace {
/**
* @brief 计算两个浮点数的绝对差值。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 安全校验需要比较目标值和当前值的差值。该函数封装 `std::fabs`，
* 让关节和笛卡尔校验代码表达更清晰。
*
* @param[in] left 左侧数值。
* @param[in] right 右侧数值。
*
* @return 两个数值的绝对差。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 调用方负责保证输入为有限值；JSON 解析层和驱动读取层已做基础约束。
*
* @warning
* 如果输入 NaN，返回值也会是 NaN，比较逻辑不会触发大于阈值分支。
*
* @thread_safety
* 函数无全局状态，线程安全。
*
* @par 边界条件
* 正负零差值返回 0。
*
* @par 后续维护
* 若引入统一数学工具模块，可迁移该函数。
*/
float absDiff(float left, float right) { float absDiff(float left, float right) {
return std::fabs(left - right); return std::fabs(left - right);
} }
/**
* @brief 将 TCP 位姿写入日志流。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 多个命令分支都需要输出 TCP 位姿。该函数集中格式化位置和欧拉角，
* 避免输出格式在不同分支中漂移。
*
* @param[out] out 日志输出流。
* @param[in] pose TCP 位姿，位置单位为 m，欧拉角单位为 rad。
*
* @return 无返回值。
*
* @throws 本函数不主动抛出异常；输出流异常由调用方处理。
*
* @note
* 输出精度固定为 4 位小数，适合人工查看。
*
* @warning
* 修改输出格式可能影响依赖日志文本的外部脚本。
*
* @thread_safety
* 函数自身无状态；并发写同一输出流需要调用方保护。
*
* @par 边界条件
* 零位姿按固定格式输出。
*
* @par 后续维护
* 如果需要机器可读日志，应新增结构化日志接口，不要复用此人工格式。
*/
void printPose(std::ostream &out, const common::Pose &pose) { void printPose(std::ostream &out, const common::Pose &pose) {
out << std::fixed << std::setprecision(4) out << std::fixed << std::setprecision(4)
<< "pos_m=(" << pose.x << "," << pose.y << "," << pose.z << ")" << "pos_m=(" << pose.x << "," << pose.y << "," << pose.z << ")"
<< ", euler_rad=(" << pose.rx << "," << pose.ry << "," << pose.rz << ")"; << ", euler_rad=(" << pose.rx << "," << pose.ry << "," << pose.rz << ")";
} }
/**
* @brief 将关节角列表写入日志流。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 关节状态和关节运动目标都需要统一输出。该函数按 `J1=...` 的形式输出，
* 便于人工对照 RM75 的 7 轴状态。
*
* @param[out] out 日志输出流。
* @param[in] joints 关节角列表，单位为度。
*
* @return 无返回值。
*
* @throws 本函数不主动抛出异常；输出流异常由调用方处理。
*
* @note
* 函数不强制关节数量，实际数量由调用方输入决定。
*
* @warning
* 修改输出格式可能影响外部日志解析脚本。
*
* @thread_safety
* 函数自身无状态；并发写同一输出流需要调用方保护。
*
* @par 边界条件
* 空关节列表输出 `[]`。
*
* @par 后续维护
* 如果后续支持非度单位，应在调用前转换，而不是让本函数判断单位。
*/
void printJoints(std::ostream &out, const std::vector<float> &joints) { void printJoints(std::ostream &out, const std::vector<float> &joints) {
out << std::fixed << std::setprecision(3) << "["; out << std::fixed << std::setprecision(3) << "[";
for (size_t i = 0; i < joints.size(); ++i) { for (size_t joint_index = 0; joint_index < joints.size(); ++joint_index) {
if (i > 0) { if (joint_index > 0) {
out << ", "; out << ", ";
} }
out << "J" << (i + 1) << "=" << joints[i]; out << "J" << (joint_index + 1) << "=" << joints[joint_index];
} }
out << "]"; out << "]";
} }
} // namespace /**
* @brief 打印 JSON 程序启动摘要。
JsonCommandRunner::JsonCommandRunner(interfaces::IRobotArm &robot) *
: robot_(robot) {} * @author mashiro
* @date 2026-07-09
common::Result<void> JsonCommandRunner::run(const config::JsonCommandProgram &program, std::ostream &out) { * @last_modified 2026-07-09
// 执行顺序固定：先连接，再按 commands 顺序逐条执行。 *
// 这里不做文件解析，保证 runner 可以直接用结构体做单元测试。 * @details
* 执行前统一输出连接目标和安全配置，便于用户确认当前 JSON 是否仍处于 dry-run
* 或是否允许真实运动。该函数只写日志，不连接硬件。
*
* @param[in] program JSON 指令程序。
* @param[out] out 日志输出流。
*
* @return 无返回值。
*
* @throws 本函数不主动抛出异常；输出流异常由调用方处理。
*
* @note
* 输出内容是人工可读诊断信息，不作为稳定机器协议。
*
* @warning
* 不要在这里触发任何硬件状态读取，保持启动日志纯输出。
*
* @thread_safety
* 函数无全局状态；并发写同一输出流需要调用方保护。
*
* @par 边界条件
* 空命令列表也会输出连接和 safety 摘要。
*
* @par 后续维护
* 新增 safety 字段时应同步输出，避免运行日志遗漏关键安全开关。
*/
void printProgramSummary(const config::JsonCommandProgram &program, std::ostream &out) {
out << "Connecting to RealMan arm at " << program.connection.ip << ':' out << "Connecting to RealMan arm at " << program.connection.ip << ':'
<< program.connection.port << " timeout=" << program.connection.timeout_ms << " ms\n"; << program.connection.port << " timeout=" << program.connection.timeout_ms << " ms\n";
out << "Safety: dry_run=" << (program.safety.dry_run ? "true" : "false") out << "Safety: dry_run=" << (program.safety.dry_run ? "true" : "false")
...@@ -46,168 +193,186 @@ common::Result<void> JsonCommandRunner::run(const config::JsonCommandProgram &pr ...@@ -46,168 +193,186 @@ common::Result<void> JsonCommandRunner::run(const config::JsonCommandProgram &pr
<< ", max_speed_percent=" << program.safety.max_speed_percent << ", max_speed_percent=" << program.safety.max_speed_percent
<< ", max_joint_delta_deg=" << program.safety.max_joint_delta_deg << ", max_joint_delta_deg=" << program.safety.max_joint_delta_deg
<< ", max_cartesian_step_m=" << program.safety.max_cartesian_step_m << '\n'; << ", max_cartesian_step_m=" << program.safety.max_cartesian_step_m << '\n';
}
/**
* @brief 基于当前关节角和相对增量生成绝对关节目标。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* RM_API2 的 `rm_movej` 接收绝对关节角。JSON 为人工调试提供相对增量命令，
* 因此执行前需要把当前关节值和增量相加，生成实际发送给驱动的目标。
*
* @param[in] current_joints_deg 当前关节角，单位为度。
* @param[in] delta_deg 相对关节增量，单位为度，长度应与当前关节角一致。
*
* @return 绝对关节目标列表。
*
* @throws 本函数不主动抛出异常；向量分配失败由标准库报告。
*
* @note
* 长度一致性由安全校验函数提前保证。
*
* @warning
* 如果调用方跳过长度校验，较短 delta 会造成越界；因此本函数只在校验后使用。
*
* @thread_safety
* 函数只使用局部变量，线程安全。
*
* @par 边界条件
* 空输入返回空输出。
*
* @par 后续维护
* 如果引入关节限位，应在安全校验层处理，不应让本函数承担策略判断。
*/
std::vector<float> buildAbsoluteJointTarget(const std::vector<float> &current_joints_deg,
const std::vector<float> &delta_deg) {
std::vector<float> target = current_joints_deg;
for (size_t joint_index = 0; joint_index < target.size(); ++joint_index) {
target[joint_index] += delta_deg[joint_index];
}
return target;
}
} // namespace
JsonCommandRunner::JsonCommandRunner(interfaces::IRobotArm &robot)
: robot_(robot) {}
auto connect_result = robot_.connect(program.connection); common::Result<void> JsonCommandRunner::run(const config::JsonCommandProgram &program,
std::ostream &out) {
printProgramSummary(program, out);
const auto connect_result = robot_.connect(program.connection);
if (!connect_result.ok) { if (!connect_result.ok) {
return connect_result; return connect_result;
} }
for (size_t i = 0; i < program.commands.size(); ++i) { for (size_t command_index = 0; command_index < program.commands.size(); ++command_index) {
const config::ArmCommand &command = program.commands[i]; const auto result = runSingleCommand(program,
out << "\n[" << (i + 1) << "/" << program.commands.size() << "] " program.commands[command_index],
<< command.name << " (" << config::commandTypeName(command.type) << ")\n"; command_index,
program.commands.size(),
switch (command.type) { out);
case config::CommandType::GetRobotInfo: { if (!result.ok) {
auto info = robot_.getRobotInfo(); return result;
if (!info.ok) {
return common::Result<void>::failure(info.error_message);
}
out << "arm_dof=" << info.value.arm_dof
<< ", arm_model=" << info.value.arm_model
<< ", force_type=" << info.value.force_type
<< ", controller_version=" << info.value.controller_version << '\n';
break;
}
case config::CommandType::GetCurrentArmState: {
auto state = robot_.getCurrentArmState();
if (!state.ok) {
return common::Result<void>::failure(state.error_message);
}
out << "joints_deg=";
printJoints(out, state.value.joints_deg);
out << ", tcp_pose=";
printPose(out, state.value.tcp_pose);
out << ", error_count=" << state.value.error_codes.size() << '\n';
break;
} }
case config::CommandType::GetForceData: { }
auto force = robot_.getForceData();
if (!force.ok) { return common::Result<void>::success();
return common::Result<void>::failure(force.error_message); }
}
out << std::fixed << std::setprecision(4) common::Result<void> JsonCommandRunner::runSingleCommand(
<< "work_zero_force=[Fx=" << force.value.work_zero[0] const config::JsonCommandProgram &program,
<< ", Fy=" << force.value.work_zero[1] const config::ArmCommand &command,
<< ", Fz=" << force.value.work_zero[2] size_t command_index,
<< ", Mx=" << force.value.work_zero[3] size_t command_count,
<< ", My=" << force.value.work_zero[4] std::ostream &out) {
<< ", Mz=" << force.value.work_zero[5] << "]\n"; out << "\n[" << (command_index + 1) << "/" << command_count << "] "
break; << command.name << " (" << config::commandTypeName(command.type) << ")\n";
switch (command.type) {
case config::CommandType::GetRobotInfo:
case config::CommandType::GetCurrentArmState:
case config::CommandType::GetForceData:
return runReadOnlyCommand(command, out);
case config::CommandType::SleepMs:
out << "sleep " << command.duration_ms << " ms\n";
std::this_thread::sleep_for(std::chrono::milliseconds(command.duration_ms));
return common::Result<void>::success();
case config::CommandType::MoveJ:
case config::CommandType::MoveJDelta:
case config::CommandType::MoveJDeltaSteps:
case config::CommandType::MoveL:
return runMotionCommand(program, command, out);
}
return common::Result<void>::failure("unsupported command enum value");
}
common::Result<void> JsonCommandRunner::runReadOnlyCommand(const config::ArmCommand &command,
std::ostream &out) {
if (command.type == config::CommandType::GetRobotInfo) {
const auto info = robot_.getRobotInfo();
if (!info.ok) {
return common::Result<void>::failure(info.error_message);
} }
case config::CommandType::SleepMs: out << "arm_dof=" << info.value.arm_dof
out << "sleep " << command.duration_ms << " ms\n"; << ", arm_model=" << info.value.arm_model
std::this_thread::sleep_for(std::chrono::milliseconds(command.duration_ms)); << ", force_type=" << info.value.force_type
break; << ", controller_version=" << info.value.controller_version << '\n';
case config::CommandType::MoveJ: return common::Result<void>::success();
case config::CommandType::MoveJDelta: }
case config::CommandType::MoveJDeltaSteps:
case config::CommandType::MoveL: { if (command.type == config::CommandType::GetCurrentArmState) {
auto current_state = robot_.getCurrentArmState(); const auto state = robot_.getCurrentArmState();
if (!current_state.ok) { if (!state.ok) {
return common::Result<void>::failure(current_state.error_message); return common::Result<void>::failure(state.error_message);
}
auto validation = validateMotionCommand(program, command, current_state.value);
if (!validation.ok) {
return validation;
}
if (program.safety.dry_run) {
// dry_run 仍然会连接并读取当前状态，以便验证安全边界；
// 但不会发送 rm_movej/rm_movel。
out << "dry_run=true, motion command validated but not sent.\n";
break;
}
if (!program.safety.allow_motion) {
return common::Result<void>::failure(
"motion command refused because safety.allow_motion is false");
}
common::Result<void> move_result;
if (command.type == config::CommandType::MoveJ) {
out << "sending rm_movej target=";
printJoints(out, command.joints_deg);
out << ", speed=" << command.motion.speed_percent << "%\n";
move_result = robot_.moveJ(command.joints_deg,
command.motion.speed_percent,
command.motion.blend_radius,
command.motion.trajectory_connect,
command.motion.blocking);
} else if (command.type == config::CommandType::MoveJDelta) {
// 相对关节运动：每次发送前基于当前读数生成绝对目标。
// RM_API2 的 rm_movej 需要绝对关节角，不接受相对增量。
std::vector<float> target = current_state.value.joints_deg;
for (size_t joint = 0; joint < target.size(); ++joint) {
target[joint] += command.joint_delta_deg[joint];
}
out << "sending rm_movej relative delta=";
printJoints(out, command.joint_delta_deg);
out << ", target=";
printJoints(out, target);
out << ", speed=" << command.motion.speed_percent << "%\n";
move_result = robot_.moveJ(target,
command.motion.speed_percent,
command.motion.blend_radius,
command.motion.trajectory_connect,
command.motion.blocking);
} else if (command.type == config::CommandType::MoveJDeltaSteps) {
// 阶梯相对运动：适合把 20 度这类较大总幅度拆成多次小步，
// 既便于观察，也避免单条命令产生大幅跳动。
out << "sending stepped rm_movej total_delta=";
printJoints(out, command.joint_delta_deg);
out << ", steps=" << command.steps
<< ", speed=" << command.motion.speed_percent << "%\n";
move_result = common::Result<void>::success();
for (int step = 0; step < command.steps; ++step) {
// 每一步都重新读取当前状态，而不是用上一条目标值递推。
// 这样可以吸收控制器实际到位误差，避免累计误差越来越大。
auto step_state = robot_.getCurrentArmState();
if (!step_state.ok) {
move_result = common::Result<void>::failure(step_state.error_message);
break;
}
std::vector<float> target = step_state.value.joints_deg;
for (size_t joint = 0; joint < target.size(); ++joint) {
target[joint] += command.joint_delta_deg[joint] / static_cast<float>(command.steps);
}
out << " step " << (step + 1) << "/" << command.steps << " target=";
printJoints(out, target);
out << '\n';
move_result = robot_.moveJ(target,
command.motion.speed_percent,
command.motion.blend_radius,
command.motion.trajectory_connect,
command.motion.blocking);
if (!move_result.ok) {
break;
}
if (command.step_sleep_ms > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(command.step_sleep_ms));
}
}
} else {
out << "sending rm_movel target=";
printPose(out, command.pose);
out << ", speed=" << command.motion.speed_percent << "%\n";
move_result = robot_.moveL(command.pose,
command.motion.speed_percent,
command.motion.blend_radius,
command.motion.trajectory_connect,
command.motion.blocking);
}
if (!move_result.ok) {
return move_result;
}
break;
} }
out << "joints_deg=";
printJoints(out, state.value.joints_deg);
out << ", tcp_pose=";
printPose(out, state.value.tcp_pose);
out << ", error_count=" << state.value.error_codes.size() << '\n';
return common::Result<void>::success();
}
if (command.type == config::CommandType::GetForceData) {
const auto force = robot_.getForceData();
if (!force.ok) {
return common::Result<void>::failure(force.error_message);
} }
out << std::fixed << std::setprecision(4)
<< "work_zero_force=[Fx=" << force.value.work_zero[0]
<< ", Fy=" << force.value.work_zero[1]
<< ", Fz=" << force.value.work_zero[2]
<< ", Mx=" << force.value.work_zero[3]
<< ", My=" << force.value.work_zero[4]
<< ", Mz=" << force.value.work_zero[5] << "]\n";
return common::Result<void>::success();
} }
return common::Result<void>::success(); return common::Result<void>::failure("command is not a read-only command");
}
common::Result<void> JsonCommandRunner::runMotionCommand(
const config::JsonCommandProgram &program,
const config::ArmCommand &command,
std::ostream &out) {
const auto current_state = robot_.getCurrentArmState();
if (!current_state.ok) {
return common::Result<void>::failure(current_state.error_message);
}
const auto validation = validateMotionCommand(program, command, current_state.value);
if (!validation.ok) {
return validation;
}
if (program.safety.dry_run) {
out << "dry_run=true, motion command validated but not sent.\n";
return common::Result<void>::success();
}
if (!program.safety.allow_motion) {
return common::Result<void>::failure(
"motion command refused because safety.allow_motion is false");
}
switch (command.type) {
case config::CommandType::MoveJ:
return executeMoveJCommand(command, out);
case config::CommandType::MoveJDelta:
return executeMoveJDeltaCommand(command, current_state.value, out);
case config::CommandType::MoveJDeltaSteps:
return executeMoveJDeltaStepsCommand(command, out);
case config::CommandType::MoveL:
return executeMoveLCommand(command, out);
default:
return common::Result<void>::failure("command is not a motion command");
}
} }
common::Result<void> JsonCommandRunner::validateMotionCommand( common::Result<void> JsonCommandRunner::validateMotionCommand(
...@@ -215,71 +380,92 @@ common::Result<void> JsonCommandRunner::validateMotionCommand( ...@@ -215,71 +380,92 @@ common::Result<void> JsonCommandRunner::validateMotionCommand(
const config::ArmCommand &command, const config::ArmCommand &command,
const common::ArmState &current_state) const { const common::ArmState &current_state) const {
if (command.motion.speed_percent > program.safety.max_speed_percent) { if (command.motion.speed_percent > program.safety.max_speed_percent) {
// 速度上限是全局安全策略，不允许单条命令绕过。
return common::Result<void>::failure("motion speed exceeds safety.max_speed_percent"); return common::Result<void>::failure("motion speed exceeds safety.max_speed_percent");
} }
if (command.type == config::CommandType::MoveJ) { switch (command.type) {
// 绝对关节目标最容易写错，所以用当前关节值计算差值做限幅。 case config::CommandType::MoveJ:
if (command.joints_deg.size() != current_state.joints_deg.size()) { return validateMoveJCommand(program, command, current_state);
return common::Result<void>::failure( case config::CommandType::MoveJDelta:
"movej joints_deg size must match current robot DOF"); return validateMoveJDeltaCommand(program, command, current_state);
} case config::CommandType::MoveJDeltaSteps:
return validateMoveJDeltaStepsCommand(program, command, current_state);
case config::CommandType::MoveL:
return validateMoveLCommand(program, command, current_state);
default:
return common::Result<void>::failure("command is not a motion command");
}
}
for (size_t i = 0; i < command.joints_deg.size(); ++i) { common::Result<void> JsonCommandRunner::validateMoveJCommand(
const float delta = absDiff(command.joints_deg[i], current_state.joints_deg[i]); const config::JsonCommandProgram &program,
if (delta > program.safety.max_joint_delta_deg) { const config::ArmCommand &command,
return common::Result<void>::failure( const common::ArmState &current_state) const {
"movej J" + std::to_string(i + 1) + " delta " + std::to_string(delta) + if (command.joints_deg.size() != current_state.joints_deg.size()) {
" deg exceeds safety.max_joint_delta_deg"); return common::Result<void>::failure("movej joints_deg size must match current robot DOF");
}
}
return common::Result<void>::success();
} }
if (command.type == config::CommandType::MoveJDelta) { for (size_t joint_index = 0; joint_index < command.joints_deg.size(); ++joint_index) {
// 相对关节目标直接检查每个关节的增量绝对值。 const float delta = absDiff(command.joints_deg[joint_index],
if (command.joint_delta_deg.size() != current_state.joints_deg.size()) { current_state.joints_deg[joint_index]);
if (delta > program.safety.max_joint_delta_deg) {
return common::Result<void>::failure( return common::Result<void>::failure(
"movej_delta delta_deg size must match current robot DOF"); "movej J" + std::to_string(joint_index + 1) + " delta " +
std::to_string(delta) + " deg exceeds safety.max_joint_delta_deg");
} }
}
return common::Result<void>::success();
}
for (size_t i = 0; i < command.joint_delta_deg.size(); ++i) { common::Result<void> JsonCommandRunner::validateMoveJDeltaCommand(
const float delta = std::fabs(command.joint_delta_deg[i]); const config::JsonCommandProgram &program,
if (delta > program.safety.max_joint_delta_deg) { const config::ArmCommand &command,
return common::Result<void>::failure( const common::ArmState &current_state) const {
"movej_delta J" + std::to_string(i + 1) + " delta " + std::to_string(delta) + if (command.joint_delta_deg.size() != current_state.joints_deg.size()) {
" deg exceeds safety.max_joint_delta_deg"); return common::Result<void>::failure("movej_delta delta_deg size must match current robot DOF");
}
}
return common::Result<void>::success();
} }
if (command.type == config::CommandType::MoveJDeltaSteps) { for (size_t joint_index = 0; joint_index < command.joint_delta_deg.size(); ++joint_index) {
// 阶梯运动允许总幅度较大，但每一步仍必须满足安全步进限制。 const float delta = std::fabs(command.joint_delta_deg[joint_index]);
if (command.joint_delta_deg.size() != current_state.joints_deg.size()) { if (delta > program.safety.max_joint_delta_deg) {
return common::Result<void>::failure( return common::Result<void>::failure(
"movej_delta_steps delta_deg size must match current robot DOF"); "movej_delta J" + std::to_string(joint_index + 1) + " delta " +
} std::to_string(delta) + " deg exceeds safety.max_joint_delta_deg");
if (command.steps <= 0) {
return common::Result<void>::failure("movej_delta_steps steps must be greater than 0");
} }
}
return common::Result<void>::success();
}
for (size_t i = 0; i < command.joint_delta_deg.size(); ++i) { common::Result<void> JsonCommandRunner::validateMoveJDeltaStepsCommand(
const float per_step_delta = std::fabs(command.joint_delta_deg[i]) / const config::JsonCommandProgram &program,
static_cast<float>(command.steps); const config::ArmCommand &command,
if (per_step_delta > program.safety.max_joint_delta_deg) { const common::ArmState &current_state) const {
return common::Result<void>::failure( if (command.joint_delta_deg.size() != current_state.joints_deg.size()) {
"movej_delta_steps J" + std::to_string(i + 1) + " per-step delta " + return common::Result<void>::failure(
std::to_string(per_step_delta) + "movej_delta_steps delta_deg size must match current robot DOF");
" deg exceeds safety.max_joint_delta_deg"); }
} if (command.steps <= 0) {
return common::Result<void>::failure("movej_delta_steps steps must be greater than 0");
}
for (size_t joint_index = 0; joint_index < command.joint_delta_deg.size(); ++joint_index) {
const float per_step_delta =
std::fabs(command.joint_delta_deg[joint_index]) / static_cast<float>(command.steps);
if (per_step_delta > program.safety.max_joint_delta_deg) {
return common::Result<void>::failure(
"movej_delta_steps J" + std::to_string(joint_index + 1) +
" per-step delta " + std::to_string(per_step_delta) +
" deg exceeds safety.max_joint_delta_deg");
} }
return common::Result<void>::success();
} }
return common::Result<void>::success();
}
common::Result<void> JsonCommandRunner::validateMoveLCommand(
const config::JsonCommandProgram &program,
const config::ArmCommand &command,
const common::ArmState &current_state) const {
const float dx = absDiff(command.pose.x, current_state.tcp_pose.x); const float dx = absDiff(command.pose.x, current_state.tcp_pose.x);
// 笛卡尔运动按 xyz 和欧拉角分别限幅，避免误把 m/rad 写成 mm/deg。
const float dy = absDiff(command.pose.y, current_state.tcp_pose.y); const float dy = absDiff(command.pose.y, current_state.tcp_pose.y);
const float dz = absDiff(command.pose.z, current_state.tcp_pose.z); const float dz = absDiff(command.pose.z, current_state.tcp_pose.z);
if (dx > program.safety.max_cartesian_step_m || if (dx > program.safety.max_cartesian_step_m ||
...@@ -300,4 +486,87 @@ common::Result<void> JsonCommandRunner::validateMotionCommand( ...@@ -300,4 +486,87 @@ common::Result<void> JsonCommandRunner::validateMotionCommand(
return common::Result<void>::success(); return common::Result<void>::success();
} }
common::Result<void> JsonCommandRunner::executeMoveJCommand(const config::ArmCommand &command,
std::ostream &out) {
out << "sending rm_movej target=";
printJoints(out, command.joints_deg);
out << ", speed=" << command.motion.speed_percent << "%\n";
return robot_.moveJ(command.joints_deg,
command.motion.speed_percent,
command.motion.blend_radius,
command.motion.trajectory_connect,
command.motion.blocking);
}
common::Result<void> JsonCommandRunner::executeMoveJDeltaCommand(
const config::ArmCommand &command,
const common::ArmState &current_state,
std::ostream &out) {
const std::vector<float> target =
buildAbsoluteJointTarget(current_state.joints_deg, command.joint_delta_deg);
out << "sending rm_movej relative delta=";
printJoints(out, command.joint_delta_deg);
out << ", target=";
printJoints(out, target);
out << ", speed=" << command.motion.speed_percent << "%\n";
return robot_.moveJ(target,
command.motion.speed_percent,
command.motion.blend_radius,
command.motion.trajectory_connect,
command.motion.blocking);
}
common::Result<void> JsonCommandRunner::executeMoveJDeltaStepsCommand(
const config::ArmCommand &command,
std::ostream &out) {
out << "sending stepped rm_movej total_delta=";
printJoints(out, command.joint_delta_deg);
out << ", steps=" << command.steps
<< ", speed=" << command.motion.speed_percent << "%\n";
for (int step_index = 0; step_index < command.steps; ++step_index) {
const auto step_state = robot_.getCurrentArmState();
if (!step_state.ok) {
return common::Result<void>::failure(step_state.error_message);
}
std::vector<float> per_step_delta = command.joint_delta_deg;
for (float &joint_delta : per_step_delta) {
joint_delta /= static_cast<float>(command.steps);
}
const std::vector<float> target =
buildAbsoluteJointTarget(step_state.value.joints_deg, per_step_delta);
out << " step " << (step_index + 1) << "/" << command.steps << " target=";
printJoints(out, target);
out << '\n';
const auto move_result = robot_.moveJ(target,
command.motion.speed_percent,
command.motion.blend_radius,
command.motion.trajectory_connect,
command.motion.blocking);
if (!move_result.ok) {
return move_result;
}
if (command.step_sleep_ms > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(command.step_sleep_ms));
}
}
return common::Result<void>::success();
}
common::Result<void> JsonCommandRunner::executeMoveLCommand(const config::ArmCommand &command,
std::ostream &out) {
out << "sending rm_movel target=";
printPose(out, command.pose);
out << ", speed=" << command.motion.speed_percent << "%\n";
return robot_.moveL(command.pose,
command.motion.speed_percent,
command.motion.blend_radius,
command.motion.trajectory_connect,
command.motion.blocking);
}
} // namespace rm_control::core } // namespace rm_control::core
RM/src/core/o6_bus_scanner.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/core/o6_bus_scanner.h"
#include <utility>
namespace rm_control::core {
namespace {
bool MatchesO6IdentityShape(std::uint8_t device_id,
const std::vector<std::uint16_t> &registers) {
if (registers.size() != 6U || registers[0] != 6U) {
return false;
}
const char expected_direction = device_id == 0x27U ? 'R' : 'L';
return registers[5] == static_cast<std::uint16_t>(expected_direction);
}
common::Result<void> ValidateScanConfig(const O6BusScanConfig &config) {
const std::vector<std::uint8_t> expected_device_ids{0x27U, 0x28U};
if (config.device_ids != expected_device_ids ||
config.identity_start_address != 30U ||
config.identity_register_count != 6U) {
return common::Result<void>::failure(
"O6 bus probe is fixed to device IDs 0x27/0x28 and FC04 addresses 30..35");
}
return common::Result<void>::success();
}
O6BusScanResponse ProbeOneDevice(interfaces::IToolModbusBus &tool_bus,
const O6BusScanConfig &config,
std::uint8_t device_id) {
O6BusScanResponse response;
response.device_id = device_id;
const auto multiple = tool_bus.ReadInputRegisters(
device_id, config.identity_start_address,
config.identity_register_count);
response.multiple_read_succeeded = multiple.ok;
if (multiple.ok) {
response.identity_registers = multiple.value;
if (multiple.value.size() != config.identity_register_count) {
response.multiple_read_succeeded = false;
response.multiple_read_error =
"multiple input read returned an unexpected register count";
}
} else {
response.multiple_read_error = multiple.error_message;
}
const auto single = tool_bus.ReadInputRegister(
device_id, config.identity_start_address);
response.single_read_succeeded = single.ok;
if (single.ok) {
response.single_freedom_register = single.value;
} else {
response.single_read_error = single.error_message;
}
response.single_and_multiple_reads_match =
response.multiple_read_succeeded && response.single_read_succeeded &&
response.identity_registers.front() == response.single_freedom_register;
response.matches_o6_identity_shape =
response.single_and_multiple_reads_match &&
MatchesO6IdentityShape(device_id, response.identity_registers);
return response;
}
} // namespace
common::Result<O6BusScanReport> ScanO6IdentityRegisters(
interfaces::IToolModbusBus &tool_bus,
const O6BusScanConfig &config) {
const auto valid = ValidateScanConfig(config);
if (!valid.ok) {
return common::Result<O6BusScanReport>::failure(valid.error_message);
}
O6BusScanReport report;
for (const std::uint8_t device_id : config.device_ids) {
++report.attempted_device_count;
auto response = ProbeOneDevice(tool_bus, config, device_id);
if (response.matches_o6_identity_shape) {
++report.confirmed_o6_device_count;
} else {
++report.failed_device_count;
}
report.responses.push_back(std::move(response));
}
return common::Result<O6BusScanReport>::success(report);
}
} // namespace rm_control::core
RM/src/drag_teach_record_main.cpp
View file @ 8275ee2e
#include "rm_control/core/drag_teach_recorder.h" /**
#include "rm_control/drivers/realman/realman_robot_arm.h" * @file drag_teach_record_main.cpp
#include "rm_control/modules/trajectory/trajectory_file_store.h" * @brief RM75-6F 拖动示教原始轨迹采集程序入口。
*
#include <cstdlib> * @author 唐永康
* @date 2026-07-10
*
* @details
* Application 只负责 CLI、配置、依赖组装、人工确认和进程退出码。型号/状态预检、
* 拖动状态机、内存采样、轨迹校验、唯一目录、SHA-256 和 manifest 均由服务层完成。
*/
#include "flight_robot/realman/realman_arm_adapter.h"
#include "rm_control/config/drag_teach_cli_config.h"
#include "rm_control/config/drag_teach_safety_config.h"
#include "rm_control/core/drag_teach_service.h"
#include <chrono>
#include <cstddef>
#include <iostream> #include <iostream>
#include <string> #include <string>
#include <thread>
namespace { namespace {
using rm_control::core::DragTeachRecordConfig; constexpr int kInvalidArgumentsExitCode = 64;
constexpr int kInvalidSafetyConfigExitCode = 65;
void printUsage(const char *program_name) { constexpr int kHardwareExecutionRefusedExitCode = 77;
std::cout << "Usage: " << program_name constexpr int kConnectionFailureExitCode = 1;
<< " [session_name] [duration_seconds] [sample_period_ms] [storage_root]" constexpr int kRecordingFailureExitCode = 2;
<< " [--no-vendor-save] [--ordinary-recorded] [--no-singular-wall]\n" constexpr int kMinimumRecordingDurationSeconds = 1;
<< "\n" constexpr int kMaximumRecordingDurationSeconds = 3600;
<< "Default mode is RM75 six-dof force drag teach with position and orientation enabled.\n" constexpr int kMinimumSamplePeriodMilliseconds = 10;
<< "The program records synchronized CSV as the primary artifact.\n"; constexpr int kMaximumSamplePeriodMilliseconds = 1000;
} constexpr const char *kOperatorConfirmationPhrase = "CONFIRM RM75 DRAG TEACH";
bool parseInt(const char *value, int *out) { rm_control::common::Result<void> ValidateApplicationRecordingConfig(
if (value == nullptr || out == nullptr || value[0] == '\0') { const rm_control::config::DragTeachRecordCliConfig &config) {
return false; if (config.record_config.duration_seconds < kMinimumRecordingDurationSeconds ||
config.record_config.duration_seconds > kMaximumRecordingDurationSeconds) {
return rm_control::common::Result<void>::failure(
"duration_seconds must be in range 1..3600");
} }
if (config.record_config.sample_period_ms < kMinimumSamplePeriodMilliseconds ||
char *end = nullptr; config.record_config.sample_period_ms > kMaximumSamplePeriodMilliseconds) {
const long parsed = std::strtol(value, &end, 10); return rm_control::common::Result<void>::failure(
if (end == value || *end != '\0') { "sample_period_ms must be in range 10..1000");
return false; }
if (config.record_config.storage_root.empty() ||
config.record_config.session_name.empty()) {
return rm_control::common::Result<void>::failure(
"storage_root and session_name must not be empty");
} }
*out = static_cast<int>(parsed); if (!config.record_config.save_vendor_trajectory) {
return true; return rm_control::common::Result<void>::failure(
"DragTeachService requires vendor raw trajectory saving; remove --no-vendor-save");
}
return rm_control::common::Result<void>::success();
} }
std::string defaultSessionName() { bool IsO6ConnectionVerifiedByDeviceService() noexcept {
return "rm75_drag_pose_" + rm_control::modules::trajectory::timestampString(); // 当前 RM 独立子工程尚未接入 IDexterousHand 设备服务。安全规则要求真实 O6
// 连接必须来自实际状态读取，因此这里保持 false，禁止用 CLI 或环境变量伪造。
return false;
} }
bool parseArguments(int argc, char **argv, DragTeachRecordConfig *config) { bool RequestSecondOperatorConfirmation(std::istream &input, std::ostream &output) {
if (config == nullptr) { output << "Type exactly '" << kOperatorConfirmationPhrase
return false; << "' to authorize RM75 drag teach: " << std::flush;
} std::string confirmation;
if (argc > 1 && (std::string(argv[1]) == "-h" || std::string(argv[1]) == "--help")) { return std::getline(input, confirmation) &&
printUsage(argv[0]); confirmation == kOperatorConfirmationPhrase;
std::exit(0); }
}
config->session_name = defaultSessionName();
config->storage_root = rm_control::modules::trajectory::defaultTrajectoryStorageRoot();
config->drag_options.mode = rm_control::common::DragTeachMode::SixDofForcePositionAndOrientation;
config->drag_options.enable_singular_wall = true;
config->drag_options.record_vendor_trajectory = true;
int positional_index = 0; rm_control::core::DragTeachServiceConfig BuildServiceConfig(
for (int i = 1; i < argc; ++i) { const rm_control::config::DragTeachRecordCliConfig &cli,
const std::string arg = argv[i] == nullptr ? "" : argv[i]; const rm_control::config::DragTeachSafetyConfig &safety) {
if (arg == "--no-vendor-save") { rm_control::core::DragTeachServiceConfig config;
config->save_vendor_trajectory = false; config.storage_root = cli.record_config.storage_root;
continue; config.session_name = cli.record_config.session_name;
} config.sample_period =
if (arg == "--ordinary-recorded") { std::chrono::milliseconds(cli.record_config.sample_period_ms);
config->drag_options.mode = rm_control::common::DragTeachMode::OrdinaryRecorded; config.drag_options = cli.record_config.drag_options;
continue; config.validation_limits.maximum_joint_jump_deg = safety.max_joint_jump_deg;
} config.validation_limits.maximum_joint_velocity_deg_per_second =
if (arg == "--no-singular-wall") { safety.max_joint_velocity_deg_per_second;
config->drag_options.enable_singular_wall = false; config.validation_limits.maximum_joint_acceleration_deg_per_second_squared =
continue; safety.max_joint_acceleration_deg_per_second_squared;
} config.maximum_absolute_force_newton = safety.maximum_absolute_force_newton;
if (!arg.empty() && arg[0] == '-') { config.maximum_absolute_torque_newton_meter =
std::cerr << "Unknown option: " << arg << '\n'; safety.maximum_absolute_torque_newton_meter;
return false; const auto expected_sample_count = static_cast<std::size_t>(
} (cli.record_config.duration_seconds * 1000) /
cli.record_config.sample_period_ms);
config.maximum_sample_count = expected_sample_count + 2U;
return config;
}
++positional_index; rm_control::core::HardwareExecutionGateInput BuildExecutionGate(
if (positional_index == 1) { const rm_control::config::DragTeachRecordCliConfig &cli,
config->session_name = arg; const rm_control::config::DragTeachSafetyConfig &safety,
} else if (positional_index == 2) { bool arm_connected,
if (!parseInt(arg.c_str(), &config->duration_seconds)) { bool hand_connected,
std::cerr << "Invalid duration_seconds: " << arg << '\n'; bool operator_confirmed) {
return false; rm_control::core::HardwareExecutionGateInput gate;
} gate.live_requested = cli.live_requested;
} else if (positional_index == 3) { gate.hardware_execution_enabled = safety.hardware_execution_enabled;
if (!parseInt(arg.c_str(), &config->sample_period_ms)) { gate.arm_connected = arm_connected;
std::cerr << "Invalid sample_period_ms: " << arg << '\n'; gate.hand_connected = hand_connected;
return false; gate.emergency_stop_verified = safety.emergency_stop_verified;
} gate.safety_limits_loaded = safety.safety_limits_loaded;
} else if (positional_index == 4) { gate.operator_confirmed = operator_confirmed;
config->storage_root = arg; gate.trajectory_validated = true;
} else { gate.trajectory_sample_period_confirmed =
std::cerr << "Too many positional arguments.\n"; safety.trajectory_sample_period_confirmed;
return false; gate.vendor_raw_units_per_degree_confirmed =
} safety.vendor_raw_units_per_degree_confirmed;
} gate.workspace_and_virtual_wall_verified =
safety.workspace_and_virtual_wall_verified;
gate.configured_max_speed_percent = safety.max_speed_percent;
gate.native_replay_without_speed_control_accepted =
safety.native_replay_without_speed_control_accepted;
gate.native_replay_speed_within_config_verified = false;
return gate;
}
return true; void PrintArtifactSummary(
const rm_control::modules::trajectory::RawTrajectoryArtifact &artifact) {
std::cout << "Trajectory points: " << artifact.metadata.trajectory_point_count << '\n'
<< "Recorded samples: " << artifact.metadata.recorded_sample_count << '\n'
<< "Session directory: " << artifact.paths.directory << '\n'
<< "Raw trajectory: " << artifact.paths.raw_trajectory << '\n'
<< "Manifest: " << artifact.paths.manifest << '\n'
<< "Raw bytes: " << artifact.raw_file_size_bytes << '\n'
<< "SHA-256: " << artifact.raw_file_sha256 << '\n'
<< "Save return code: "
<< artifact.metadata.vendor_save_return_code << '\n';
} }
} // namespace } // namespace
/**
* @brief 安全地执行一次 RM75-6F 拖动示教原始轨迹采集。
*
* @reason 入口按 fail-closed 顺序装配所有独立授权条件，任何缺项都在运动 API 前退出。
*
* @author 唐永康
* @date 2026-07-10
*
* @param argc 命令行参数数量。
* @param argv 命令行参数数组；所有外部值在使用前校验。
* @return 0 表示帮助、DryRun 检查或采集成功；其他值表示参数、门控、连接或采集失败。
*
* @calls ParseDragTeachRecordCliArguments、LoadDragTeachSafetyConfig、
* RealmanArmAdapter::connect 和 DragTeachService。
* @safety 只有 `--live`、启用配置、真实设备状态、人工二次确认等条件全部满足才会启动拖动。
* @note 当前入口没有 O6 设备服务，因此 Live 模式会明确 fail-closed；单元测试通过 Mock 覆盖完整流程。
*/
int main(int argc, char **argv) { int main(int argc, char **argv) {
DragTeachRecordConfig config; const auto cli = rm_control::config::ParseDragTeachRecordCliArguments(argc, argv);
if (!parseArguments(argc, argv, &config)) { if (!cli.ok) {
printUsage(argv[0]); std::cerr << "Invalid arguments: " << cli.error_message << '\n';
return 64; rm_control::config::PrintDragTeachRecordUsage(
argv == nullptr ? nullptr : argv[0], std::cerr);
return kInvalidArgumentsExitCode;
}
if (cli.value.help_requested) {
rm_control::config::PrintDragTeachRecordUsage(
argv == nullptr ? nullptr : argv[0], std::cout);
return 0;
}
const auto application_config = ValidateApplicationRecordingConfig(cli.value);
const auto safety = rm_control::config::LoadDragTeachSafetyConfig(
cli.value.safety_config_path);
if (!application_config.ok || !safety.ok) {
std::cerr << (!application_config.ok ? application_config.error_message
: safety.error_message)
<< '\n';
return kInvalidSafetyConfigExitCode;
}
if (!cli.value.live_requested) {
std::cout << "DryRun: configuration validated; no device was connected and no motion was executed.\n";
return 0;
}
if (!safety.value.hardware_execution_enabled) {
std::cerr << "Hardware execution refused: hardware_execution_enabled is false.\n";
return kHardwareExecutionRefusedExitCode;
} }
rm_control::drivers::realman::RealManRobotArm robot; flight_robot::realman::RealmanArmAdapter robot;
rm_control::common::RobotConnectionConfig connection; const rm_control::common::RobotConnectionConfig connection;
auto connect_result = robot.connect(connection); const auto connected = robot.connect(connection);
if (!connect_result.ok) { if (!connected.ok) {
std::cerr << "Failed to connect RM75: " << connect_result.error_message << '\n'; std::cerr << "Failed to connect RM75: " << connected.error_message << '\n';
return 1; return kConnectionFailureExitCode;
} }
rm_control::core::DragTeachRecorder recorder(robot); const bool hand_connected = IsO6ConnectionVerifiedByDeviceService();
auto summary = recorder.record(config, std::cout); const bool operator_confirmed =
if (!summary.ok) { hand_connected && RequestSecondOperatorConfirmation(std::cin, std::cout);
std::cerr << "Drag teach record failed: " << summary.error_message << '\n'; const auto gate = BuildExecutionGate(cli.value, safety.value,
return 2; robot.isConnected(), hand_connected,
operator_confirmed);
rm_control::core::DragTeachService service(robot);
const auto start = service.StartRecording(
BuildServiceConfig(cli.value, safety.value), gate);
if (!start.ok) {
std::cerr << "Drag teach start refused: " << start.error_message << '\n';
return kHardwareExecutionRefusedExitCode;
} }
std::cout << "Recorded samples: " << summary.value.sample_count << '\n' std::cout << "Recording started. Manually drag the RM75 under direct supervision.\n";
<< "CSV: " << summary.value.files.csv_path << '\n' std::this_thread::sleep_for(
<< "Vendor: " << summary.value.files.vendor_path << '\n' std::chrono::seconds(cli.value.record_config.duration_seconds));
<< "Converted C++ target path: " << summary.value.files.converted_cpp_path << '\n'; const auto stopped = service.StopRecording();
return 0; if (!stopped.ok) {
std::cerr << "Drag teach stop/validation failed: "
<< stopped.error_message << '\n';
return kRecordingFailureExitCode;
}
const auto artifact = service.SaveRawTrajectory();
if (!artifact.ok) {
std::cerr << "Raw trajectory finalization failed: "
<< artifact.error_message << '\n';
return kRecordingFailureExitCode;
}
PrintArtifactSummary(artifact.value);
return artifact.value.raw_file_present &&
artifact.value.metadata.vendor_save_return_code == 0
? 0
: kRecordingFailureExitCode;
} }
RM/src/drivers/realman/realman_robot_arm.cpp
View file @ 8275ee2e
#include "rm_control/drivers/realman/realman_robot_arm.h" #include "rm_control/drivers/realman/realman_robot_arm.h"
#include <cstdarg> // 兼容空翻译单元：唯一 RM_API2 实现在 src/realman/realman_arm_adapter.cpp。
#include <cstdio>
#include <string>
namespace rm_control::drivers::realman {
namespace {
void apiLogCallback(const char *message, va_list args) {
// RM_API2 的日志回调是 C 风格 va_list，这里统一转到 stderr。
if (message == nullptr) {
return;
}
char buffer[1024] = {};
std::vsnprintf(buffer, sizeof(buffer), message, args);
std::fprintf(stderr, "[rm_api] %s\n", buffer);
}
int destroyApi() {
// 兼容不同版本 RM_API2：有些头文件拼写为 rm_destroy，有些为 rm_destory。
#if defined(REALMAN_RM_API_HAS_RM_DESTROY)
return rm_destroy();
#else
return rm_destory();
#endif
}
common::Pose convertPose(const rm_pose_t &pose) {
// 只把上层需要的位置和欧拉角带出去，四元数留在驱动内部。
common::Pose result;
result.x = pose.position.x;
result.y = pose.position.y;
result.z = pose.position.z;
result.rx = pose.euler.rx;
result.ry = pose.euler.ry;
result.rz = pose.euler.rz;
return result;
}
rm_pose_t toRmPose(const common::Pose &pose) {
// RM_API2 的 rm_pose_t 同时包含四元数和欧拉角。
// 当前 JSON 使用欧拉角表达，四元数给单位值作为安全默认。
rm_pose_t result = {};
result.position.x = pose.x;
result.position.y = pose.y;
result.position.z = pose.z;
result.euler.rx = pose.rx;
result.euler.ry = pose.ry;
result.euler.rz = pose.rz;
result.quaternion.w = 1.0F;
return result;
}
} // namespace
RealManRobotArm::~RealManRobotArm() {
// RAII：无论调用方是否显式 disconnect，析构时都释放 SDK 资源。
disconnect();
}
common::Result<void> RealManRobotArm::connect(const common::RobotConnectionConfig &config) {
// 先断开旧连接，保证重复 connect 不会泄漏旧句柄。
disconnect();
rm_set_log_call_back(apiLogCallback, 2);
rm_set_timeout(config.timeout_ms);
const int init_result = rm_init(RM_TRIPLE_MODE_E);
initialized_ = init_result == 0;
if (!initialized_) {
return common::Result<void>::failure("rm_init failed, error=" + std::to_string(init_result));
}
handle_ = rm_create_robot_arm(config.ip.c_str(), config.port);
if (handle_ == nullptr || handle_->id < 0) {
disconnect();
return common::Result<void>::failure("rm_create_robot_arm failed: " + config.ip + ":" +
std::to_string(config.port));
}
auto info = getRobotInfo();
if (!info.ok) {
disconnect();
return common::Result<void>::failure(info.error_message);
}
dof_ = info.value.arm_dof;
// dof_ 后续用于校验 moveJ 目标长度，避免给 7 轴机械臂传错数组长度。
return common::Result<void>::success();
}
void RealManRobotArm::disconnect() {
// RM_API2 要求先 delete robot arm，再 destroy API。
if (handle_ != nullptr) {
rm_delete_robot_arm(handle_);
handle_ = nullptr;
}
if (initialized_) {
destroyApi();
initialized_ = false;
}
dof_ = 0;
}
common::Result<common::RobotInfo> RealManRobotArm::getRobotInfo() {
// 把 SDK 原始枚举转成 int，避免公共头文件暴露 RM SDK 类型。
auto connected = ensureConnected();
if (!connected.ok) {
return common::Result<common::RobotInfo>::failure(connected.error_message);
}
rm_robot_info_t info = {};
const int result = rm_get_robot_info(handle_, &info);
if (result != 0) {
return common::Result<common::RobotInfo>::failure(
"rm_get_robot_info failed, error=" + std::to_string(result));
}
common::RobotInfo output;
output.arm_dof = static_cast<int>(info.arm_dof);
output.arm_model = static_cast<int>(info.arm_model);
output.force_type = static_cast<int>(info.force_type);
output.controller_version = static_cast<int>(info.robot_controller_version);
return common::Result<common::RobotInfo>::success(output);
}
common::Result<common::ArmState> RealManRobotArm::getCurrentArmState() {
// 当前状态是所有相对运动和安全校验的基础数据。
auto connected = ensureConnected();
if (!connected.ok) {
return common::Result<common::ArmState>::failure(connected.error_message);
}
rm_current_arm_state_t state = {};
const int result = rm_get_current_arm_state(handle_, &state);
if (result != 0) {
return common::Result<common::ArmState>::failure(
"rm_get_current_arm_state failed, error=" + std::to_string(result));
}
const int dof = dof_ > 0 ? dof_ : ARM_DOF;
common::ArmState output;
output.joints_deg.assign(state.joint, state.joint + dof);
output.tcp_pose = convertPose(state.pose);
for (int i = 0; i < static_cast<int>(state.err.err_len) && i < 24; ++i) {
output.error_codes.push_back(state.err.err[i]);
}
return common::Result<common::ArmState>::success(output);
}
common::Result<common::ForceData> RealManRobotArm::getForceData() {
// 力数据当前只读不控制，主要用于状态观察和后续力控模块。
auto connected = ensureConnected();
if (!connected.ok) {
return common::Result<common::ForceData>::failure(connected.error_message);
}
rm_force_data_t data = {};
const int result = rm_get_force_data(handle_, &data);
if (result != 0) {
return common::Result<common::ForceData>::failure(
"rm_get_force_data failed, error=" + std::to_string(result));
}
common::ForceData output;
for (size_t i = 0; i < output.raw.size(); ++i) {
output.raw[i] = data.force_data[i];
output.zero[i] = data.zero_force_data[i];
output.work_zero[i] = data.work_zero_force_data[i];
output.tool_zero[i] = data.tool_zero_force_data[i];
}
return common::Result<common::ForceData>::success(output);
}
common::Result<void> RealManRobotArm::moveJ(const std::vector<float> &joints_deg,
int speed_percent,
int blend_radius,
int trajectory_connect,
bool blocking) {
// IRobotArm 接口使用 vector，但 RM_API2 需要固定长度 C 数组。
// 这里集中做长度校验和数组转换。
auto connected = ensureConnected();
if (!connected.ok) {
return connected;
}
if (static_cast<int>(joints_deg.size()) != dof_) {
return common::Result<void>::failure("moveJ target size must match robot DOF");
}
float target[ARM_DOF] = {};
for (int i = 0; i < dof_; ++i) {
target[i] = joints_deg[static_cast<size_t>(i)];
}
const int result = rm_movej(handle_, target, speed_percent, blend_radius,
trajectory_connect, blocking ? 1 : 0);
if (result != 0) {
return common::Result<void>::failure("rm_movej failed, error=" + std::to_string(result));
}
return common::Result<void>::success();
}
common::Result<void> RealManRobotArm::moveL(const common::Pose &pose,
int speed_percent,
int blend_radius,
int trajectory_connect,
bool blocking) {
// 笛卡尔直线运动保持在驱动层，core 只负责安全限幅和流程编排。
auto connected = ensureConnected();
if (!connected.ok) {
return connected;
}
const rm_pose_t target = toRmPose(pose);
const int result = rm_movel(handle_, target, speed_percent, blend_radius,
trajectory_connect, blocking ? 1 : 0);
if (result != 0) {
return common::Result<void>::failure("rm_movel failed, error=" + std::to_string(result));
}
return common::Result<void>::success();
}
common::Result<void> RealManRobotArm::startDragTeach(const common::DragTeachStartOptions &options) {
auto connected = ensureConnected();
if (!connected.ok) {
return connected;
}
if (options.mode == common::DragTeachMode::OrdinaryRecorded) {
const int record_flag = options.record_vendor_trajectory ? 1 : 0;
const int result = rm_start_drag_teach(handle_, record_flag);
if (result != 0) {
return common::Result<void>::failure(
"rm_start_drag_teach failed, error=" + std::to_string(result));
}
return common::Result<void>::success();
}
// RM75 三代控制器支持复合拖动示教；mode=3 表示六维力位置和姿态同时可拖动。
const int mode = 3;
const int singular_wall = options.enable_singular_wall ? 1 : 0;
const int result = rm_start_multi_drag_teach(handle_, mode, singular_wall);
if (result != 0) {
return common::Result<void>::failure(
"rm_start_multi_drag_teach(mode=3) failed, error=" + std::to_string(result));
}
return common::Result<void>::success();
}
common::Result<void> RealManRobotArm::stopDragTeach() {
auto connected = ensureConnected();
if (!connected.ok) {
return connected;
}
const int result = rm_stop_drag_teach(handle_);
if (result != 0) {
return common::Result<void>::failure("rm_stop_drag_teach failed, error=" + std::to_string(result));
}
return common::Result<void>::success();
}
common::Result<common::SavedVendorTrajectory>
RealManRobotArm::saveDragTeachTrajectory(const std::string &path) {
auto connected = ensureConnected();
if (!connected.ok) {
return common::Result<common::SavedVendorTrajectory>::failure(connected.error_message);
}
if (path.empty()) {
return common::Result<common::SavedVendorTrajectory>::failure("vendor trajectory path must not be empty");
}
if (path.size() > 299) {
return common::Result<common::SavedVendorTrajectory>::failure(
"vendor trajectory path must be shorter than 300 characters for rm_save_trajectory");
}
int point_count = 0;
const int result = rm_save_trajectory(handle_, path.c_str(), &point_count);
if (result != 0) {
return common::Result<common::SavedVendorTrajectory>::failure(
"rm_save_trajectory failed, error=" + std::to_string(result));
}
common::SavedVendorTrajectory output;
output.path = path;
output.point_count = point_count;
return common::Result<common::SavedVendorTrajectory>::success(output);
}
common::Result<void> RealManRobotArm::ensureConnected() const {
// 所有 SDK 调用前统一检查，避免空指针进入 C SDK。
if (!initialized_ || handle_ == nullptr || handle_->id < 0) {
return common::Result<void>::failure("RealMan robot is not connected");
}
return common::Result<void>::success();
}
} // namespace rm_control::drivers::realman
RM/src/main.cpp
View file @ 8275ee2e
/**
* @file main.cpp
* @brief RM JSON 指令运行器入口文件。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件是 `rm_json_runner` 可执行程序入口，只负责命令行参数解析结果处理、
* JSON 文件加载、RealMan 驱动创建、核心 runner 装配和进程退出码返回。
* 具体 JSON 解析、安全校验和硬件调用分别放在 config、core 和 drivers 层。
*
* 设计原则：
* - `main()` 不包含业务控制逻辑；
* - `--validate-only` 不连接 SDK、不访问真实机械臂；
* - 真实硬件动作必须经过 JSON safety 配置和 `JsonCommandRunner` 校验；
* - 所有失败路径返回明确退出码。
*
* @dependencies
* - rm_control/config/json_command.h
* - rm_control/config/json_runner_cli_config.h
* - rm_control/core/json_command_runner.h
* - rm_control/drivers/realman/realman_robot_arm.h
*/
#include "rm_control/config/json_command.h" #include "rm_control/config/json_command.h"
#include "rm_control/config/json_runner_cli_config.h"
#include "rm_control/core/json_command_runner.h" #include "rm_control/core/json_command_runner.h"
#include "rm_control/drivers/realman/realman_robot_arm.h" #include "rm_control/drivers/realman/realman_robot_arm.h"
#include <cstdlib>
#include <iostream> #include <iostream>
#include <string>
namespace {
void printUsage(const char *program_name) {
std::cout << "Usage: " << program_name << " <commands.json> [--validate-only]\n"
<< "\n"
<< "Examples:\n"
<< " " << program_name << " ../config/sample_commands.json --validate-only\n"
<< " " << program_name << " ../config/sample_commands.json\n"
<< "\n"
<< "Safety:\n"
<< " JSON defaults should keep dry_run=true and allow_motion=false first.\n"
<< " Set safety.dry_run=false and safety.allow_motion=true only after checking the target.\n";
}
} // namespace
/**
* @brief 启动 RM JSON 指令运行器。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数完成进程级调度：先解析 CLI 参数，再加载 JSON 指令文件；如果用户请求
* `--validate-only`，函数在 JSON 结构校验成功后直接返回，不创建 RealMan 驱动。
* 非 validate-only 模式下，函数创建 `RealManRobotArm` 并注入 `JsonCommandRunner`，
* 由 core 层执行安全校验和命令调度。
*
* @param[in] argc 命令行参数数量。
* @param[in] argv 命令行参数数组。
*
* @return 0 执行成功或帮助信息输出成功。
* @return 1 CLI 参数错误或 JSON 文件加载失败。
* @return 2 JSON 命令执行失败。
* @return 64 命令行使用方式错误。
*
* @throws 本函数不主动抛出异常；底层模块通过 `Result` 返回错误。
*
* @note
* `--validate-only` 只解析文件，不连接 SDK，因此适合作为新 JSON 文件的第一步验证。
*
* @warning
* 非 validate-only 模式会连接真实 RM75；真实运动仍由 JSON safety 字段控制。
*
* @thread_safety
* 入口函数按单进程主线程使用设计，不提供多线程重入保证。
*
* @par 边界条件
* 参数数量错误、路径为空、JSON 文件不存在或格式错误都会在连接硬件前失败。
*
* @par 后续维护
* 新增启动参数时应修改 CLI 配置模块，不应把解析逻辑写回本函数。
*/
int main(int argc, char **argv) { int main(int argc, char **argv) {
// main 只负责参数解析、加载 JSON、创建具体驱动并启动 runner。 const auto cli_config = rm_control::config::ParseJsonRunnerCliArguments(argc, argv);
// 具体安全校验和硬件调用都放在 core/drivers 中，避免入口文件膨胀。 if (!cli_config.ok) {
if (argc < 2 || argc > 3) { std::cerr << "Invalid arguments: " << cli_config.error_message << '\n';
printUsage(argv[0]); rm_control::config::PrintJsonRunnerUsage(argv == nullptr ? nullptr : argv[0], std::cerr);
return 64; return 64;
} }
if (cli_config.value.help_requested) {
const std::string first_arg = argv[1] == nullptr ? "" : argv[1]; rm_control::config::PrintJsonRunnerUsage(argv == nullptr ? nullptr : argv[0], std::cout);
if (first_arg == "-h" || first_arg == "--help") {
printUsage(argv[0]);
return 0; return 0;
} }
bool validate_only = false; const auto program = rm_control::config::loadCommandProgramFromFile(
if (argc == 3) { cli_config.value.command_file_path);
const std::string option = argv[2] == nullptr ? "" : argv[2];
if (option != "--validate-only") {
std::cerr << "Unknown option: " << option << '\n';
printUsage(argv[0]);
return 64;
}
validate_only = true;
}
auto program = rm_control::config::loadCommandProgramFromFile(first_arg);
if (!program.ok) { if (!program.ok) {
std::cerr << "Failed to load JSON command file: " << program.error_message << '\n'; std::cerr << "Failed to load JSON command file: " << program.error_message << '\n';
return 1; return 1;
...@@ -56,18 +90,14 @@ int main(int argc, char **argv) { ...@@ -56,18 +90,14 @@ int main(int argc, char **argv) {
std::cout << "JSON command file parsed successfully. command_count=" std::cout << "JSON command file parsed successfully. command_count="
<< program.value.commands.size() << '\n'; << program.value.commands.size() << '\n';
if (validate_only) { if (cli_config.value.validate_only) {
// 只校验 JSON 结构，不连接 SDK、不访问真实机械臂。
// 调试新 JSON 时建议先跑这个模式。
std::cout << "validate-only mode: no SDK connection or command execution.\n"; std::cout << "validate-only mode: no SDK connection or command execution.\n";
return 0; return 0;
} }
// 这里选择 RealMan 驱动。以后更换机械臂品牌，只需要替换这个具体实现，
// JsonCommandRunner 和 JSON 格式不需要变。
rm_control::drivers::realman::RealManRobotArm robot; rm_control::drivers::realman::RealManRobotArm robot;
rm_control::core::JsonCommandRunner runner(robot); rm_control::core::JsonCommandRunner runner(robot);
auto result = runner.run(program.value, std::cout); const auto result = runner.run(program.value, std::cout);
if (!result.ok) { if (!result.ok) {
std::cerr << "Command execution failed: " << result.error_message << '\n'; std::cerr << "Command execution failed: " << result.error_message << '\n';
return 2; return 2;
......
RM/src/modules/trajectory/drag_trajectory_validator.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/modules/trajectory/drag_trajectory_validator.h"
#include "rm_control/common/rm75_constants.h"
#include <cmath>
#include <cstddef>
#include <sstream>
#include <string>
#include <vector>
namespace rm_control::modules::trajectory {
namespace {
constexpr double kMillisecondsPerSecond = 1000.0;
common::Result<void> ValidateLimits(const DragTrajectoryValidationLimits &limits) {
const bool jump_is_valid = std::isfinite(limits.maximum_joint_jump_deg) &&
limits.maximum_joint_jump_deg > 0.0;
const bool velocity_is_valid =
std::isfinite(limits.maximum_joint_velocity_deg_per_second) &&
limits.maximum_joint_velocity_deg_per_second > 0.0;
const bool acceleration_is_valid =
std::isfinite(limits.maximum_joint_acceleration_deg_per_second_squared) &&
limits.maximum_joint_acceleration_deg_per_second_squared > 0.0;
if (!jump_is_valid || !velocity_is_valid || !acceleration_is_valid) {
return common::Result<void>::failure(
"trajectory validation limits must be finite positive values");
}
return common::Result<void>::success();
}
bool PoseIsFinite(const common::Pose &pose) {
return std::isfinite(pose.x) && std::isfinite(pose.y) && std::isfinite(pose.z) &&
std::isfinite(pose.rx) && std::isfinite(pose.ry) && std::isfinite(pose.rz);
}
common::Result<void> ValidateSampleShapeAndValues(const common::TrajectorySample &sample,
std::size_t sample_index) {
if (sample.sample_index != sample_index) {
return common::Result<void>::failure(
"trajectory sample_index must be contiguous and start at zero");
}
if (sample.source.empty()) {
return common::Result<void>::failure(
"trajectory sample " + std::to_string(sample_index) +
" must contain a non-empty source");
}
if (sample.joints_deg.size() != common::kRm75JointCount) {
return common::Result<void>::failure(
"trajectory sample " + std::to_string(sample_index) +
" must contain exactly 7 RM75 joints");
}
for (std::size_t joint_index = 0; joint_index < common::kRm75JointCount; ++joint_index) {
if (!std::isfinite(sample.joints_deg[joint_index])) {
return common::Result<void>::failure(
"trajectory sample " + std::to_string(sample_index) + " joint " +
std::to_string(joint_index) + " is not finite");
}
}
if (!PoseIsFinite(sample.tcp_pose)) {
return common::Result<void>::failure(
"trajectory sample " + std::to_string(sample_index) + " TCP pose is not finite");
}
return common::Result<void>::success();
}
common::Result<void> ValidateSamplesBeforeKinematics(
const std::vector<common::TrajectorySample> &samples) {
if (samples.size() < 2U) {
return common::Result<void>::failure(
"trajectory must contain at least two samples");
}
if (samples.front().elapsed_ms < 0) {
return common::Result<void>::failure(
"trajectory first elapsed timestamp must not be negative");
}
for (std::size_t sample_index = 0; sample_index < samples.size(); ++sample_index) {
const auto value_result = ValidateSampleShapeAndValues(samples[sample_index], sample_index);
if (!value_result.ok) {
return value_result;
}
if (sample_index > 0 &&
samples[sample_index].elapsed_ms <= samples[sample_index - 1].elapsed_ms) {
return common::Result<void>::failure(
"trajectory timestamps must be strictly increasing at sample " +
std::to_string(sample_index));
}
}
return common::Result<void>::success();
}
std::string LimitExceededMessage(const std::string &metric_name,
std::size_t sample_index,
std::size_t joint_index,
double observed_value,
double allowed_value) {
std::ostringstream message;
message << metric_name << " limit exceeded at sample " << sample_index << ", joint "
<< joint_index << ": observed " << observed_value << ", allowed " << allowed_value;
return message.str();
}
common::Result<std::vector<double>> MeasureIntervalVelocities(
const common::TrajectorySample &previous,
const common::TrajectorySample &current,
std::size_t current_sample_index,
const DragTrajectoryValidationLimits &limits,
DragTrajectoryValidationReport *report) {
const double interval_seconds =
static_cast<double>(current.elapsed_ms - previous.elapsed_ms) / kMillisecondsPerSecond;
std::vector<double> velocities(common::kRm75JointCount, 0.0);
for (std::size_t joint_index = 0; joint_index < common::kRm75JointCount; ++joint_index) {
const double joint_jump_deg = std::abs(
static_cast<double>(current.joints_deg[joint_index]) -
static_cast<double>(previous.joints_deg[joint_index]));
const double velocity_deg_per_second = joint_jump_deg / interval_seconds;
velocities[joint_index] =
(static_cast<double>(current.joints_deg[joint_index]) -
static_cast<double>(previous.joints_deg[joint_index])) /
interval_seconds;
if (joint_jump_deg > report->observed_maximum_joint_jump_deg) {
report->observed_maximum_joint_jump_deg = joint_jump_deg;
report->maximum_jump_sample_index = current_sample_index;
report->maximum_jump_joint_index = joint_index;
}
if (velocity_deg_per_second > report->observed_maximum_joint_velocity_deg_per_second) {
report->observed_maximum_joint_velocity_deg_per_second = velocity_deg_per_second;
report->maximum_velocity_sample_index = current_sample_index;
report->maximum_velocity_joint_index = joint_index;
}
if (joint_jump_deg > limits.maximum_joint_jump_deg) {
return common::Result<std::vector<double>>::failure(LimitExceededMessage(
"joint jump", current_sample_index, joint_index, joint_jump_deg,
limits.maximum_joint_jump_deg));
}
if (velocity_deg_per_second > limits.maximum_joint_velocity_deg_per_second) {
return common::Result<std::vector<double>>::failure(LimitExceededMessage(
"joint velocity", current_sample_index, joint_index, velocity_deg_per_second,
limits.maximum_joint_velocity_deg_per_second));
}
}
return common::Result<std::vector<double>>::success(velocities);
}
common::Result<void> MeasureIntervalAcceleration(
const std::vector<double> &previous_velocities,
const std::vector<double> &current_velocities,
double previous_interval_seconds,
double current_interval_seconds,
std::size_t current_sample_index,
const DragTrajectoryValidationLimits &limits,
DragTrajectoryValidationReport *report) {
const double interval_midpoint_delta_seconds =
(previous_interval_seconds + current_interval_seconds) / 2.0;
for (std::size_t joint_index = 0; joint_index < common::kRm75JointCount; ++joint_index) {
const double acceleration_deg_per_second_squared =
std::abs(current_velocities[joint_index] - previous_velocities[joint_index]) /
interval_midpoint_delta_seconds;
if (acceleration_deg_per_second_squared >
report->observed_maximum_joint_acceleration_deg_per_second_squared) {
report->observed_maximum_joint_acceleration_deg_per_second_squared =
acceleration_deg_per_second_squared;
report->maximum_acceleration_sample_index = current_sample_index;
report->maximum_acceleration_joint_index = joint_index;
}
if (acceleration_deg_per_second_squared >
limits.maximum_joint_acceleration_deg_per_second_squared) {
return common::Result<void>::failure(LimitExceededMessage(
"joint acceleration", current_sample_index, joint_index,
acceleration_deg_per_second_squared,
limits.maximum_joint_acceleration_deg_per_second_squared));
}
}
return common::Result<void>::success();
}
} // namespace
common::Result<DragTrajectoryValidationReport> ValidateDragTrajectory(
const std::vector<common::TrajectorySample> &samples,
const DragTrajectoryValidationLimits &limits) {
const auto limits_result = ValidateLimits(limits);
if (!limits_result.ok) {
return common::Result<DragTrajectoryValidationReport>::failure(limits_result.error_message);
}
const auto samples_result = ValidateSamplesBeforeKinematics(samples);
if (!samples_result.ok) {
return common::Result<DragTrajectoryValidationReport>::failure(samples_result.error_message);
}
DragTrajectoryValidationReport report;
report.sample_count = samples.size();
report.duration_ms = samples.back().elapsed_ms - samples.front().elapsed_ms;
std::vector<double> previous_velocities;
double previous_interval_seconds = 0.0;
for (std::size_t sample_index = 1; sample_index < samples.size(); ++sample_index) {
const double current_interval_seconds =
static_cast<double>(samples[sample_index].elapsed_ms -
samples[sample_index - 1].elapsed_ms) /
kMillisecondsPerSecond;
const auto velocities_result = MeasureIntervalVelocities(
samples[sample_index - 1], samples[sample_index], sample_index, limits, &report);
if (!velocities_result.ok) {
return common::Result<DragTrajectoryValidationReport>::failure(
velocities_result.error_message);
}
if (!previous_velocities.empty()) {
const auto acceleration_result = MeasureIntervalAcceleration(
previous_velocities, velocities_result.value, previous_interval_seconds,
current_interval_seconds, sample_index, limits, &report);
if (!acceleration_result.ok) {
return common::Result<DragTrajectoryValidationReport>::failure(
acceleration_result.error_message);
}
}
previous_velocities = velocities_result.value;
previous_interval_seconds = current_interval_seconds;
}
return common::Result<DragTrajectoryValidationReport>::success(report);
}
} // namespace rm_control::modules::trajectory
RM/src/modules/trajectory/raw_trajectory_store.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file raw_trajectory_store.cpp
* @brief RM75 厂商原始拖动示教轨迹的不可覆盖存储和 manifest 固化。
*
* @author 唐永康
* @date 2026-07-10
*/
#include "rm_control/modules/trajectory/raw_trajectory_store.h"
#include "rm_control/modules/trajectory/sha256_file_checksum.h"
#include <algorithm>
#include <chrono>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <iterator>
#include <sstream>
#include <system_error>
namespace rm_control::modules::trajectory {
namespace {
constexpr std::size_t kMaximumSanitizedSessionNameLength = 64U;
constexpr int kMaximumUniqueDirectoryAttempts = 1000;
constexpr int kCollisionSuffixWidth = 3;
constexpr const char *kSessionsDirectoryName = "sessions";
constexpr const char *kRawTrajectoryFileName = "raw_trajectory.txt";
constexpr const char *kSynchronizedSamplesFileName = "synchronized_samples.csv";
constexpr const char *kManifestFileName = "manifest.json";
constexpr const char *kManifestTemporaryFileName = "manifest.json.tmp";
struct UtcTimestamp {
std::string compact_with_milliseconds;
std::string iso8601_with_milliseconds;
};
std::string PathForError(const std::filesystem::path &path) {
return path.string();
}
bool ConvertToUtc(std::time_t time, std::tm *utc) {
if (utc == nullptr) {
return false;
}
#if defined(_WIN32)
return ::gmtime_s(utc, &time) == 0;
#else
return ::gmtime_r(&time, utc) != nullptr;
#endif
}
common::Result<UtcTimestamp> CurrentUtcTimestamp() {
const auto now = std::chrono::system_clock::now();
const auto milliseconds_since_epoch =
std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
const auto whole_seconds =
std::chrono::duration_cast<std::chrono::seconds>(milliseconds_since_epoch);
const auto fractional_milliseconds = milliseconds_since_epoch - whole_seconds;
const std::time_t time = std::chrono::system_clock::to_time_t(now);
std::tm utc{};
if (!ConvertToUtc(time, &utc)) {
return common::Result<UtcTimestamp>::failure("failed to convert current time to UTC");
}
std::ostringstream compact;
compact << std::put_time(&utc, "%Y%m%dT%H%M%S") << '_'
<< std::setw(3) << std::setfill('0') << fractional_milliseconds.count() << 'Z';
std::ostringstream iso8601;
iso8601 << std::put_time(&utc, "%Y-%m-%dT%H:%M:%S") << '.'
<< std::setw(3) << std::setfill('0') << fractional_milliseconds.count() << 'Z';
return common::Result<UtcTimestamp>::success(
UtcTimestamp{compact.str(), iso8601.str()});
}
char ToUpperAscii(char value) {
if (value >= 'a' && value <= 'z') {
return static_cast<char>(value - 'a' + 'A');
}
return value;
}
bool IsWindowsReservedFileName(const std::string &name) {
std::string uppercase;
uppercase.reserve(name.size());
std::transform(name.begin(), name.end(), std::back_inserter(uppercase), ToUpperAscii);
if (uppercase == "CON" || uppercase == "PRN" || uppercase == "AUX" ||
uppercase == "NUL") {
return true;
}
if (uppercase.size() != 4U) {
return false;
}
const bool has_reserved_prefix = uppercase.compare(0, 3, "COM") == 0 ||
uppercase.compare(0, 3, "LPT") == 0;
return has_reserved_prefix && uppercase[3] >= '1' && uppercase[3] <= '9';
}
void RemoveLeadingAndTrailingUnderscores(std::string *value) {
if (value == nullptr) {
return;
}
const auto first_character = value->find_first_not_of('_');
if (first_character == std::string::npos) {
value->clear();
return;
}
const auto last_character = value->find_last_not_of('_');
*value = value->substr(first_character, last_character - first_character + 1U);
}
std::string FormatCollisionSuffix(int attempt) {
if (attempt == 0) {
return "";
}
std::ostringstream suffix;
suffix << '_' << std::setw(kCollisionSuffixWidth) << std::setfill('0') << attempt;
return suffix.str();
}
RawTrajectorySessionPaths MakeSessionPaths(const std::filesystem::path &directory,
const std::string &session_id,
const std::string &saved_at_utc) {
RawTrajectorySessionPaths paths;
paths.directory = directory;
paths.raw_trajectory = directory / kRawTrajectoryFileName;
paths.synchronized_csv = directory / kSynchronizedSamplesFileName;
paths.manifest = directory / kManifestFileName;
paths.session_id = session_id;
paths.saved_at_utc = saved_at_utc;
return paths;
}
common::Result<void> ValidateSessionPaths(const RawTrajectorySessionPaths &paths) {
if (paths.directory.empty() || paths.session_id.empty() || paths.saved_at_utc.empty()) {
return common::Result<void>::failure(
"raw trajectory session directory, session ID, and saved UTC time must not be empty");
}
const std::filesystem::path normalized_directory = paths.directory.lexically_normal();
if (normalized_directory.filename() != paths.session_id ||
paths.raw_trajectory.lexically_normal() != normalized_directory / kRawTrajectoryFileName ||
paths.synchronized_csv.lexically_normal() !=
normalized_directory / kSynchronizedSamplesFileName ||
paths.manifest.lexically_normal() != normalized_directory / kManifestFileName) {
return common::Result<void>::failure(
"raw trajectory artifact paths do not match the controlled session layout: " +
PathForError(paths.directory));
}
std::error_code filesystem_error;
const std::filesystem::file_status directory_status =
std::filesystem::symlink_status(normalized_directory, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to inspect raw trajectory session directory " +
PathForError(normalized_directory) + ": " + filesystem_error.message());
}
if (!std::filesystem::is_directory(directory_status)) {
return common::Result<void>::failure(
"raw trajectory session path is not a real directory: " +
PathForError(normalized_directory));
}
return common::Result<void>::success();
}
std::string EscapeJsonString(const std::string &value) {
std::ostringstream escaped;
escaped << std::hex << std::setfill('0');
for (const unsigned char character : value) {
switch (character) {
case '"':
escaped << "\\\"";
break;
case '\\':
escaped << "\\\\";
break;
case '\b':
escaped << "\\b";
break;
case '\f':
escaped << "\\f";
break;
case '\n':
escaped << "\\n";
break;
case '\r':
escaped << "\\r";
break;
case '\t':
escaped << "\\t";
break;
default:
if (character < 0x20U) {
escaped << "\\u" << std::setw(4) << static_cast<unsigned int>(character);
} else {
escaped << static_cast<char>(character);
}
break;
}
}
return escaped.str();
}
std::string BuildManifestJson(const RawTrajectoryArtifact &artifact) {
std::ostringstream manifest;
manifest << "{\n"
<< " \"schema_version\": 1,\n"
<< " \"session_id\": \"" << EscapeJsonString(artifact.paths.session_id) << "\",\n"
<< " \"robot_model\": \"" << EscapeJsonString(artifact.metadata.robot_model) << "\",\n"
<< " \"sdk_version\": \"" << EscapeJsonString(artifact.metadata.sdk_version) << "\",\n"
<< " \"firmware_version\": \""
<< EscapeJsonString(artifact.metadata.firmware_version) << "\",\n"
<< " \"saved_at_utc\": \"" << EscapeJsonString(artifact.paths.saved_at_utc) << "\",\n"
<< " \"manifest_written_at_utc\": \""
<< EscapeJsonString(artifact.manifest_written_at_utc) << "\",\n"
<< " \"trajectory_point_count\": "
<< artifact.metadata.trajectory_point_count << ",\n"
<< " \"recorded_sample_count\": "
<< artifact.metadata.recorded_sample_count << ",\n"
<< " \"synchronized_timestamp_unit\": \"millisecond\",\n"
<< " \"synchronized_monotonic_clock\": \"std::chrono::steady_clock\",\n"
<< " \"synchronized_timestamp_is_derived\": false,\n"
<< " \"vendor_raw_timestamp_semantics\": \"vendor_defined_unmodified\",\n"
<< " \"raw_trajectory_file\": \"" << kRawTrajectoryFileName << "\",\n"
<< " \"synchronized_samples_file\": \""
<< kSynchronizedSamplesFileName << "\",\n"
<< " \"synchronized_csv_write_succeeded\": "
<< (artifact.metadata.synchronized_csv_write_succeeded ? "true" : "false")
<< ",\n"
<< " \"raw_file_present\": "
<< (artifact.raw_file_present ? "true" : "false") << ",\n"
<< " \"raw_file_size_bytes\": " << artifact.raw_file_size_bytes << ",\n"
<< " \"sha256\": \"" << artifact.raw_file_sha256 << "\",\n"
<< " \"save_interface_called\": "
<< (artifact.metadata.vendor_save_api_called ? "true" : "false") << ",\n"
<< " \"save_interface_return_code\": "
<< artifact.metadata.vendor_save_return_code << ",\n"
<< " \"save_interface_error\": \""
<< EscapeJsonString(artifact.metadata.vendor_save_error_message) << "\"\n"
<< "}\n";
return manifest.str();
}
common::Result<void> WriteManifestAtomically(const RawTrajectoryArtifact &artifact) {
std::error_code filesystem_error;
if (std::filesystem::exists(artifact.paths.manifest, filesystem_error)) {
return common::Result<void>::failure(
"refusing to overwrite existing raw trajectory manifest: " +
PathForError(artifact.paths.manifest));
}
if (filesystem_error) {
return common::Result<void>::failure(
"failed to inspect raw trajectory manifest " +
PathForError(artifact.paths.manifest) + ": " + filesystem_error.message());
}
const std::filesystem::path temporary_manifest =
artifact.paths.directory / kManifestTemporaryFileName;
if (std::filesystem::exists(temporary_manifest, filesystem_error)) {
return common::Result<void>::failure(
"refusing to overwrite existing temporary manifest: " +
PathForError(temporary_manifest));
}
if (filesystem_error) {
return common::Result<void>::failure(
"failed to inspect temporary manifest " + PathForError(temporary_manifest) + ": " +
filesystem_error.message());
}
std::ofstream output(temporary_manifest, std::ios::out | std::ios::trunc);
if (!output) {
return common::Result<void>::failure(
"failed to create temporary manifest: " + PathForError(temporary_manifest));
}
output << BuildManifestJson(artifact);
output.flush();
if (!output) {
return common::Result<void>::failure(
"failed to write temporary manifest: " + PathForError(temporary_manifest));
}
output.close();
if (!output) {
return common::Result<void>::failure(
"failed to close temporary manifest: " + PathForError(temporary_manifest));
}
std::filesystem::create_hard_link(
temporary_manifest, artifact.paths.manifest, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to atomically publish manifest " + PathForError(artifact.paths.manifest) +
": " + filesystem_error.message());
}
std::filesystem::remove(temporary_manifest, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"manifest was published but its temporary hard link could not be removed " +
PathForError(temporary_manifest) + ": " + filesystem_error.message());
}
return common::Result<void>::success();
}
common::Result<void> InspectAndProtectRawFile(RawTrajectoryArtifact *artifact) {
if (artifact == nullptr) {
return common::Result<void>::failure("raw trajectory artifact output must not be null");
}
std::error_code filesystem_error;
const bool raw_file_exists =
std::filesystem::exists(artifact->paths.raw_trajectory, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to inspect raw trajectory file " +
PathForError(artifact->paths.raw_trajectory) + ": " + filesystem_error.message());
}
if (!raw_file_exists) {
return common::Result<void>::success();
}
const std::filesystem::file_status raw_status =
std::filesystem::symlink_status(artifact->paths.raw_trajectory, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to inspect raw trajectory file type " +
PathForError(artifact->paths.raw_trajectory) + ": " + filesystem_error.message());
}
if (!std::filesystem::is_regular_file(raw_status)) {
return common::Result<void>::failure(
"raw trajectory artifact is not a regular file: " +
PathForError(artifact->paths.raw_trajectory));
}
artifact->raw_file_present = true;
artifact->raw_file_size_bytes =
std::filesystem::file_size(artifact->paths.raw_trajectory, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to read raw trajectory file size " +
PathForError(artifact->paths.raw_trajectory) + ": " + filesystem_error.message());
}
constexpr std::filesystem::perms kAllWritePermissions =
std::filesystem::perms::owner_write | std::filesystem::perms::group_write |
std::filesystem::perms::others_write;
std::filesystem::permissions(artifact->paths.raw_trajectory, kAllWritePermissions,
std::filesystem::perm_options::remove, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to make raw trajectory read-only " +
PathForError(artifact->paths.raw_trajectory) + ": " + filesystem_error.message());
}
const auto modification_time_before_checksum =
std::filesystem::last_write_time(artifact->paths.raw_trajectory, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to inspect raw trajectory modification time " +
PathForError(artifact->paths.raw_trajectory) + ": " + filesystem_error.message());
}
auto checksum_result = CalculateFileSha256(artifact->paths.raw_trajectory);
if (!checksum_result.ok) {
return common::Result<void>::failure(checksum_result.error_message);
}
artifact->raw_file_sha256 = checksum_result.value;
const std::uintmax_t size_after_checksum =
std::filesystem::file_size(artifact->paths.raw_trajectory, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to verify raw trajectory file size " +
PathForError(artifact->paths.raw_trajectory) + ": " + filesystem_error.message());
}
if (size_after_checksum != artifact->raw_file_size_bytes) {
return common::Result<void>::failure(
"raw trajectory file changed while SHA-256 was being calculated: " +
PathForError(artifact->paths.raw_trajectory));
}
const auto modification_time_after_checksum =
std::filesystem::last_write_time(artifact->paths.raw_trajectory, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to verify raw trajectory modification time " +
PathForError(artifact->paths.raw_trajectory) + ": " + filesystem_error.message());
}
if (modification_time_after_checksum != modification_time_before_checksum) {
return common::Result<void>::failure(
"raw trajectory modification time changed during SHA-256 calculation: " +
PathForError(artifact->paths.raw_trajectory));
}
return common::Result<void>::success();
}
common::Result<void> RemoveWritePermissions(const std::filesystem::path &path) {
constexpr std::filesystem::perms kAllWritePermissions =
std::filesystem::perms::owner_write | std::filesystem::perms::group_write |
std::filesystem::perms::others_write;
std::error_code filesystem_error;
std::filesystem::permissions(path, kAllWritePermissions,
std::filesystem::perm_options::remove, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to remove write permissions from " + PathForError(path) + ": " +
filesystem_error.message());
}
return common::Result<void>::success();
}
common::Result<void> ProtectCompletedSession(
const RawTrajectorySessionPaths &paths) {
std::error_code filesystem_error;
const bool synchronized_csv_exists =
std::filesystem::exists(paths.synchronized_csv, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to inspect synchronized trajectory CSV permissions: " +
filesystem_error.message());
}
if (synchronized_csv_exists) {
const auto csv_result = RemoveWritePermissions(paths.synchronized_csv);
if (!csv_result.ok) {
return csv_result;
}
}
const auto manifest_result = RemoveWritePermissions(paths.manifest);
if (!manifest_result.ok) {
return manifest_result;
}
return RemoveWritePermissions(paths.directory);
}
} // namespace
std::string SanitizeRawTrajectorySessionName(const std::string &requested_session_name) {
std::string sanitized;
sanitized.reserve(std::min(requested_session_name.size(),
kMaximumSanitizedSessionNameLength));
bool previous_character_was_invalid = false;
for (const unsigned char character : requested_session_name) {
const bool is_ascii_letter = (character >= 'A' && character <= 'Z') ||
(character >= 'a' && character <= 'z');
const bool is_ascii_digit = character >= '0' && character <= '9';
const bool is_allowed = is_ascii_letter || is_ascii_digit || character == '_' ||
character == '-';
if (is_allowed) {
sanitized.push_back(static_cast<char>(character));
previous_character_was_invalid = false;
} else if (!sanitized.empty() && !previous_character_was_invalid) {
sanitized.push_back('_');
previous_character_was_invalid = true;
}
if (sanitized.size() >= kMaximumSanitizedSessionNameLength) {
break;
}
}
RemoveLeadingAndTrailingUnderscores(&sanitized);
if (sanitized.empty()) {
sanitized = "trajectory";
}
if (IsWindowsReservedFileName(sanitized)) {
sanitized = "trajectory_" + sanitized;
}
if (sanitized.size() > kMaximumSanitizedSessionNameLength) {
sanitized.resize(kMaximumSanitizedSessionNameLength);
RemoveLeadingAndTrailingUnderscores(&sanitized);
}
return sanitized;
}
common::Result<RawTrajectorySessionPaths>
CreateUniqueRawTrajectorySession(const std::filesystem::path &storage_root,
const std::string &requested_session_name) {
if (storage_root.empty()) {
return common::Result<RawTrajectorySessionPaths>::failure(
"raw trajectory storage root must not be empty");
}
const std::filesystem::path sessions_root = storage_root / kSessionsDirectoryName;
std::error_code filesystem_error;
std::filesystem::create_directories(sessions_root, filesystem_error);
if (filesystem_error) {
return common::Result<RawTrajectorySessionPaths>::failure(
"failed to create raw trajectory sessions root " + PathForError(sessions_root) +
": " + filesystem_error.message());
}
const std::filesystem::file_status sessions_status =
std::filesystem::symlink_status(sessions_root, filesystem_error);
if (filesystem_error || !std::filesystem::is_directory(sessions_status)) {
const std::string detail = filesystem_error ? filesystem_error.message() :
"path is not a real directory";
return common::Result<RawTrajectorySessionPaths>::failure(
"invalid raw trajectory sessions root " + PathForError(sessions_root) + ": " +
detail);
}
auto timestamp_result = CurrentUtcTimestamp();
if (!timestamp_result.ok) {
return common::Result<RawTrajectorySessionPaths>::failure(timestamp_result.error_message);
}
const std::string sanitized_name =
SanitizeRawTrajectorySessionName(requested_session_name);
const std::string base_session_id =
sanitized_name + "_" + timestamp_result.value.compact_with_milliseconds;
for (int attempt = 0; attempt < kMaximumUniqueDirectoryAttempts; ++attempt) {
const std::string session_id = base_session_id + FormatCollisionSuffix(attempt);
const std::filesystem::path session_directory = sessions_root / session_id;
filesystem_error.clear();
const bool directory_created =
std::filesystem::create_directory(session_directory, filesystem_error);
if (directory_created) {
return common::Result<RawTrajectorySessionPaths>::success(
MakeSessionPaths(session_directory, session_id,
timestamp_result.value.iso8601_with_milliseconds));
}
if (filesystem_error) {
return common::Result<RawTrajectorySessionPaths>::failure(
"failed to create unique raw trajectory session " +
PathForError(session_directory) + ": " + filesystem_error.message());
}
}
return common::Result<RawTrajectorySessionPaths>::failure(
"failed to reserve a unique raw trajectory session after " +
std::to_string(kMaximumUniqueDirectoryAttempts) + " attempts under " +
PathForError(sessions_root));
}
common::Result<RawTrajectoryArtifact>
FinalizeRawTrajectorySession(const RawTrajectorySessionPaths &paths,
const RawTrajectoryManifestMetadata &metadata) {
auto validation_result = ValidateSessionPaths(paths);
if (!validation_result.ok) {
return common::Result<RawTrajectoryArtifact>::failure(validation_result.error_message);
}
RawTrajectoryArtifact artifact;
artifact.paths = paths;
artifact.metadata = metadata;
auto timestamp_result = CurrentUtcTimestamp();
if (!timestamp_result.ok) {
return common::Result<RawTrajectoryArtifact>::failure(timestamp_result.error_message);
}
artifact.manifest_written_at_utc = timestamp_result.value.iso8601_with_milliseconds;
auto raw_file_result = InspectAndProtectRawFile(&artifact);
if (!raw_file_result.ok) {
return common::Result<RawTrajectoryArtifact>::failure(raw_file_result.error_message);
}
auto manifest_result = WriteManifestAtomically(artifact);
if (!manifest_result.ok) {
return common::Result<RawTrajectoryArtifact>::failure(manifest_result.error_message);
}
const auto protection_result = ProtectCompletedSession(paths);
if (!protection_result.ok) {
return common::Result<RawTrajectoryArtifact>::failure(
protection_result.error_message);
}
return common::Result<RawTrajectoryArtifact>::success(artifact);
}
} // namespace rm_control::modules::trajectory
RM/src/modules/trajectory/reversed_vendor_trajectory_store.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file reversed_vendor_trajectory_store.cpp
* @brief RM75 厂商 TXT 轨迹的只读反向派生文件实现。
*
* @author 唐永康
* @date 2026-07-11
*/
#include "rm_control/modules/trajectory/reversed_vendor_trajectory_store.h"
#include "rm_control/modules/trajectory/sha256_file_checksum.h"
#include <algorithm>
#include <chrono>
#include <fstream>
#include <string>
#include <system_error>
#include <utility>
#include <vector>
#ifndef RM_RUNTIME_DATA_ROOT
#define RM_RUNTIME_DATA_ROOT "build/runtime_data"
#endif
namespace rm_control::modules::trajectory {
namespace {
constexpr int kMaximumUniqueDirectoryAttempts = 1000;
constexpr std::size_t kChecksumNamePrefixLength = 12U;
constexpr const char *kReversedTrajectoryFileName = "reversed_trajectory.txt";
struct PhysicalTextLines {
std::vector<std::string> lines;
std::string newline_sequence = "\n";
bool has_trailing_newline = false;
};
struct ReversedArtifactPaths {
std::filesystem::path directory;
std::filesystem::path trajectory_file;
};
class CreatedArtifactCleanup final {
public:
explicit CreatedArtifactCleanup(ReversedArtifactPaths paths) : paths_(std::move(paths)) {}
~CreatedArtifactCleanup() {
if (released_) {
return;
}
std::error_code ignored_error;
std::filesystem::remove(paths_.trajectory_file, ignored_error);
ignored_error.clear();
std::filesystem::remove(paths_.directory, ignored_error);
}
void Release() noexcept {
released_ = true;
}
private:
ReversedArtifactPaths paths_;
bool released_ = false;
};
std::string PathForError(const std::filesystem::path &path) {
return path.string();
}
common::Result<void> ValidateForwardMetadata(
const VendorDragTrajectoryParseResult &trajectory) {
if (trajectory.source_file.empty() || trajectory.samples.empty() ||
trajectory.trajectory_point_count != trajectory.samples.size()) {
return common::Result<void>::failure(
"forward vendor trajectory path, samples, and point count are inconsistent");
}
if (trajectory.source_file_size_bytes == 0U || trajectory.source_file_sha256.empty() ||
trajectory.sample_period_ms <= 0 || trajectory.raw_units_per_degree <= 0 ||
!trajectory.timestamp_is_derived) {
return common::Result<void>::failure(
"forward vendor trajectory is missing size, SHA-256, units, or derived timing");
}
return common::Result<void>::success();
}
common::Result<void> InspectReadOnlyRegularFile(
const std::filesystem::path &path,
std::uintmax_t expected_size) {
std::error_code filesystem_error;
const auto status = std::filesystem::symlink_status(path, filesystem_error);
if (filesystem_error || !std::filesystem::is_regular_file(status)) {
return common::Result<void>::failure(
filesystem_error ? "failed to inspect trajectory file " + PathForError(path) +
": " + filesystem_error.message()
: "trajectory file must be a non-symlink regular file: " +
PathForError(path));
}
const auto write_permissions = std::filesystem::perms::owner_write |
std::filesystem::perms::group_write | std::filesystem::perms::others_write;
if ((status.permissions() & write_permissions) != std::filesystem::perms::none) {
return common::Result<void>::failure(
"trajectory file must be read-only for owner, group, and others: " +
PathForError(path));
}
const std::uintmax_t actual_size = std::filesystem::file_size(path, filesystem_error);
if (filesystem_error || actual_size != expected_size) {
return common::Result<void>::failure(
filesystem_error ? "failed to read trajectory file size " + PathForError(path) +
": " + filesystem_error.message()
: "trajectory file size differs from parsed metadata: " +
PathForError(path));
}
return common::Result<void>::success();
}
common::Result<void> VerifyFileIntegrity(
const VendorDragTrajectoryParseResult &trajectory) {
const auto before = InspectReadOnlyRegularFile(
trajectory.source_file, trajectory.source_file_size_bytes);
if (!before.ok) {
return before;
}
const auto checksum = CalculateFileSha256(trajectory.source_file);
if (!checksum.ok) {
return common::Result<void>::failure(checksum.error_message);
}
const auto after = InspectReadOnlyRegularFile(
trajectory.source_file, trajectory.source_file_size_bytes);
if (!after.ok) {
return after;
}
if (checksum.value != trajectory.source_file_sha256) {
return common::Result<void>::failure(
"trajectory file SHA-256 differs from parsed metadata: " +
PathForError(trajectory.source_file));
}
return common::Result<void>::success();
}
common::Result<bool> FileHasTrailingLineFeed(const std::filesystem::path &path) {
std::ifstream input(path, std::ios::binary);
if (!input) {
return common::Result<bool>::failure(
"failed to open trajectory while checking its final newline: " +
PathForError(path));
}
input.seekg(-1, std::ios::end);
char final_byte = '\0';
input.get(final_byte);
if (!input) {
return common::Result<bool>::failure(
"failed to read trajectory final byte: " + PathForError(path));
}
return common::Result<bool>::success(final_byte == '\n');
}
common::Result<void> NormalizeAndValidateNewlineStyle(PhysicalTextLines *text) {
if (text == nullptr || text->lines.empty()) {
return common::Result<void>::failure("trajectory physical lines must not be empty");
}
bool newline_style_selected = false;
bool uses_carriage_return = false;
for (std::size_t index = 0U; index < text->lines.size(); ++index) {
const bool line_had_terminator = index + 1U < text->lines.size() ||
text->has_trailing_newline;
if (!line_had_terminator) {
continue;
}
const bool has_carriage_return = !text->lines[index].empty() &&
text->lines[index].back() == '\r';
if (newline_style_selected && uses_carriage_return != has_carriage_return) {
return common::Result<void>::failure(
"vendor trajectory uses mixed LF and CRLF physical line endings");
}
newline_style_selected = true;
uses_carriage_return = has_carriage_return;
if (has_carriage_return) {
text->lines[index].pop_back();
}
}
text->newline_sequence = uses_carriage_return ? "\r\n" : "\n";
return common::Result<void>::success();
}
common::Result<PhysicalTextLines> ReadPhysicalTextLines(
const std::filesystem::path &path) {
const auto trailing_line_feed = FileHasTrailingLineFeed(path);
if (!trailing_line_feed.ok) {
return common::Result<PhysicalTextLines>::failure(trailing_line_feed.error_message);
}
std::ifstream input(path, std::ios::binary);
if (!input) {
return common::Result<PhysicalTextLines>::failure(
"failed to open trajectory for physical-line reading: " + PathForError(path));
}
PhysicalTextLines text;
text.has_trailing_newline = trailing_line_feed.value;
std::string line;
while (std::getline(input, line)) {
text.lines.push_back(line);
}
if (!input.eof()) {
return common::Result<PhysicalTextLines>::failure(
"failed while reading trajectory physical lines: " + PathForError(path));
}
const auto newline = NormalizeAndValidateNewlineStyle(&text);
return newline.ok ? common::Result<PhysicalTextLines>::success(text)
: common::Result<PhysicalTextLines>::failure(newline.error_message);
}
common::Result<void> ValidateOutputDirectory(const std::filesystem::path &output_directory) {
if (output_directory.empty()) {
return common::Result<void>::failure(
"reversed vendor trajectory output directory must not be empty");
}
std::error_code filesystem_error;
std::filesystem::create_directories(output_directory, filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to create reversed trajectory output directory " +
PathForError(output_directory) + ": " + filesystem_error.message());
}
const auto status = std::filesystem::symlink_status(output_directory, filesystem_error);
if (filesystem_error || !std::filesystem::is_directory(status)) {
return common::Result<void>::failure(
"reversed trajectory output path must be a non-symlink directory: " +
PathForError(output_directory));
}
return common::Result<void>::success();
}
common::Result<ReversedArtifactPaths> CreateUniqueArtifactDirectory(
const std::filesystem::path &output_directory,
const std::string &source_sha256) {
const auto directory_check = ValidateOutputDirectory(output_directory);
if (!directory_check.ok) {
return common::Result<ReversedArtifactPaths>::failure(
directory_check.error_message);
}
const auto timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::system_clock::now().time_since_epoch()).count();
const std::string prefix = "reverse_" +
source_sha256.substr(0U, std::min(kChecksumNamePrefixLength, source_sha256.size())) +
"_" + std::to_string(timestamp);
for (int attempt = 0; attempt < kMaximumUniqueDirectoryAttempts; ++attempt) {
const std::string suffix = attempt == 0 ? "" : "_" + std::to_string(attempt);
const std::filesystem::path directory = output_directory / (prefix + suffix);
std::error_code filesystem_error;
if (std::filesystem::create_directory(directory, filesystem_error)) {
return common::Result<ReversedArtifactPaths>::success(
{directory, directory / kReversedTrajectoryFileName});
}
if (filesystem_error && filesystem_error != std::errc::file_exists) {
return common::Result<ReversedArtifactPaths>::failure(
"failed to create unique reversed trajectory directory " +
PathForError(directory) + ": " + filesystem_error.message());
}
}
return common::Result<ReversedArtifactPaths>::failure(
"failed to allocate a unique reversed trajectory directory after " +
std::to_string(kMaximumUniqueDirectoryAttempts) + " attempts");
}
common::Result<void> WriteReversedPhysicalLinesWithoutOverwrite(
const PhysicalTextLines &forward_text,
const std::filesystem::path &output_path) {
std::error_code filesystem_error;
if (std::filesystem::exists(output_path, filesystem_error) || filesystem_error) {
return common::Result<void>::failure(
filesystem_error ? "failed to inspect reversed trajectory target " +
PathForError(output_path) + ": " + filesystem_error.message()
: "refusing to overwrite reversed trajectory target: " +
PathForError(output_path));
}
std::ofstream output(output_path, std::ios::binary | std::ios::out);
if (!output) {
return common::Result<void>::failure(
"failed to create reversed trajectory file: " + PathForError(output_path));
}
for (std::size_t output_index = 0U; output_index < forward_text.lines.size(); ++output_index) {
const auto &line = forward_text.lines[forward_text.lines.size() - 1U - output_index];
output.write(line.data(), static_cast<std::streamsize>(line.size()));
if (output_index + 1U < forward_text.lines.size() || forward_text.has_trailing_newline) {
output.write(forward_text.newline_sequence.data(),
static_cast<std::streamsize>(forward_text.newline_sequence.size()));
}
}
output.flush();
output.close();
if (!output) {
return common::Result<void>::failure(
"failed to finish reversed trajectory file: " + PathForError(output_path));
}
return common::Result<void>::success();
}
common::Result<void> MakeFileReadOnly(const std::filesystem::path &path) {
std::error_code filesystem_error;
const auto read_only_permissions = std::filesystem::perms::owner_read |
std::filesystem::perms::group_read | std::filesystem::perms::others_read;
std::filesystem::permissions(
path, read_only_permissions, std::filesystem::perm_options::replace, filesystem_error);
return filesystem_error
? common::Result<void>::failure(
"failed to make reversed trajectory read-only " + PathForError(path) + ": " +
filesystem_error.message())
: common::Result<void>::success();
}
common::Result<void> ValidateExactPhysicalLineReversal(
const PhysicalTextLines &forward_text,
const PhysicalTextLines &reversed_text) {
if (forward_text.newline_sequence != reversed_text.newline_sequence ||
forward_text.has_trailing_newline != reversed_text.has_trailing_newline ||
forward_text.lines.size() != reversed_text.lines.size()) {
return common::Result<void>::failure(
"reversed trajectory physical-line count or newline format differs from source");
}
for (std::size_t index = 0U; index < reversed_text.lines.size(); ++index) {
if (reversed_text.lines[index] !=
forward_text.lines[forward_text.lines.size() - 1U - index]) {
return common::Result<void>::failure(
"reversed trajectory line " + std::to_string(index + 1U) +
" does not exactly match the corresponding source line");
}
}
return common::Result<void>::success();
}
bool PoseExactlyEqual(const common::Pose &left, const common::Pose &right) {
return left.x == right.x && left.y == right.y && left.z == right.z &&
left.rx == right.rx && left.ry == right.ry && left.rz == right.rz;
}
common::Result<void> ValidateReversedSamples(
const VendorDragTrajectoryParseResult &forward,
const VendorDragTrajectoryParseResult &reversed) {
if (reversed.samples.size() != forward.samples.size() ||
reversed.trajectory_point_count != forward.trajectory_point_count ||
reversed.sample_period_ms != forward.sample_period_ms ||
reversed.raw_units_per_degree != forward.raw_units_per_degree) {
return common::Result<void>::failure(
"reversed trajectory metadata differs from the forward trajectory");
}
for (std::size_t index = 0U; index < reversed.samples.size(); ++index) {
const auto &actual = reversed.samples[index];
const auto &expected = forward.samples[forward.samples.size() - 1U - index];
const long long expected_elapsed =
static_cast<long long>(index) * forward.sample_period_ms;
if (actual.sample_index != index || actual.elapsed_ms != expected_elapsed ||
actual.joints_deg != expected.joints_deg ||
!PoseExactlyEqual(actual.tcp_pose, expected.tcp_pose) ||
actual.source != expected.source) {
return common::Result<void>::failure(
"reversed trajectory sample " + std::to_string(index) +
" does not exactly match the inverse forward sample");
}
}
return common::Result<void>::success();
}
common::Result<PhysicalTextLines> ReadVerifiedForwardText(
const VendorDragTrajectoryParseResult &forward) {
const auto metadata = ValidateForwardMetadata(forward);
if (!metadata.ok) {
return common::Result<PhysicalTextLines>::failure(metadata.error_message);
}
const auto integrity_before_read = VerifyFileIntegrity(forward);
if (!integrity_before_read.ok) {
return common::Result<PhysicalTextLines>::failure(
integrity_before_read.error_message);
}
const auto text = ReadPhysicalTextLines(forward.source_file);
const auto integrity_after_read = VerifyFileIntegrity(forward);
if (!text.ok || !integrity_after_read.ok) {
return common::Result<PhysicalTextLines>::failure(
!text.ok ? text.error_message : integrity_after_read.error_message);
}
if (text.value.lines.size() != forward.samples.size()) {
return common::Result<PhysicalTextLines>::failure(
"source physical-line count differs from parsed samples");
}
return text;
}
common::Result<VendorDragTrajectoryParseResult> ParseAndValidateReversedArtifact(
const std::filesystem::path &path,
const VendorDragTrajectoryParseResult &forward) {
const auto reversed = ParseVendorDragTrajectoryJsonLines(
path, forward.sample_period_ms, forward.raw_units_per_degree);
if (!reversed.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(
"failed to parse generated reversed trajectory: " + reversed.error_message);
}
const auto integrity = VerifyFileIntegrity(reversed.value);
if (!integrity.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(
integrity.error_message);
}
const auto samples = ValidateReversedSamples(forward, reversed.value);
return samples.ok
? reversed
: common::Result<VendorDragTrajectoryParseResult>::failure(samples.error_message);
}
} // namespace
std::filesystem::path DefaultReversedVendorTrajectoryDirectory() {
return std::filesystem::path(RM_RUNTIME_DATA_ROOT) / "reversed_vendor_trajectories";
}
common::Result<VendorDragTrajectoryParseResult>
CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(
const VendorDragTrajectoryParseResult &forward_trajectory,
const std::filesystem::path &output_directory) {
const auto forward_text = ReadVerifiedForwardText(forward_trajectory);
if (!forward_text.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(
forward_text.error_message);
}
const auto paths = CreateUniqueArtifactDirectory(
output_directory, forward_trajectory.source_file_sha256);
if (!paths.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(paths.error_message);
}
CreatedArtifactCleanup cleanup(paths.value);
const auto write = WriteReversedPhysicalLinesWithoutOverwrite(
forward_text.value, paths.value.trajectory_file);
const auto read_only = write.ok
? MakeFileReadOnly(paths.value.trajectory_file)
: common::Result<void>::failure(write.error_message);
if (!read_only.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(read_only.error_message);
}
const auto reversed_text = ReadPhysicalTextLines(paths.value.trajectory_file);
const auto exact_lines = reversed_text.ok
? ValidateExactPhysicalLineReversal(forward_text.value, reversed_text.value)
: common::Result<void>::failure(reversed_text.error_message);
if (!exact_lines.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(exact_lines.error_message);
}
const auto reversed = ParseAndValidateReversedArtifact(
paths.value.trajectory_file, forward_trajectory);
if (!reversed.ok) {
return reversed;
}
cleanup.Release();
return reversed;
}
} // namespace rm_control::modules::trajectory
RM/src/modules/trajectory/sha256_file_checksum.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file sha256_file_checksum.cpp
* @brief 使用 OpenSSL EVP 流式计算轨迹文件 SHA-256。
*
* @author 唐永康
* @date 2026-07-10
*/
#include "rm_control/modules/trajectory/sha256_file_checksum.h"
#include <openssl/evp.h>
#include <array>
#include <fstream>
#include <iomanip>
#include <memory>
#include <sstream>
namespace rm_control::modules::trajectory {
namespace {
constexpr std::size_t kDigestReadBufferSize = 64U * 1024U;
std::string PathForError(const std::filesystem::path &path) {
return path.string();
}
} // namespace
common::Result<std::string> CalculateFileSha256(const std::filesystem::path &file_path) {
if (file_path.empty()) {
return common::Result<std::string>::failure("SHA-256 file path must not be empty");
}
std::error_code filesystem_error;
const bool is_regular_file = std::filesystem::is_regular_file(file_path, filesystem_error);
if (filesystem_error) {
return common::Result<std::string>::failure(
"failed to inspect SHA-256 input " + PathForError(file_path) + ": " +
filesystem_error.message());
}
if (!is_regular_file) {
return common::Result<std::string>::failure(
"SHA-256 input is not a regular file: " + PathForError(file_path));
}
std::ifstream input(file_path, std::ios::binary);
if (!input) {
return common::Result<std::string>::failure(
"failed to open SHA-256 input: " + PathForError(file_path));
}
using DigestContext = std::unique_ptr<EVP_MD_CTX, decltype(&EVP_MD_CTX_free)>;
DigestContext digest_context(EVP_MD_CTX_new(), &EVP_MD_CTX_free);
if (!digest_context) {
return common::Result<std::string>::failure(
"OpenSSL failed to allocate SHA-256 digest context for " + PathForError(file_path));
}
if (EVP_DigestInit_ex(digest_context.get(), EVP_sha256(), nullptr) != 1) {
return common::Result<std::string>::failure(
"OpenSSL failed to initialize SHA-256 for " + PathForError(file_path));
}
std::array<char, kDigestReadBufferSize> buffer{};
while (input) {
input.read(buffer.data(), static_cast<std::streamsize>(buffer.size()));
const std::streamsize bytes_read = input.gcount();
if (bytes_read > 0 &&
EVP_DigestUpdate(digest_context.get(), buffer.data(),
static_cast<std::size_t>(bytes_read)) != 1) {
return common::Result<std::string>::failure(
"OpenSSL failed to update SHA-256 for " + PathForError(file_path));
}
}
if (!input.eof()) {
return common::Result<std::string>::failure(
"failed while reading SHA-256 input: " + PathForError(file_path));
}
std::array<unsigned char, EVP_MAX_MD_SIZE> digest{};
unsigned int digest_size = 0;
if (EVP_DigestFinal_ex(digest_context.get(), digest.data(), &digest_size) != 1) {
return common::Result<std::string>::failure(
"OpenSSL failed to finalize SHA-256 for " + PathForError(file_path));
}
std::ostringstream hexadecimal_digest;
hexadecimal_digest << std::hex << std::setfill('0');
for (unsigned int index = 0; index < digest_size; ++index) {
hexadecimal_digest << std::setw(2) << static_cast<unsigned int>(digest[index]);
}
return common::Result<std::string>::success(hexadecimal_digest.str());
}
} // namespace rm_control::modules::trajectory
RM/src/modules/trajectory/trajectory_file_store.cpp
View file @ 8275ee2e
/**
* @file trajectory_file_store.cpp
* @brief RM75 拖动示教轨迹文件存储模块实现。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件负责轨迹目录创建、CSV 读写、会话名清洗和 C++ TCP waypoint 文件导出。
* 该模块属于离线文件处理模块，不访问真实机械臂、不调用 RM_API2。core 层只把采样数据
* 传入本模块，本模块负责稳定地落盘和读取。
*
* 设计原则：
* - 文件路径和 CSV 格式集中管理；
* - 硬件控制逻辑不进入文件模块；
* - 所有文件错误通过 `Result` 返回；
* - RM75 7 轴 CSV 格式使用命名常量表达，避免魔法数字散落。
*
* @dependencies
* - rm_control/modules/trajectory/trajectory_file_store.h
*/
#include "rm_control/modules/trajectory/trajectory_file_store.h" #include "rm_control/modules/trajectory/trajectory_file_store.h"
#include "rm_control/common/rm75_constants.h"
#include <algorithm> #include <algorithm>
#include <cerrno> #include <cerrno>
#include <cctype> #include <cctype>
#include <cmath> #include <cmath>
#include <cstdlib> #include <cstdlib>
#include <cstring>
#include <ctime> #include <ctime>
#include <filesystem>
#include <fstream> #include <fstream>
#include <iomanip> #include <iomanip>
#include <limits>
#include <sstream> #include <sstream>
#include <sys/stat.h>
#include <vector> #include <vector>
#ifndef RM_PROJECT_ROOT #ifndef RM_RUNTIME_DATA_ROOT
#define RM_PROJECT_ROOT "." #define RM_RUNTIME_DATA_ROOT "build/runtime_data"
#endif #endif
namespace rm_control::modules::trajectory { namespace rm_control::modules::trajectory {
namespace { namespace {
constexpr int kExpectedCsvColumnCount = 15; constexpr int kCsvSampleIndexColumn = 0;
constexpr int kCsvElapsedMsColumn = 1;
bool isPathSeparator(char value) { constexpr int kCsvJointStartColumn = 2;
return value == '/'; constexpr int kCsvTcpXColumn = 9;
} constexpr int kCsvTcpYColumn = 10;
constexpr int kCsvTcpZColumn = 11;
constexpr int kCsvTcpRxColumn = 12;
constexpr int kCsvTcpRyColumn = 13;
constexpr int kCsvTcpRzColumn = 14;
constexpr int kCsvSourceColumn = 15;
constexpr int kLegacyCsvColumnCount = 15;
constexpr int kExpectedCsvColumnCount = 16;
constexpr const char *kCurrentCsvHeader =
"sample_index,timestamp_ms,"
"j1_deg,j2_deg,j3_deg,j4_deg,j5_deg,j6_deg,j7_deg,"
"tcp_x_m,tcp_y_m,tcp_z_m,tcp_rx_rad,tcp_ry_rad,tcp_rz_rad,source";
constexpr const char *kLegacyCsvHeader =
"sample_index,elapsed_ms,"
"j1_deg,j2_deg,j3_deg,j4_deg,j5_deg,j6_deg,j7_deg,"
"tcp_x_m,tcp_y_m,tcp_z_m,tcp_rx_rad,tcp_ry_rad,tcp_rz_rad";
std::string parentPath(const std::string &path) { std::string parentPath(const std::string &path) {
const size_t slash = path.find_last_of('/'); return std::filesystem::path(path).parent_path().string();
if (slash == std::string::npos) {
return "";
}
if (slash == 0) {
return "/";
}
return path.substr(0, slash);
}
bool directoryExists(const std::string &path) {
struct stat status = {};
return ::stat(path.c_str(), &status) == 0 && S_ISDIR(status.st_mode);
} }
common::Result<void> ensureDirectoryRecursive(const std::string &path) { common::Result<void> ensureDirectoryRecursive(const std::string &path) {
if (path.empty() || directoryExists(path)) { if (path.empty()) {
return common::Result<void>::success(); return common::Result<void>::success();
} }
std::error_code filesystem_error;
const std::string parent = parentPath(path); std::filesystem::create_directories(path, filesystem_error);
if (!parent.empty() && parent != path) { if (filesystem_error) {
auto parent_result = ensureDirectoryRecursive(parent); return common::Result<void>::failure(
if (!parent_result.ok) { "failed to create trajectory directory " + path + ": " +
return parent_result; filesystem_error.message());
}
} }
if (!std::filesystem::is_directory(path, filesystem_error) || filesystem_error) {
if (::mkdir(path.c_str(), 0755) == 0 || errno == EEXIST) { return common::Result<void>::failure(
return common::Result<void>::success(); "trajectory directory path is not a directory: " + path);
} }
return common::Result<void>::success();
return common::Result<void>::failure("mkdir failed for " + path + ": " + std::strerror(errno));
} }
std::string joinPath(const std::string &left, const std::string &right) { std::string joinPath(const std::string &left, const std::string &right) {
...@@ -69,10 +94,18 @@ std::string joinPath(const std::string &left, const std::string &right) { ...@@ -69,10 +94,18 @@ std::string joinPath(const std::string &left, const std::string &right) {
if (right.empty()) { if (right.empty()) {
return left; return left;
} }
if (isPathSeparator(left.back())) { return (std::filesystem::path(left) / right).string();
return left + right; }
bool ConvertToLocalTime(std::time_t time, std::tm *local_time) {
if (local_time == nullptr) {
return false;
} }
return left + "/" + right; #if defined(_WIN32)
return ::localtime_s(local_time, &time) == 0;
#else
return ::localtime_r(&time, local_time) != nullptr;
#endif
} }
std::string trimCopy(const std::string &value) { std::string trimCopy(const std::string &value) {
...@@ -92,6 +125,9 @@ std::vector<std::string> splitCsvLine(const std::string &line) { ...@@ -92,6 +125,9 @@ std::vector<std::string> splitCsvLine(const std::string &line) {
while (std::getline(input, column, ',')) { while (std::getline(input, column, ',')) {
columns.push_back(trimCopy(column)); columns.push_back(trimCopy(column));
} }
if (!line.empty() && line.back() == ',') {
columns.emplace_back();
}
return columns; return columns;
} }
...@@ -127,51 +163,176 @@ bool parseLongLong(const std::string &value, long long *out) { ...@@ -127,51 +163,176 @@ bool parseLongLong(const std::string &value, long long *out) {
return true; return true;
} }
bool isValidTrajectorySource(const std::string &source) {
if (source.empty()) {
return false;
}
return std::all_of(source.begin(), source.end(), [](unsigned char character) {
return std::isalnum(character) != 0 || character == '_' || character == '-' ||
character == '.';
});
}
/**
* @brief 从 CSV 列中解析 RM75 关节角。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* CSV 固定使用 7 个关节角列。该函数集中处理关节列解析和错误信息生成，
* 避免 `parseSample()` 同时承担列索引细节和样本结构装配。
*
* @param[in] columns CSV 行拆分后的列。
* @param[in] line_number 当前 CSV 行号，用于错误信息。
* @param[out] joints_deg 解析后的关节角列表，单位为度。
*
* @return 成功或具体关节列错误。
*
* @throws 本函数不主动抛出异常；向量分配失败由标准库报告。
*
* @note
* 该函数假设调用方已经验证列数为 `kExpectedCsvColumnCount`。
*
* @warning
* 当前 CSV 格式固定为 RM75 7 轴；支持其他 DOF 时需要扩展格式版本。
*
* @thread_safety
* 函数只操作局部变量和输出容器，线程安全。
*
* @par 边界条件
* 任一关节列为空、非数字或非有限值都会失败。
*
* @par 后续维护
* 如果 CSV 增加表头版本，应根据版本选择关节数量。
*/
common::Result<void> parseJointColumns(const std::vector<std::string> &columns,
int line_number,
std::vector<float> *joints_deg) {
if (joints_deg == nullptr) {
return common::Result<void>::failure("joints_deg output must not be null");
}
joints_deg->assign(common::kRm75JointCount, 0.0F);
for (std::size_t joint_index = 0;
joint_index < common::kRm75JointCount;
++joint_index) {
const std::size_t column_index =
static_cast<std::size_t>(kCsvJointStartColumn) + joint_index;
if (!parseFloat(columns[column_index], &(*joints_deg)[joint_index])) {
return common::Result<void>::failure(
"CSV line " + std::to_string(line_number) + " has invalid J" +
std::to_string(joint_index + 1));
}
}
return common::Result<void>::success();
}
/**
* @brief 从 CSV 列中解析 TCP 位姿。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 轨迹 CSV 使用 6 个 TCP 位姿列：x/y/z/rx/ry/rz。该函数集中处理列索引和格式校验，
* 让 `parseSample()` 只关注样本级流程。
*
* @param[in] columns CSV 行拆分后的列。
* @param[in] line_number 当前 CSV 行号，用于错误信息。
* @param[out] pose 解析后的 TCP 位姿。
*
* @return 成功或 TCP 位姿字段错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 位置单位为 m，欧拉角单位为 rad。
*
* @warning
* 本函数不判断位姿是否可达或适合执行。
*
* @thread_safety
* 函数只操作输出对象，线程安全。
*
* @par 边界条件
* 任一 TCP 列为空、非数字或非有限值都会失败。
*
* @par 后续维护
* 如果 CSV 支持四元数，应新增字段并扩展 `TrajectorySample`。
*/
common::Result<void> parseTcpPoseColumns(const std::vector<std::string> &columns,
int line_number,
common::Pose *pose) {
if (pose == nullptr) {
return common::Result<void>::failure("pose output must not be null");
}
if (!parseFloat(columns[kCsvTcpXColumn], &pose->x) ||
!parseFloat(columns[kCsvTcpYColumn], &pose->y) ||
!parseFloat(columns[kCsvTcpZColumn], &pose->z) ||
!parseFloat(columns[kCsvTcpRxColumn], &pose->rx) ||
!parseFloat(columns[kCsvTcpRyColumn], &pose->ry) ||
!parseFloat(columns[kCsvTcpRzColumn], &pose->rz)) {
return common::Result<void>::failure(
"CSV line " + std::to_string(line_number) + " has invalid TCP pose");
}
return common::Result<void>::success();
}
common::Result<common::TrajectorySample> parseSample(const std::vector<std::string> &columns, common::Result<common::TrajectorySample> parseSample(const std::vector<std::string> &columns,
int line_number) { int line_number) {
if (columns.size() != kExpectedCsvColumnCount) { if (columns.size() != kExpectedCsvColumnCount &&
columns.size() != kLegacyCsvColumnCount) {
return common::Result<common::TrajectorySample>::failure( return common::Result<common::TrajectorySample>::failure(
"CSV line " + std::to_string(line_number) + " must have " + "CSV line " + std::to_string(line_number) +
std::to_string(kExpectedCsvColumnCount) + " columns"); " must have 15 legacy or 16 current columns");
} }
long long ignored_sample_index = 0; long long parsed_sample_index = 0;
common::TrajectorySample sample; common::TrajectorySample sample;
if (!parseLongLong(columns[0], &ignored_sample_index) || if (!parseLongLong(columns[kCsvSampleIndexColumn], &parsed_sample_index) ||
!parseLongLong(columns[1], &sample.elapsed_ms)) { parsed_sample_index < 0 ||
static_cast<unsigned long long>(parsed_sample_index) >
std::numeric_limits<std::size_t>::max() ||
!parseLongLong(columns[kCsvElapsedMsColumn], &sample.elapsed_ms)) {
return common::Result<common::TrajectorySample>::failure( return common::Result<common::TrajectorySample>::failure(
"CSV line " + std::to_string(line_number) + " has invalid sample_index or elapsed_ms"); "CSV line " + std::to_string(line_number) + " has invalid sample_index or elapsed_ms");
} }
sample.sample_index = static_cast<std::size_t>(parsed_sample_index);
sample.joints_deg.resize(7); auto joint_result = parseJointColumns(columns, line_number, &sample.joints_deg);
for (int i = 0; i < 7; ++i) { if (!joint_result.ok) {
if (!parseFloat(columns[2 + i], &sample.joints_deg[static_cast<size_t>(i)])) { return common::Result<common::TrajectorySample>::failure(joint_result.error_message);
return common::Result<common::TrajectorySample>::failure(
"CSV line " + std::to_string(line_number) + " has invalid J" + std::to_string(i + 1));
}
} }
if (!parseFloat(columns[9], &sample.tcp_pose.x) || auto pose_result = parseTcpPoseColumns(columns, line_number, &sample.tcp_pose);
!parseFloat(columns[10], &sample.tcp_pose.y) || if (!pose_result.ok) {
!parseFloat(columns[11], &sample.tcp_pose.z) || return common::Result<common::TrajectorySample>::failure(pose_result.error_message);
!parseFloat(columns[12], &sample.tcp_pose.rx) || }
!parseFloat(columns[13], &sample.tcp_pose.ry) ||
!parseFloat(columns[14], &sample.tcp_pose.rz)) { if (columns.size() == kExpectedCsvColumnCount) {
return common::Result<common::TrajectorySample>::failure( sample.source = columns[kCsvSourceColumn];
"CSV line " + std::to_string(line_number) + " has invalid TCP pose"); if (!isValidTrajectorySource(sample.source)) {
return common::Result<common::TrajectorySample>::failure(
"CSV line " + std::to_string(line_number) +
" has an invalid trajectory source");
}
} else {
sample.source = "legacy_rm75_csv";
} }
return common::Result<common::TrajectorySample>::success(sample); return common::Result<common::TrajectorySample>::success(sample);
} }
void writeCsvHeader(std::ostream &out) { void writeCsvHeader(std::ostream &out) {
out << "sample_index,elapsed_ms," out << kCurrentCsvHeader << '\n';
<< "j1_deg,j2_deg,j3_deg,j4_deg,j5_deg,j6_deg,j7_deg,"
<< "tcp_x_m,tcp_y_m,tcp_z_m,tcp_rx_rad,tcp_ry_rad,tcp_rz_rad\n";
} }
void writeCsvSample(std::ostream &out, int sample_index, const common::TrajectorySample &sample) { void writeCsvSample(std::ostream &out, const common::TrajectorySample &sample) {
out << sample_index << ',' << sample.elapsed_ms; out << sample.sample_index << ',' << sample.elapsed_ms;
out << std::fixed << std::setprecision(6); out << std::fixed << std::setprecision(6);
for (float joint : sample.joints_deg) { for (float joint : sample.joints_deg) {
out << ',' << joint; out << ',' << joint;
...@@ -181,7 +342,8 @@ void writeCsvSample(std::ostream &out, int sample_index, const common::Trajector ...@@ -181,7 +342,8 @@ void writeCsvSample(std::ostream &out, int sample_index, const common::Trajector
<< ',' << sample.tcp_pose.z << ',' << sample.tcp_pose.z
<< ',' << sample.tcp_pose.rx << ',' << sample.tcp_pose.rx
<< ',' << sample.tcp_pose.ry << ',' << sample.tcp_pose.ry
<< ',' << sample.tcp_pose.rz << '\n'; << ',' << sample.tcp_pose.rz
<< ',' << sample.source << '\n';
} }
std::string makeCppIdentifier(const std::string &session_name) { std::string makeCppIdentifier(const std::string &session_name) {
...@@ -193,12 +355,202 @@ std::string makeCppIdentifier(const std::string &session_name) { ...@@ -193,12 +355,202 @@ std::string makeCppIdentifier(const std::string &session_name) {
return identifier; return identifier;
} }
/**
* @brief 校验轨迹样本能否写入当前 RM75 CSV 格式。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 当前 CSV 格式固定写出 7 个关节角。该函数在打开文件前统一校验样本列表和每个样本
* 的关节数量，避免写到一半才发现格式不一致。
*
* @param[in] samples 待写入样本列表。
*
* @return 成功或样本格式错误。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 空样本列表会被拒绝。
*
* @warning
* 支持其他 DOF 前不要放宽此校验，否则会破坏 CSV 读写一致性。
*
* @thread_safety
* 函数只读取输入容器，线程安全。
*
* @par 边界条件
* 任一样本关节数量不是 7 都会失败。
*
* @par 后续维护
* 如果引入 CSV 版本字段，应根据版本校验不同关节数量。
*/
common::Result<void> validateSamplesForRm75Csv(
const std::vector<common::TrajectorySample> &samples) {
if (samples.empty()) {
return common::Result<void>::failure("trajectory sample list must not be empty");
}
for (std::size_t sample_index = 0; sample_index < samples.size(); ++sample_index) {
const common::TrajectorySample &sample = samples[sample_index];
if (sample.sample_index != sample_index) {
return common::Result<void>::failure(
"trajectory sample_index must be contiguous and start at zero");
}
if (sample.joints_deg.size() != common::kRm75JointCount) {
return common::Result<void>::failure("trajectory sample joints_deg size must be 7");
}
if (!isValidTrajectorySource(sample.source)) {
return common::Result<void>::failure(
"trajectory sample source must contain only letters, digits, '_', '-' or '.'");
}
}
return common::Result<void>::success();
}
/**
* @brief 写出 C++ waypoint 文件头部。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* C++ 导出文件由头部类型定义、waypoint 数据和尾部命名空间闭合组成。
* 该函数只负责头部，避免 `exportTcpPathCpp()` 同时拼接所有文本。
*
* @param[out] output 输出文件流。
* @param[in] variable_name waypoint 变量名。
*
* @return 无返回值。
*
* @throws 本函数不主动抛出异常；输出流异常由调用方处理。
*
* @note
* 输出命名空间固定为 `rm_control_generated`。
*
* @warning
* 修改结构体字段会影响所有生成文件的使用方。
*
* @thread_safety
* 函数无全局状态；并发写同一流需调用方保护。
*
* @par 边界条件
* 空变量名会生成非法 C++，调用方应先构造合法标识符。
*
* @par 后续维护
* 如果生成头文件而非 cpp，应新增导出函数，不要复用此头部文本。
*/
void writeTcpPathCppHeader(std::ostream &output, const std::string &variable_name) {
output << "#include <cstdint>\n"
<< "#include <vector>\n\n"
<< "namespace rm_control_generated {\n\n"
<< "struct TcpWaypoint {\n"
<< " std::int64_t elapsed_ms;\n"
<< " float x;\n"
<< " float y;\n"
<< " float z;\n"
<< " float rx;\n"
<< " float ry;\n"
<< " float rz;\n"
<< "};\n\n"
<< "const std::vector<TcpWaypoint> " << variable_name << " = {\n";
}
/**
* @brief 写出 C++ waypoint 数据项。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数只负责把样本列表中的 TCP 位姿写成 C++ 初始化列表，不写 include、类型定义或
* 命名空间尾部。拆分后便于以后复用 waypoint 数据写出逻辑。
*
* @param[out] output 输出文件流。
* @param[in] samples 轨迹样本列表。
*
* @return 无返回值。
*
* @throws 本函数不主动抛出异常；输出流异常由调用方处理。
*
* @note
* 只导出 TCP 位姿，不导出关节角。
*
* @warning
* 本函数不校验样本为空；调用方负责提前校验。
*
* @thread_safety
* 函数无全局状态；并发写同一流需调用方保护。
*
* @par 边界条件
* 空样本列表只写出空数据项区域。
*
* @par 后续维护
* 如需导出关节轨迹，应新增独立数据结构和导出函数。
*/
void writeTcpPathCppSamples(std::ostream &output,
const std::vector<common::TrajectorySample> &samples) {
output << std::fixed << std::setprecision(6);
for (const common::TrajectorySample &sample : samples) {
output << " {" << sample.elapsed_ms
<< ", " << sample.tcp_pose.x
<< "F, " << sample.tcp_pose.y
<< "F, " << sample.tcp_pose.z
<< "F, " << sample.tcp_pose.rx
<< "F, " << sample.tcp_pose.ry
<< "F, " << sample.tcp_pose.rz
<< "F},\n";
}
}
/**
* @brief 写出 C++ waypoint 文件尾部。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数闭合 waypoint 初始化列表和命名空间。拆分尾部输出可以让导出主流程保持短小。
*
* @param[out] output 输出文件流。
*
* @return 无返回值。
*
* @throws 本函数不主动抛出异常；输出流异常由调用方处理。
*
* @note
* 尾部命名空间必须和头部保持一致。
*
* @warning
* 修改命名空间时必须同步头部和使用方。
*
* @thread_safety
* 函数无全局状态；并发写同一流需调用方保护。
*
* @par 边界条件
* 即使样本为空也能输出语法完整尾部。
*
* @par 后续维护
* 如果生成多段变量，应为每段生成独立尾部或新增 writer 类。
*/
void writeTcpPathCppFooter(std::ostream &output) {
output << "};\n\n"
<< "} // namespace rm_control_generated\n";
}
} // namespace } // namespace
std::string timestampString() { std::string timestampString() {
const std::time_t now = std::time(nullptr); const std::time_t now = std::time(nullptr);
std::tm local_time = {}; std::tm local_time = {};
localtime_r(&now, &local_time); if (!ConvertToLocalTime(now, &local_time)) {
return "time_unavailable";
}
std::ostringstream out; std::ostringstream out;
out << std::put_time(&local_time, "%Y%m%d_%H%M%S"); out << std::put_time(&local_time, "%Y%m%d_%H%M%S");
...@@ -230,7 +582,7 @@ std::string sanitizeSessionName(const std::string &raw_name) { ...@@ -230,7 +582,7 @@ std::string sanitizeSessionName(const std::string &raw_name) {
} }
std::string defaultTrajectoryStorageRoot() { std::string defaultTrajectoryStorageRoot() {
return std::string(RM_PROJECT_ROOT) + "/data/trajectories"; return std::string(RM_RUNTIME_DATA_ROOT) + "/trajectories";
} }
common::Result<TrajectoryStoragePaths> ensureTrajectoryStorage(const std::string &root_dir) { common::Result<TrajectoryStoragePaths> ensureTrajectoryStorage(const std::string &root_dir) {
...@@ -274,8 +626,9 @@ common::Result<TrajectoryFileSet> makeTrajectoryFileSet(const std::string &root_ ...@@ -274,8 +626,9 @@ common::Result<TrajectoryFileSet> makeTrajectoryFileSet(const std::string &root_
common::Result<void> writeTrajectoryCsv(const std::string &csv_path, common::Result<void> writeTrajectoryCsv(const std::string &csv_path,
const std::vector<common::TrajectorySample> &samples) { const std::vector<common::TrajectorySample> &samples) {
if (samples.empty()) { auto validation = validateSamplesForRm75Csv(samples);
return common::Result<void>::failure("trajectory sample list must not be empty"); if (!validation.ok) {
return validation;
} }
auto parent_result = ensureDirectoryRecursive(parentPath(csv_path)); auto parent_result = ensureDirectoryRecursive(parentPath(csv_path));
...@@ -289,11 +642,59 @@ common::Result<void> writeTrajectoryCsv(const std::string &csv_path, ...@@ -289,11 +642,59 @@ common::Result<void> writeTrajectoryCsv(const std::string &csv_path,
} }
writeCsvHeader(output); writeCsvHeader(output);
for (size_t i = 0; i < samples.size(); ++i) { for (const common::TrajectorySample &sample : samples) {
if (samples[i].joints_deg.size() != 7) { writeCsvSample(output, sample);
return common::Result<void>::failure("trajectory sample joints_deg size must be 7"); }
} return common::Result<void>::success();
writeCsvSample(output, static_cast<int>(i), samples[i]); }
common::Result<void> writeTrajectoryCsvWithoutOverwrite(
const std::filesystem::path &csv_path,
const std::vector<common::TrajectorySample> &samples) {
const auto validation = validateSamplesForRm75Csv(samples);
if (!validation.ok) {
return validation;
}
if (csv_path.empty() || csv_path.parent_path().empty()) {
return common::Result<void>::failure(
"synchronized trajectory CSV path and parent must not be empty");
}
std::error_code filesystem_error;
std::filesystem::create_directories(csv_path.parent_path(), filesystem_error);
if (filesystem_error) {
return common::Result<void>::failure(
"failed to create synchronized trajectory CSV directory " +
csv_path.parent_path().string() + ": " + filesystem_error.message());
}
if (std::filesystem::exists(csv_path, filesystem_error)) {
return common::Result<void>::failure(
"refusing to overwrite synchronized trajectory CSV: " + csv_path.string());
}
if (filesystem_error) {
return common::Result<void>::failure(
"failed to inspect synchronized trajectory CSV " + csv_path.string() +
": " + filesystem_error.message());
}
std::ofstream output(csv_path, std::ios::out);
if (!output) {
return common::Result<void>::failure(
"failed to create synchronized trajectory CSV: " + csv_path.string());
}
writeCsvHeader(output);
for (const common::TrajectorySample &sample : samples) {
writeCsvSample(output, sample);
}
output.flush();
if (!output) {
return common::Result<void>::failure(
"failed to write synchronized trajectory CSV: " + csv_path.string());
}
output.close();
if (!output) {
return common::Result<void>::failure(
"failed to close synchronized trajectory CSV: " + csv_path.string());
} }
return common::Result<void>::success(); return common::Result<void>::success();
} }
...@@ -310,7 +711,15 @@ common::Result<std::vector<common::TrajectorySample>> loadTrajectoryCsv(const st ...@@ -310,7 +711,15 @@ common::Result<std::vector<common::TrajectorySample>> loadTrajectoryCsv(const st
int line_number = 0; int line_number = 0;
while (std::getline(input, line)) { while (std::getline(input, line)) {
++line_number; ++line_number;
if (line_number == 1 || trimCopy(line).empty()) { if (line_number == 1) {
const std::string header = trimCopy(line);
if (header != kCurrentCsvHeader && header != kLegacyCsvHeader) {
return common::Result<std::vector<common::TrajectorySample>>::failure(
"trajectory CSV has an unsupported header: " + csv_path);
}
continue;
}
if (trimCopy(line).empty()) {
continue; continue;
} }
...@@ -318,6 +727,19 @@ common::Result<std::vector<common::TrajectorySample>> loadTrajectoryCsv(const st ...@@ -318,6 +727,19 @@ common::Result<std::vector<common::TrajectorySample>> loadTrajectoryCsv(const st
if (!sample.ok) { if (!sample.ok) {
return common::Result<std::vector<common::TrajectorySample>>::failure(sample.error_message); return common::Result<std::vector<common::TrajectorySample>>::failure(sample.error_message);
} }
if (sample.value.sample_index != samples.size()) {
return common::Result<std::vector<common::TrajectorySample>>::failure(
"trajectory CSV sample_index must be contiguous and start at zero");
}
if (sample.value.elapsed_ms < 0) {
return common::Result<std::vector<common::TrajectorySample>>::failure(
"trajectory CSV timestamp must not be negative");
}
if (!samples.empty() &&
sample.value.elapsed_ms <= samples.back().elapsed_ms) {
return common::Result<std::vector<common::TrajectorySample>>::failure(
"trajectory CSV timestamps must be strictly increasing");
}
samples.push_back(sample.value); samples.push_back(sample.value);
} }
...@@ -345,34 +767,9 @@ common::Result<void> exportTcpPathCpp(const std::string &cpp_path, ...@@ -345,34 +767,9 @@ common::Result<void> exportTcpPathCpp(const std::string &cpp_path,
} }
const std::string variable_name = "kTcpPath_" + makeCppIdentifier(session_name); const std::string variable_name = "kTcpPath_" + makeCppIdentifier(session_name);
output << "#include <cstdint>\n" writeTcpPathCppHeader(output, variable_name);
<< "#include <vector>\n\n" writeTcpPathCppSamples(output, samples);
<< "namespace rm_control_generated {\n\n" writeTcpPathCppFooter(output);
<< "struct TcpWaypoint {\n"
<< " std::int64_t elapsed_ms;\n"
<< " float x;\n"
<< " float y;\n"
<< " float z;\n"
<< " float rx;\n"
<< " float ry;\n"
<< " float rz;\n"
<< "};\n\n"
<< "const std::vector<TcpWaypoint> " << variable_name << " = {\n";
output << std::fixed << std::setprecision(6);
for (const auto &sample : samples) {
output << " {" << sample.elapsed_ms
<< ", " << sample.tcp_pose.x
<< "F, " << sample.tcp_pose.y
<< "F, " << sample.tcp_pose.z
<< "F, " << sample.tcp_pose.rx
<< "F, " << sample.tcp_pose.ry
<< "F, " << sample.tcp_pose.rz
<< "F},\n";
}
output << "};\n\n"
<< "} // namespace rm_control_generated\n";
return common::Result<void>::success(); return common::Result<void>::success();
} }
......
RM/src/modules/trajectory/trajectory_playback_report_store.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/modules/trajectory/trajectory_playback_report_store.h"
#include <algorithm>
#include <cctype>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <system_error>
#ifndef RM_RUNTIME_DATA_ROOT
#define RM_RUNTIME_DATA_ROOT "runtime_data"
#endif
namespace rm_control::modules::trajectory {
namespace {
std::string EscapeJsonString(const std::string &value) {
std::ostringstream escaped;
for (const unsigned char character : value) {
if (character == '"' || character == '\\') {
escaped << '\\' << static_cast<char>(character);
} else if (character == '\n') {
escaped << "\\n";
} else if (character == '\r') {
escaped << "\\r";
} else if (character == '\t') {
escaped << "\\t";
} else if (character < 0x20U) {
escaped << "\\u" << std::hex << std::setw(4) << std::setfill('0')
<< static_cast<int>(character) << std::dec;
} else {
escaped << static_cast<char>(character);
}
}
return escaped.str();
}
bool IsSha256(const std::string &value) {
return value.size() == 64U &&
std::all_of(value.begin(), value.end(), [](unsigned char character) {
return std::isxdigit(character) != 0;
});
}
bool IsSafeReportIdentifier(const std::string &value) {
return !value.empty() && value.size() <= 64U &&
std::all_of(value.begin(), value.end(), [](unsigned char character) {
return std::isalnum(character) != 0 || character == '_' ||
character == '-';
});
}
bool O6ValuesAreValid(const PlaybackReportO6Values &values) {
return std::all_of(values.begin(), values.end(),
[](std::uint16_t value) { return value <= 255U; });
}
common::Result<void> ValidateReport(const ControllerManagedPlaybackReport &report) {
const bool timing_is_valid = report.sample_period_ms > 0 &&
report.source_duration_ms >= 0 && report.observed_execution_duration_ms >= 0 &&
std::isfinite(report.minimum_poll_period_ms) &&
std::isfinite(report.maximum_poll_period_ms) &&
std::isfinite(report.average_poll_period_ms) &&
std::isfinite(report.maximum_absolute_poll_jitter_ms) &&
report.minimum_poll_period_ms >= 0.0 && report.maximum_poll_period_ms >= 0.0 &&
report.average_poll_period_ms >= 0.0 &&
report.maximum_absolute_poll_jitter_ms >= 0.0;
const bool return_timing_is_valid = !report.returned_from_trajectory_end ||
(report.return_observed_execution_duration_ms >= 0.0 &&
std::isfinite(report.return_minimum_poll_period_ms) &&
std::isfinite(report.return_maximum_poll_period_ms) &&
std::isfinite(report.return_average_poll_period_ms) &&
std::isfinite(report.return_maximum_absolute_poll_jitter_ms) &&
report.return_minimum_poll_period_ms >= 0.0 &&
report.return_maximum_poll_period_ms >= 0.0 &&
report.return_average_poll_period_ms >= 0.0 &&
report.return_maximum_absolute_poll_jitter_ms >= 0.0);
const bool identifiers_are_valid = report.plan_speed_percent >= 1 &&
report.plan_speed_percent <= 100 && report.controller_program_id >= 1 &&
report.controller_program_id <= 100 && report.o6_device_id >= 1U &&
report.o6_device_id <= 247U;
const bool return_metadata_is_valid = !report.returned_from_trajectory_end ||
(!report.return_source_file.empty() &&
IsSha256(report.return_source_file_sha256) &&
report.return_controller_program_id >= 1 &&
report.return_controller_program_id <= 100 &&
report.return_controller_program_id != report.controller_program_id);
if (!IsSafeReportIdentifier(report.report_identifier) ||
report.source_file.empty() || !IsSha256(report.source_file_sha256) ||
report.trajectory_point_count == 0U || !timing_is_valid ||
!return_timing_is_valid || !identifiers_are_valid ||
!return_metadata_is_valid ||
!O6ValuesAreValid(report.target_positions) ||
!O6ValuesAreValid(report.speed_commands) ||
!O6ValuesAreValid(report.torque_commands) ||
!O6ValuesAreValid(report.final_positions)) {
return common::Result<void>::failure(
"controller-managed playback report contains invalid metadata or O6 values");
}
return common::Result<void>::success();
}
template <std::size_t Size>
void WriteJsonArray(std::ostream &output, const std::array<std::uint16_t, Size> &values) {
output << '[';
for (std::size_t index = 0U; index < values.size(); ++index) {
output << (index == 0U ? "" : ", ") << values[index];
}
output << ']';
}
void WriteReportMetadata(std::ostream &output,
const ControllerManagedPlaybackReport &report) {
output << "{\n \"report_type\": \"controller_managed_vendor_trajectory\",\n"
<< " \"report_identifier\": \"" << report.report_identifier << "\",\n"
<< " \"source_file\": \"" << EscapeJsonString(report.source_file.string())
<< "\",\n \"source_file_sha256\": \"" << report.source_file_sha256 << "\",\n"
<< " \"trajectory_point_count\": " << report.trajectory_point_count << ",\n"
<< " \"sample_period_ms\": " << report.sample_period_ms << ",\n"
<< " \"source_duration_ms\": " << report.source_duration_ms << ",\n"
<< " \"timestamp_is_derived\": "
<< (report.timestamp_is_derived ? "true" : "false") << ",\n"
<< " \"plan_speed_percent\": " << report.plan_speed_percent << ",\n"
<< " \"controller_program_id\": " << report.controller_program_id << ",\n"
<< " \"returned_from_trajectory_end\": "
<< (report.returned_from_trajectory_end ? "true" : "false") << ",\n"
<< " \"return_source_file\": \""
<< EscapeJsonString(report.return_source_file.string()) << "\",\n"
<< " \"return_source_file_sha256\": \""
<< report.return_source_file_sha256 << "\",\n"
<< " \"return_controller_program_id\": "
<< report.return_controller_program_id << ",\n";
}
void WriteReportTiming(std::ostream &output,
const ControllerManagedPlaybackReport &report) {
output << " \"return_observed_execution_duration_ms\": "
<< report.return_observed_execution_duration_ms << ",\n"
<< " \"return_program_status_poll_count\": "
<< report.return_program_status_poll_count << ",\n"
<< " \"return_minimum_poll_period_ms\": "
<< report.return_minimum_poll_period_ms << ",\n"
<< " \"return_maximum_poll_period_ms\": "
<< report.return_maximum_poll_period_ms << ",\n"
<< " \"return_average_poll_period_ms\": "
<< report.return_average_poll_period_ms << ",\n"
<< " \"return_maximum_absolute_poll_jitter_ms\": "
<< report.return_maximum_absolute_poll_jitter_ms << ",\n"
<< " \"return_final_controller_state\": "
<< report.return_final_controller_state << ",\n"
<< " \"observed_execution_duration_ms\": "
<< report.observed_execution_duration_ms << ",\n"
<< " \"program_status_poll_count\": " << report.program_status_poll_count << ",\n"
<< " \"minimum_poll_period_ms\": " << report.minimum_poll_period_ms << ",\n"
<< " \"maximum_poll_period_ms\": " << report.maximum_poll_period_ms << ",\n"
<< " \"average_poll_period_ms\": " << report.average_poll_period_ms << ",\n"
<< " \"maximum_absolute_poll_jitter_ms\": "
<< report.maximum_absolute_poll_jitter_ms << ",\n"
<< " \"poll_timing_scope\": \"host_program_state_polling_not_point_dispatch\",\n"
<< " \"final_controller_state\": " << report.final_controller_state << ",\n"
<< " \"o6_device_id\": " << static_cast<int>(report.o6_device_id) << ",\n";
}
void WriteReportO6Values(std::ostream &output,
const ControllerManagedPlaybackReport &report) {
output << " \"target_positions\": ";
WriteJsonArray(output, report.target_positions);
output << ",\n \"speed_commands\": ";
WriteJsonArray(output, report.speed_commands);
output << ",\n \"torque_commands\": ";
WriteJsonArray(output, report.torque_commands);
output << ",\n \"final_positions\": ";
WriteJsonArray(output, report.final_positions);
output << "\n}\n";
}
void WriteReportJson(std::ostream &output,
const ControllerManagedPlaybackReport &report) {
output << std::fixed << std::setprecision(3);
WriteReportMetadata(output, report);
WriteReportTiming(output, report);
WriteReportO6Values(output, report);
}
} // namespace
std::filesystem::path DefaultTrajectoryPlaybackReportDirectory() {
return std::filesystem::path(RM_RUNTIME_DATA_ROOT) /
"trajectory_playback_reports";
}
common::Result<std::filesystem::path>
WriteControllerManagedPlaybackReportWithoutOverwrite(
const ControllerManagedPlaybackReport &report,
const std::filesystem::path &output_directory) {
const auto validation = ValidateReport(report);
if (!validation.ok) {
return common::Result<std::filesystem::path>::failure(validation.error_message);
}
if (output_directory.empty()) {
return common::Result<std::filesystem::path>::failure(
"trajectory playback report directory must not be empty");
}
std::error_code filesystem_error;
std::filesystem::create_directories(output_directory, filesystem_error);
if (filesystem_error) {
return common::Result<std::filesystem::path>::failure(
"failed to create trajectory playback report directory: " +
filesystem_error.message());
}
const std::filesystem::path report_path =
output_directory /
("rm75_o6_playback_" + report.report_identifier + "_" +
report.source_file_sha256.substr(0U, 12U) + ".json");
if (std::filesystem::exists(report_path, filesystem_error) || filesystem_error) {
return common::Result<std::filesystem::path>::failure(
filesystem_error ? "failed to inspect playback report target: " +
filesystem_error.message()
: "refusing to overwrite existing playback report: " +
report_path.string());
}
std::ofstream output(report_path, std::ios::out | std::ios::trunc);
if (!output) {
return common::Result<std::filesystem::path>::failure(
"failed to create trajectory playback report: " + report_path.string());
}
WriteReportJson(output, report);
output.flush();
if (!output.good()) {
return common::Result<std::filesystem::path>::failure(
"failed to write trajectory playback report: " + report_path.string());
}
output.close();
return common::Result<std::filesystem::path>::success(report_path);
}
} // namespace rm_control::modules::trajectory
RM/src/modules/trajectory/vendor_drag_trajectory_parser.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file vendor_drag_trajectory_parser.cpp
* @brief RealMan 厂商拖动轨迹 JSONL 的严格离线解析实现。
*
* @author 唐永康
* @date 2026-07-10
*/
#include "rm_control/modules/trajectory/vendor_drag_trajectory_parser.h"
#include "rm_control/common/rm75_constants.h"
#include "rm_control/modules/trajectory/sha256_file_checksum.h"
#include <array>
#include <cmath>
#include <cstdint>
#include <fstream>
#include <limits>
#include <string>
#include <string_view>
namespace rm_control::modules::trajectory {
namespace {
using RawJointPoint = std::array<std::int64_t, common::kRm75JointCount>;
constexpr std::size_t kMaximumVendorJsonLineLength = 1024U;
struct FileSnapshot {
std::uintmax_t size_bytes = 0U;
std::filesystem::file_time_type modification_time{};
};
std::string PathForError(const std::filesystem::path &path) {
return path.string();
}
bool IsJsonWhitespace(char character) {
return character == ' ' || character == '\t' || character == '\r' || character == '\n';
}
bool IsAsciiDigit(char character) {
return character >= '0' && character <= '9';
}
bool IsBlankJsonLine(std::string_view line) {
for (const char character : line) {
if (!IsJsonWhitespace(character)) {
return false;
}
}
return true;
}
class JsonLineCursor {
public:
explicit JsonLineCursor(std::string_view text) : text_(text) {}
bool Consume(char expected) {
SkipWhitespace();
if (position_ >= text_.size() || text_[position_] != expected) {
return false;
}
++position_;
return true;
}
bool ConsumeLiteral(std::string_view expected) {
SkipWhitespace();
if (text_.substr(position_, expected.size()) != expected) {
return false;
}
position_ += expected.size();
return true;
}
bool IsAtEnd() {
SkipWhitespace();
return position_ == text_.size();
}
common::Result<std::int64_t> ParseInteger() {
SkipWhitespace();
const bool is_negative = ConsumeMinusSign();
if (position_ >= text_.size() || !IsAsciiDigit(text_[position_])) {
return common::Result<std::int64_t>::failure("expected a JSON integer");
}
if (text_[position_] == '0' && position_ + 1U < text_.size() &&
IsAsciiDigit(text_[position_ + 1U])) {
return common::Result<std::int64_t>::failure(
"JSON integer must not contain a leading zero");
}
return ParseIntegerMagnitude(is_negative);
}
private:
void SkipWhitespace() {
while (position_ < text_.size() && IsJsonWhitespace(text_[position_])) {
++position_;
}
}
bool ConsumeMinusSign() {
if (position_ < text_.size() && text_[position_] == '-') {
++position_;
return true;
}
return false;
}
common::Result<std::int64_t> ParseIntegerMagnitude(bool is_negative) {
const std::uint64_t positive_limit =
static_cast<std::uint64_t>(std::numeric_limits<std::int64_t>::max());
const std::uint64_t magnitude_limit = is_negative ? positive_limit + 1U : positive_limit;
std::uint64_t magnitude = 0U;
while (position_ < text_.size() && IsAsciiDigit(text_[position_])) {
const std::uint64_t digit = static_cast<std::uint64_t>(text_[position_] - '0');
if (magnitude > (magnitude_limit - digit) / 10U) {
return common::Result<std::int64_t>::failure("JSON integer exceeds int64 range");
}
magnitude = magnitude * 10U + digit;
++position_;
}
if (is_negative && magnitude == positive_limit + 1U) {
return common::Result<std::int64_t>::success(
std::numeric_limits<std::int64_t>::min());
}
const auto signed_magnitude = static_cast<std::int64_t>(magnitude);
return common::Result<std::int64_t>::success(
is_negative ? -signed_magnitude : signed_magnitude);
}
std::string_view text_;
std::size_t position_ = 0U;
};
template <typename T>
common::Result<T> LineFailure(std::size_t line_number, const std::string &message) {
return common::Result<T>::failure(
"vendor trajectory line " + std::to_string(line_number) + ": " + message);
}
common::Result<RawJointPoint> ParseVendorPointLine(std::string_view line,
std::size_t line_number) {
JsonLineCursor cursor(line);
if (!cursor.Consume('{') || !cursor.ConsumeLiteral("\"point\"") ||
!cursor.Consume(':') || !cursor.Consume('[')) {
return LineFailure<RawJointPoint>(
line_number, "expected exact object format {\"point\":[...]}");
}
RawJointPoint point{};
for (std::size_t joint_index = 0U; joint_index < point.size(); ++joint_index) {
const auto raw_value = cursor.ParseInteger();
if (!raw_value.ok) {
return LineFailure<RawJointPoint>(
line_number, "joint " + std::to_string(joint_index + 1U) + " " +
raw_value.error_message);
}
point[joint_index] = raw_value.value;
if (joint_index + 1U < point.size() && !cursor.Consume(',')) {
return LineFailure<RawJointPoint>(line_number, "expected exactly seven integers");
}
}
if (!cursor.Consume(']') || !cursor.Consume('}') || !cursor.IsAtEnd()) {
return LineFailure<RawJointPoint>(
line_number, "expected exactly seven integers and no additional fields");
}
return common::Result<RawJointPoint>::success(point);
}
common::Result<FileSnapshot> CaptureFileSnapshot(const std::filesystem::path &file_path) {
std::error_code filesystem_error;
const std::filesystem::file_status status =
std::filesystem::symlink_status(file_path, filesystem_error);
if (filesystem_error) {
return common::Result<FileSnapshot>::failure(
"failed to inspect vendor trajectory " + PathForError(file_path) + ": " +
filesystem_error.message());
}
if (!std::filesystem::is_regular_file(status)) {
return common::Result<FileSnapshot>::failure(
"vendor trajectory must be a regular file and not a symbolic link: " +
PathForError(file_path));
}
FileSnapshot snapshot;
snapshot.size_bytes = std::filesystem::file_size(file_path, filesystem_error);
if (filesystem_error) {
return common::Result<FileSnapshot>::failure(
"failed to read vendor trajectory size " + PathForError(file_path) + ": " +
filesystem_error.message());
}
snapshot.modification_time = std::filesystem::last_write_time(file_path, filesystem_error);
if (filesystem_error) {
return common::Result<FileSnapshot>::failure(
"failed to read vendor trajectory modification time " + PathForError(file_path) +
": " + filesystem_error.message());
}
return common::Result<FileSnapshot>::success(snapshot);
}
common::Result<void> VerifyFileSnapshot(const std::filesystem::path &file_path,
const FileSnapshot &expected,
const std::string &operation) {
const auto actual = CaptureFileSnapshot(file_path);
if (!actual.ok) {
return common::Result<void>::failure(actual.error_message);
}
if (actual.value.size_bytes != expected.size_bytes ||
actual.value.modification_time != expected.modification_time) {
return common::Result<void>::failure(
"vendor trajectory changed during " + operation + ": " + PathForError(file_path));
}
return common::Result<void>::success();
}
common::Result<std::string> CalculateStableChecksum(const std::filesystem::path &file_path,
const FileSnapshot &snapshot,
const std::string &operation) {
const auto checksum = CalculateFileSha256(file_path);
if (!checksum.ok) {
return checksum;
}
const auto unchanged = VerifyFileSnapshot(file_path, snapshot, operation);
if (!unchanged.ok) {
return common::Result<std::string>::failure(unchanged.error_message);
}
return checksum;
}
common::Result<common::TrajectorySample> MakeTrajectorySample(
const RawJointPoint &raw_point,
std::size_t sample_index,
std::int64_t sample_period_ms,
std::int64_t raw_units_per_degree) {
const auto maximum_index = static_cast<std::size_t>(
std::numeric_limits<long long>::max() / sample_period_ms);
if (sample_index > maximum_index) {
return common::Result<common::TrajectorySample>::failure(
"derived timestamp exceeds int64 millisecond range");
}
common::TrajectorySample sample;
sample.sample_index = sample_index;
sample.elapsed_ms = static_cast<long long>(sample_index) * sample_period_ms;
sample.source = kRealmanVendorDragTrajectorySource;
sample.joints_deg.reserve(common::kRm75JointCount);
for (const std::int64_t raw_joint : raw_point) {
const double joint_deg = static_cast<double>(raw_joint) /
static_cast<double>(raw_units_per_degree);
if (!std::isfinite(joint_deg) ||
std::abs(joint_deg) > static_cast<double>(std::numeric_limits<float>::max())) {
return common::Result<common::TrajectorySample>::failure(
"converted joint angle exceeds finite float range");
}
sample.joints_deg.push_back(static_cast<float>(joint_deg));
}
return common::Result<common::TrajectorySample>::success(sample);
}
common::Result<std::vector<common::TrajectorySample>> ReadVendorSamples(
const std::filesystem::path &file_path,
std::int64_t sample_period_ms,
std::int64_t raw_units_per_degree) {
std::ifstream input(file_path, std::ios::binary);
if (!input) {
return common::Result<std::vector<common::TrajectorySample>>::failure(
"failed to open vendor trajectory for parsing: " + PathForError(file_path));
}
std::vector<common::TrajectorySample> samples;
std::string line;
std::size_t line_number = 0U;
while (std::getline(input, line)) {
++line_number;
if (line.size() > kMaximumVendorJsonLineLength) {
return LineFailure<std::vector<common::TrajectorySample>>(
line_number, "line exceeds the strict 1024-byte limit");
}
if (IsBlankJsonLine(line)) {
return LineFailure<std::vector<common::TrajectorySample>>(
line_number, "blank lines are not allowed");
}
const auto raw_point = ParseVendorPointLine(line, line_number);
if (!raw_point.ok) {
return common::Result<std::vector<common::TrajectorySample>>::failure(
raw_point.error_message);
}
const auto sample = MakeTrajectorySample(
raw_point.value, samples.size(), sample_period_ms, raw_units_per_degree);
if (!sample.ok) {
return LineFailure<std::vector<common::TrajectorySample>>(
line_number, sample.error_message);
}
samples.push_back(sample.value);
}
if (!input.eof()) {
return common::Result<std::vector<common::TrajectorySample>>::failure(
"failed while reading vendor trajectory: " + PathForError(file_path));
}
if (samples.empty()) {
return common::Result<std::vector<common::TrajectorySample>>::failure(
"vendor trajectory contains no samples: " + PathForError(file_path));
}
return common::Result<std::vector<common::TrajectorySample>>::success(samples);
}
common::Result<void> ValidateParseArguments(const std::filesystem::path &file_path,
std::int64_t sample_period_ms,
std::int64_t raw_units_per_degree) {
if (file_path.empty()) {
return common::Result<void>::failure("vendor trajectory path must not be empty");
}
if (sample_period_ms <= 0) {
return common::Result<void>::failure(
"vendor trajectory sample_period_ms must be greater than zero");
}
if (raw_units_per_degree <= 0) {
return common::Result<void>::failure(
"vendor trajectory raw_units_per_degree must be greater than zero");
}
return common::Result<void>::success();
}
} // namespace
common::Result<VendorDragTrajectoryParseResult> ParseVendorDragTrajectoryJsonLines(
const std::filesystem::path &file_path,
std::int64_t sample_period_ms,
std::int64_t raw_units_per_degree) {
const auto arguments =
ValidateParseArguments(file_path, sample_period_ms, raw_units_per_degree);
if (!arguments.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(
arguments.error_message);
}
const auto initial_snapshot = CaptureFileSnapshot(file_path);
if (!initial_snapshot.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(
initial_snapshot.error_message);
}
const auto checksum_before_parse = CalculateStableChecksum(
file_path, initial_snapshot.value, "initial SHA-256 calculation");
if (!checksum_before_parse.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(
checksum_before_parse.error_message);
}
const auto samples = ReadVendorSamples(file_path, sample_period_ms, raw_units_per_degree);
const auto unchanged_after_parse =
VerifyFileSnapshot(file_path, initial_snapshot.value, "JSONL parsing");
if (!unchanged_after_parse.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(
unchanged_after_parse.error_message);
}
if (!samples.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(samples.error_message);
}
const auto checksum_after_parse = CalculateStableChecksum(
file_path, initial_snapshot.value, "final SHA-256 calculation");
if (!checksum_after_parse.ok) {
return common::Result<VendorDragTrajectoryParseResult>::failure(
checksum_after_parse.error_message);
}
if (checksum_after_parse.value != checksum_before_parse.value) {
return common::Result<VendorDragTrajectoryParseResult>::failure(
"vendor trajectory content changed while it was being parsed: " +
PathForError(file_path));
}
VendorDragTrajectoryParseResult result;
result.source_file = file_path;
result.samples = samples.value;
result.trajectory_point_count = result.samples.size();
result.source_file_size_bytes = initial_snapshot.value.size_bytes;
result.source_file_sha256 = checksum_after_parse.value;
result.sample_period_ms = sample_period_ms;
result.raw_units_per_degree = raw_units_per_degree;
return common::Result<VendorDragTrajectoryParseResult>::success(result);
}
} // namespace rm_control::modules::trajectory
RM/src/o6_bus_probe_main.cpp 0 → 100644
View file @ 8275ee2e
#include "flight_robot/realman/realman_arm_adapter.h"
#include "flight_robot/linker_hand/linker_hand_adapter.h"
#include "rm_control/common/robot_types.h"
#include "rm_control/core/o6_bus_scanner.h"
#include <iomanip>
#include <iostream>
namespace {
void PrintProbeResponse(const rm_control::core::O6BusScanResponse &response) {
std::cout << "O6 candidate 0x" << std::hex << std::uppercase
<< static_cast<int>(response.device_id) << std::dec << std::nouppercase
<< ": multi=" << (response.multiple_read_succeeded ? "ok" : "failed");
if (response.multiple_read_succeeded) {
std::cout << " values=[";
for (std::size_t index = 0U; index < response.identity_registers.size(); ++index) {
std::cout << (index == 0U ? "" : ",")
<< response.identity_registers[index];
}
std::cout << ']';
} else {
std::cout << " error={" << response.multiple_read_error << '}';
}
std::cout << ", single=" << (response.single_read_succeeded ? "ok" : "failed");
if (response.single_read_succeeded) {
std::cout << " value=" << response.single_freedom_register;
} else {
std::cout << " error={" << response.single_read_error << '}';
}
std::cout << ", cross_match="
<< (response.single_and_multiple_reads_match ? "true" : "false")
<< ", confirmed_o6="
<< (response.matches_o6_identity_shape ? "true" : "false") << '\n';
}
void PrintRightHandReadiness(
const rm_control::interfaces::DexterousHandReadiness &readiness) {
std::cout << "O6 right-hand readiness confirmed: device=0x27, freedom="
<< readiness.freedom << ", direction=" << readiness.direction
<< ", version=" << readiness.hand_version << ", positions=[";
for (std::size_t index = 0U; index < readiness.joint_positions.size(); ++index) {
std::cout << (index == 0U ? "" : ",")
<< readiness.joint_positions[index];
}
std::cout << "]\n";
}
} // namespace
int main() {
flight_robot::realman::RealmanArmAdapter robot;
const auto connected = robot.connect(rm_control::common::RobotConnectionConfig{});
if (!connected.ok) {
std::cerr << "RM75 connection failed: " << connected.error_message << '\n';
return 1;
}
const auto information = robot.getRobotInfo();
if (!information.ok || information.value.controller_version != 3) {
std::cerr << "O6 probe requires a connected generation-3 RM75: "
<< (information.ok ? "unexpected controller generation"
: information.error_message) << '\n';
return 1;
}
rm_control::interfaces::ToolModbusBusConfiguration configuration;
configuration.power_cycle_tool = true;
configuration.power_off_delay_ms = 500;
configuration.startup_delay_ms = 2000;
const auto configured = robot.Configure(configuration);
if (!configured.ok) {
std::cerr << "O6 tool bus initialization failed: "
<< configured.error_message << '\n';
return 1;
}
std::cout << "Tool bus ready: port=" << configured.value.port
<< ", mode=" << configured.value.mode
<< ", baudrate=" << configured.value.baudrate
<< ", voltage_type=" << configured.value.voltage_type
<< ", timeout_units=" << configured.value.timeout_units << '\n';
const auto report = rm_control::core::ScanO6IdentityRegisters(
robot, rm_control::core::O6BusScanConfig{});
if (!report.ok) {
std::cerr << "O6 targeted probe failed: " << report.error_message << '\n';
return 1;
}
for (const auto &response : report.value.responses) {
PrintProbeResponse(response);
}
flight_robot::linker_hand::LinkerHandAdapter hand(
robot, rm_control::interfaces::DexterousHandSide::Right);
const auto readiness = hand.ReadReadiness();
if (readiness.ok) {
PrintRightHandReadiness(readiness.value);
} else {
std::cerr << "O6 single-register readiness failed: "
<< readiness.error_message << '\n';
}
std::cout << "O6 probe summary: attempted="
<< report.value.attempted_device_count
<< ", confirmed=" << report.value.confirmed_o6_device_count
<< ", unconfirmed=" << report.value.failed_device_count << '\n';
return readiness.ok ? 0 : 2;
}
RM/src/o6_grip_demo_main.cpp 0 → 100644
View file @ 8275ee2e
#include "flight_robot/linker_hand/linker_hand_adapter.h"
#include "flight_robot/realman/realman_arm_adapter.h"
#include "rm_control/common/robot_types.h"
#include <chrono>
#include <cstdlib>
#include <iostream>
#include <string>
#include <thread>
namespace {
constexpr rm_control::interfaces::O6JointPose kOpenPose{
255U, 255U, 0U, 255U, 255U, 255U};
constexpr rm_control::interfaces::O6JointPose kGripPose{
140U, 100U, 0U, 55U, 55U, 55U};
constexpr rm_control::interfaces::O6JointPose kSpeedCommands{
20U, 20U, 20U, 20U, 20U, 20U};
constexpr rm_control::interfaces::O6JointPose kTorqueCommands{
20U, 20U, 20U, 20U, 20U, 20U};
constexpr std::uint16_t kPositionTolerance = 5U;
void PrintPose(const char *label,
const rm_control::interfaces::O6JointPose &pose) {
std::cout << label << "=[";
for (std::size_t index = 0U; index < pose.size(); ++index) {
std::cout << (index == 0U ? "" : ",") << pose[index];
}
std::cout << "]\n";
}
bool PoseReached(const rm_control::interfaces::O6JointPose &actual,
const rm_control::interfaces::O6JointPose &target) {
for (std::size_t index = 0U; index < actual.size(); ++index) {
const int delta = std::abs(static_cast<int>(actual[index]) -
static_cast<int>(target[index]));
if (delta > static_cast<int>(kPositionTolerance)) {
return false;
}
}
return true;
}
rm_control::common::Result<void> InitializeToolBus(
flight_robot::realman::RealmanArmAdapter &robot) {
rm_control::interfaces::ToolModbusBusConfiguration configuration;
configuration.power_cycle_tool = false;
configuration.power_off_delay_ms = 500;
configuration.startup_delay_ms = 2000;
const auto configured = robot.Configure(configuration);
return configured.ok
? rm_control::common::Result<void>::success()
: rm_control::common::Result<void>::failure(configured.error_message);
}
rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>
WaitForGrip(flight_robot::linker_hand::LinkerHandAdapter &hand) {
const auto deadline = std::chrono::steady_clock::now() +
std::chrono::seconds(20);
while (std::chrono::steady_clock::now() < deadline) {
const auto readiness = hand.ReadReadiness();
if (!readiness.ok || PoseReached(readiness.value.joint_positions, kGripPose)) {
return readiness;
}
PrintPose("O6 moving", readiness.value.joint_positions);
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
return rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::failure(
"O6 did not reach the conservative grip pose within 20 seconds");
}
rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>
WaitForOpen(flight_robot::linker_hand::LinkerHandAdapter &hand) {
const auto deadline = std::chrono::steady_clock::now() +
std::chrono::seconds(20);
while (std::chrono::steady_clock::now() < deadline) {
const auto readiness = hand.ReadReadiness();
if (!readiness.ok || PoseReached(readiness.value.joint_positions, kOpenPose)) {
return readiness;
}
PrintPose("O6 opening", readiness.value.joint_positions);
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
return rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::failure(
"O6 did not reach the open byte-order probe within 20 seconds");
}
rm_control::common::Result<void> CommandPose(
flight_robot::linker_hand::LinkerHandAdapter &hand,
const rm_control::interfaces::O6JointPose &pose) {
return hand.CommandMotion(
rm_control::interfaces::O6MotionCommand{
pose, kSpeedCommands, kTorqueCommands});
}
} // namespace
int main(int argc, char **argv) {
if (argc != 2 || std::string(argv[1]) != "--live-grip") {
std::cerr << "Usage: " << (argc > 0 ? argv[0] : "rm_o6_grip_demo")
<< " --live-grip\n";
return 64;
}
flight_robot::realman::RealmanArmAdapter robot;
const auto connected = robot.connect(rm_control::common::RobotConnectionConfig{});
const auto initialized = connected.ok
? InitializeToolBus(robot)
: rm_control::common::Result<void>::failure(connected.error_message);
if (!initialized.ok) {
std::cerr << "O6 grip demo initialization failed: "
<< initialized.error_message << '\n';
return 1;
}
flight_robot::linker_hand::LinkerHandAdapter hand(
robot, rm_control::interfaces::DexterousHandSide::Right);
const auto before = hand.ReadReadiness();
if (!before.ok) {
std::cerr << "O6 pre-grip readiness failed: " << before.error_message << '\n';
return 2;
}
PrintPose("O6 before grip", before.value.joint_positions);
const auto opened_command = CommandPose(hand, kOpenPose);
const auto opened = opened_command.ok
? WaitForOpen(hand)
: rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::failure(
opened_command.error_message);
if (!opened.ok) {
std::cerr << "O6 safe open probe failed: " << opened.error_message << '\n';
return 3;
}
PrintPose("O6 open reached", opened.value.joint_positions);
std::this_thread::sleep_for(std::chrono::seconds(1));
const auto commanded = CommandPose(hand, kGripPose);
if (!commanded.ok) {
std::cerr << "O6 grip command failed: " << commanded.error_message << '\n';
return 4;
}
const auto after = WaitForGrip(hand);
if (!after.ok) {
std::cerr << "O6 grip verification failed: " << after.error_message << '\n';
return 5;
}
PrintPose("O6 grip reached", after.value.joint_positions);
return 0;
}
RM/src/trajectory_convert_main.cpp
View file @ 8275ee2e
/**
* @file trajectory_convert_main.cpp
* @brief RM75 轨迹 CSV 转 C++ waypoint 工具入口文件。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件是离线轨迹转换工具入口，只负责 CLI 参数解析结果处理、调用 trajectory
* 模块读取 CSV、导出 C++ waypoint 文件并返回退出码。它不连接真实机械臂，
* 也不执行任何运动。
*
* 设计原则：
* - 参数解析放在 config 层；
* - CSV 读取和 C++ 文件生成放在 modules/trajectory 层；
* - `main()` 不做字符串路径拆解；
* - 所有失败路径输出明确错误。
*
* @dependencies
* - rm_control/config/trajectory_convert_cli_config.h
* - rm_control/modules/trajectory/trajectory_file_store.h
*/
#include "rm_control/config/trajectory_convert_cli_config.h"
#include "rm_control/modules/trajectory/trajectory_file_store.h" #include "rm_control/modules/trajectory/trajectory_file_store.h"
#include <iostream> #include <iostream>
#include <string>
namespace {
void printUsage(const char *program_name) {
std::cout << "Usage: " << program_name << " <input_csv> [output_cpp]\n"
<< "\n"
<< "Converts recorded CSV into a C++ TCP waypoint table. It does not connect to the robot.\n";
}
std::string fileStem(const std::string &path) {
const size_t slash = path.find_last_of('/');
const size_t begin = slash == std::string::npos ? 0 : slash + 1;
const size_t dot = path.find_last_of('.');
const size_t end = dot == std::string::npos || dot < begin ? path.size() : dot;
return path.substr(begin, end - begin);
}
std::string defaultOutputPath(const std::string &input_csv) {
const std::string session_name = rm_control::modules::trajectory::sanitizeSessionName(fileStem(input_csv));
auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(
rm_control::modules::trajectory::defaultTrajectoryStorageRoot(), session_name);
if (!files.ok) {
return session_name + "_tcp_path.cpp";
}
return files.value.converted_cpp_path;
}
} // namespace
/**
* @brief 启动离线轨迹转换工具。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该函数先解析 CLI 参数，再加载轨迹 CSV，最后导出 C++ TCP waypoint 表。
* 由于该工具只处理文件，不连接真实机械臂，因此可作为轨迹录制后的离线验证步骤。
*
* @param[in] argc 命令行参数数量。
* @param[in] argv 命令行参数数组。
*
* @return 0 转换成功或帮助信息输出成功。
* @return 1 CSV 加载失败。
* @return 2 C++ 文件导出失败。
* @return 64 命令行参数错误。
*
* @throws 本函数不主动抛出异常；底层模块通过 `Result` 返回错误。
*
* @note
* 输入 CSV 文件必须符合 trajectory 模块写出的固定列格式。
*
* @warning
* 本函数不会验证 waypoint 是否适合真实运动；后续执行轨迹前必须另做安全检查。
*
* @thread_safety
* 按主线程入口函数设计，不提供多线程重入保证。
*
* @par 边界条件
* 路径为空、文件不存在、CSV 为空、输出目录无权限都会返回非零退出码。
*
* @par 后续维护
* 新增输出格式时应扩展 CLI 配置和 trajectory 模块，不要在入口文件写分支逻辑。
*/
int main(int argc, char **argv) { int main(int argc, char **argv) {
if (argc < 2 || argc > 3) { const auto cli_config = rm_control::config::ParseTrajectoryConvertCliArguments(argc, argv);
printUsage(argv[0]); if (!cli_config.ok) {
std::cerr << "Invalid arguments: " << cli_config.error_message << '\n';
rm_control::config::PrintTrajectoryConvertUsage(argv == nullptr ? nullptr : argv[0], std::cerr);
return 64; return 64;
} }
const std::string first_arg = argv[1] == nullptr ? "" : argv[1]; if (cli_config.value.help_requested) {
if (first_arg == "-h" || first_arg == "--help") { rm_control::config::PrintTrajectoryConvertUsage(argv == nullptr ? nullptr : argv[0], std::cout);
printUsage(argv[0]);
return 0; return 0;
} }
const std::string input_csv = first_arg; const auto samples = rm_control::modules::trajectory::loadTrajectoryCsv(
const std::string output_cpp = argc == 3 ? std::string(argv[2]) : defaultOutputPath(input_csv); cli_config.value.input_csv_path);
auto samples = rm_control::modules::trajectory::loadTrajectoryCsv(input_csv);
if (!samples.ok) { if (!samples.ok) {
std::cerr << "Failed to load CSV: " << samples.error_message << '\n'; std::cerr << "Failed to load CSV: " << samples.error_message << '\n';
return 1; return 1;
} }
const std::string session_name = rm_control::modules::trajectory::sanitizeSessionName(fileStem(input_csv)); const auto export_result = rm_control::modules::trajectory::exportTcpPathCpp(
auto export_result = rm_control::modules::trajectory::exportTcpPathCpp( cli_config.value.output_cpp_path, samples.value, cli_config.value.session_name);
output_cpp, samples.value, session_name);
if (!export_result.ok) { if (!export_result.ok) {
std::cerr << "Failed to export C++: " << export_result.error_message << '\n'; std::cerr << "Failed to export C++: " << export_result.error_message << '\n';
return 2; return 2;
} }
std::cout << "Loaded samples: " << samples.value.size() << '\n' std::cout << "Loaded samples: " << samples.value.size() << '\n'
<< "C++ TCP path: " << output_cpp << '\n'; << "C++ TCP path: " << cli_config.value.output_cpp_path << '\n';
return 0; return 0;
} }
RM/src/trajectory_o6_grasp_main.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file trajectory_o6_grasp_main.cpp
* @brief RM75 本地拖动轨迹执行与 LinkerHand O6 夹握任务入口。
*
* @author 唐永康
* @date 2026-07-11
*
* @details
* Application 只负责命令行、只读配置、依赖组装、DryRun 报告和人工二次确认。
* 七轴解析/校验、任务状态机、O6 协议和 RM_API2 分别留在各自模块与适配器中。
*/
#include "flight_robot/linker_hand/linker_hand_adapter.h"
#include "flight_robot/realman/realman_arm_adapter.h"
#include "rm_control/config/drag_teach_safety_config.h"
#include "rm_control/config/trajectory_o6_task_cli_config.h"
#include "rm_control/core/arm_o6_trajectory_task.h"
#include "rm_control/core/hardware_execution_gate.h"
#include "rm_control/modules/trajectory/drag_trajectory_validator.h"
#include "rm_control/modules/trajectory/reversed_vendor_trajectory_store.h"
#include "rm_control/modules/trajectory/trajectory_file_store.h"
#include "rm_control/modules/trajectory/trajectory_playback_report_store.h"
#include "rm_control/modules/trajectory/vendor_drag_trajectory_parser.h"
#include <algorithm>
#include <array>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <csignal>
#include <filesystem>
#include <iomanip>
#include <iostream>
#include <limits>
#include <string>
#include <vector>
namespace {
constexpr int kInvalidArgumentsExitCode = 64;
constexpr int kInvalidInputExitCode = 65;
constexpr int kHardwareExecutionRefusedExitCode = 77;
constexpr int kConnectionFailureExitCode = 1;
constexpr int kTaskFailureExitCode = 2;
constexpr long long kMinimumProgramTimeoutMilliseconds = 60000;
constexpr long long kMaximumProgramTimeoutMilliseconds = 7200000;
constexpr long long kProgramTimeoutMarginMilliseconds = 60000;
constexpr const char *kMissingOperatorConfirmationReason =
"operator second confirmation is missing";
struct OfflineTaskPreparation {
rm_control::modules::trajectory::VendorDragTrajectoryParseResult trajectory;
rm_control::modules::trajectory::VendorDragTrajectoryParseResult return_trajectory;
rm_control::modules::trajectory::DragTrajectoryValidationReport validation;
rm_control::modules::trajectory::DragTrajectoryValidationReport return_validation;
rm_control::core::ArmO6TrajectoryTaskConfig task_config;
bool return_from_trajectory_end = false;
};
volatile std::sig_atomic_t g_stop_requested = 0;
void HandleStopSignal(int) {
g_stop_requested = 1;
}
rm_control::common::Result<void> InstallStopSignalHandlers() {
g_stop_requested = 0;
if (std::signal(SIGINT, HandleStopSignal) == SIG_ERR ||
std::signal(SIGTERM, HandleStopSignal) == SIG_ERR) {
return rm_control::common::Result<void>::failure(
"failed to install SIGINT/SIGTERM stop handlers");
}
return rm_control::common::Result<void>::success();
}
bool StopWasRequestedByProcessSignal() {
return g_stop_requested != 0;
}
rm_control::modules::trajectory::DragTrajectoryValidationLimits
BuildValidationLimits(const rm_control::config::DragTeachSafetyConfig &safety) {
rm_control::modules::trajectory::DragTrajectoryValidationLimits limits;
limits.maximum_joint_jump_deg = safety.max_joint_jump_deg;
limits.maximum_joint_velocity_deg_per_second =
safety.max_joint_velocity_deg_per_second;
limits.maximum_joint_acceleration_deg_per_second_squared =
safety.max_joint_acceleration_deg_per_second_squared;
return limits;
}
std::filesystem::path BuildAbsoluteTrajectoryPath(
const std::filesystem::path &input_path,
std::error_code *error) {
const auto absolute_path = std::filesystem::absolute(input_path, *error);
return *error ? std::filesystem::path{} : absolute_path.lexically_normal();
}
void PrintJointValues(const std::vector<float> &joints_deg) {
std::cout << '[';
for (std::size_t index = 0; index < joints_deg.size(); ++index) {
std::cout << (index == 0U ? "" : ", ") << joints_deg[index];
}
std::cout << ']';
}
void PrintOfflineReport(
const char *trajectory_label,
const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
const rm_control::modules::trajectory::DragTrajectoryValidationReport &validation) {
std::cout << std::fixed << std::setprecision(3)
<< trajectory_label << " file: " << trajectory.source_file << '\n'
<< "Trajectory points: " << trajectory.trajectory_point_count << '\n'
<< "Sample period: " << trajectory.sample_period_ms << " ms (explicit)\n"
<< "Timestamp derived: "
<< (trajectory.timestamp_is_derived ? "true" : "false") << '\n'
<< "Raw units per degree: " << trajectory.raw_units_per_degree << '\n'
<< "Duration: " << validation.duration_ms << " ms\n"
<< "Source bytes: " << trajectory.source_file_size_bytes << '\n'
<< "SHA-256: " << trajectory.source_file_sha256 << '\n'
<< "Maximum jump: "
<< validation.observed_maximum_joint_jump_deg << " deg\n"
<< "Maximum velocity: "
<< validation.observed_maximum_joint_velocity_deg_per_second
<< " deg/s\n"
<< "Maximum acceleration: "
<< validation.observed_maximum_joint_acceleration_deg_per_second_squared
<< " deg/s^2\n"
<< "First RM75 joints (deg): ";
PrintJointValues(trajectory.samples.front().joints_deg);
std::cout << "\nLast RM75 joints (deg): ";
PrintJointValues(trajectory.samples.back().joints_deg);
std::cout << '\n';
}
rm_control::interfaces::DexterousHandSide ConvertHandSide(
rm_control::config::O6HandSide side) {
return side == rm_control::config::O6HandSide::Right
? rm_control::interfaces::DexterousHandSide::Right
: rm_control::interfaces::DexterousHandSide::Left;
}
template <typename SourceArray>
rm_control::interfaces::O6JointPose BuildO6ProtocolValues(
const SourceArray &source) {
rm_control::interfaces::O6JointPose values{};
std::transform(source.begin(), source.end(), values.begin(),
[](std::uint8_t value) {
return static_cast<std::uint16_t>(value);
});
return values;
}
std::chrono::milliseconds EstimateProgramTimeout(
long long source_duration_ms,
int plan_speed_percent) {
const long double scaled_duration =
static_cast<long double>(source_duration_ms) * 100.0L /
static_cast<long double>(plan_speed_percent);
const long double with_margin =
scaled_duration + static_cast<long double>(kProgramTimeoutMarginMilliseconds);
const long double bounded = std::clamp(
with_margin,
static_cast<long double>(kMinimumProgramTimeoutMilliseconds),
static_cast<long double>(kMaximumProgramTimeoutMilliseconds));
return std::chrono::milliseconds(static_cast<long long>(bounded));
}
rm_control::core::ArmO6TrajectoryTaskConfig BuildTaskConfig(
const rm_control::config::TrajectoryO6TaskCliConfig &cli,
const rm_control::config::DragTeachSafetyConfig &safety,
long long trajectory_duration_ms) {
rm_control::core::ArmO6TrajectoryTaskConfig config;
config.validation_limits = BuildValidationLimits(safety);
config.origin_speed_percent = cli.origin_speed_percent;
config.plan_speed_percent = cli.plan_speed_percent;
config.controller_program_id = cli.program_id;
config.return_controller_program_id = cli.return_program_id;
config.grip_pose = BuildO6ProtocolValues(cli.grip_pose);
config.grip_speed_commands = BuildO6ProtocolValues(cli.grip_speed);
config.grip_torque_commands = BuildO6ProtocolValues(cli.grip_torque);
config.maximum_o6_speed_command = safety.maximum_o6_speed_command;
config.maximum_o6_torque_command = safety.maximum_o6_torque_command;
config.maximum_origin_delta_deg = safety.maximum_origin_move_delta_deg;
config.maximum_absolute_force_newton = safety.maximum_absolute_force_newton;
config.maximum_absolute_torque_newton_meter =
safety.maximum_absolute_torque_newton_meter;
config.program_execution_timeout =
EstimateProgramTimeout(trajectory_duration_ms, cli.plan_speed_percent);
config.stop_requested = StopWasRequestedByProcessSignal;
return config;
}
rm_control::core::HardwareExecutionGateInput BuildExecutionGate(
const rm_control::config::TrajectoryO6TaskCliConfig &cli,
const rm_control::config::DragTeachSafetyConfig &safety,
bool arm_connected,
bool hand_connected,
bool operator_confirmed) {
rm_control::core::HardwareExecutionGateInput gate;
gate.live_requested = cli.live_requested;
gate.hardware_execution_enabled = safety.hardware_execution_enabled;
gate.arm_connected = arm_connected;
gate.hand_connected = hand_connected;
gate.emergency_stop_verified = safety.emergency_stop_verified;
gate.safety_limits_loaded = safety.safety_limits_loaded;
gate.operator_confirmed = operator_confirmed;
gate.trajectory_validated = true;
gate.trajectory_sample_period_confirmed =
safety.trajectory_sample_period_confirmed;
gate.vendor_raw_units_per_degree_confirmed =
safety.vendor_raw_units_per_degree_confirmed;
gate.workspace_and_virtual_wall_verified =
safety.workspace_and_virtual_wall_verified;
gate.configured_max_speed_percent = safety.max_speed_percent;
return gate;
}
std::string BuildConfirmationPhrase(const std::string &forward_sha256,
const std::string &return_sha256) {
std::string phrase = "CONFIRM RM75 O6 " + forward_sha256.substr(
0U, std::min<std::size_t>(12U, forward_sha256.size()));
if (!return_sha256.empty()) {
phrase += " RETURN " + return_sha256.substr(
0U, std::min<std::size_t>(12U, return_sha256.size()));
}
return phrase;
}
bool RequestOperatorConfirmation(const std::string &phrase) {
std::cout << "Type exactly '" << phrase
<< "' to authorize the displayed RM75 trajectory and O6 grip: "
<< std::flush;
std::string confirmation;
return std::getline(std::cin, confirmation) && confirmation == phrase;
}
bool PrintPreConfirmationRejections(
const rm_control::core::HardwareExecutionGateDecision &decision) {
bool rejected = false;
for (const auto &reason : decision.rejection_reasons) {
if (reason == kMissingOperatorConfirmationReason) {
continue;
}
rejected = true;
std::cerr << "Hardware execution refused: " << reason << '\n';
}
return rejected;
}
void PrintO6Readiness(
const rm_control::interfaces::DexterousHandReadiness &readiness) {
std::cout << "O6 readiness: ready=true, device_id=0x" << std::hex
<< static_cast<int>(readiness.device_id) << std::dec
<< ", freedom=" << readiness.freedom
<< ", direction=" << readiness.direction
<< ", hand_version=" << readiness.hand_version << '\n'
<< "O6 current positions: ";
std::vector<float> positions(readiness.joint_positions.begin(),
readiness.joint_positions.end());
PrintJointValues(positions);
std::cout << '\n';
}
void PrintO6ProtocolValues(const char *label,
const rm_control::interfaces::O6JointPose &values) {
std::cout << label << ": [";
for (std::size_t index = 0U; index < values.size(); ++index) {
std::cout << (index == 0U ? "" : ", ") << values[index];
}
std::cout << "]\n";
}
rm_control::common::Result<void> InitializeO6ToolModbusSession(
flight_robot::realman::RealmanArmAdapter &robot) {
rm_control::interfaces::ToolModbusBusConfiguration configuration;
configuration.port = 1;
configuration.baudrate = 115200;
configuration.voltage_type = 3;
configuration.timeout_units = 10;
configuration.reset_existing_mode = true;
configuration.power_cycle_tool = true;
configuration.power_off_delay_ms = 500;
configuration.startup_delay_ms = 2000;
const auto configured = robot.Configure(configuration);
if (!configured.ok) {
return rm_control::common::Result<void>::failure(
"failed to initialize O6 tool Modbus session: " +
configured.error_message);
}
std::cout << "O6 tool Modbus initialized: port="
<< configured.value.port << ", mode=" << configured.value.mode
<< ", baudrate=" << configured.value.baudrate
<< ", voltage_type=" << configured.value.voltage_type
<< ", timeout_units=" << configured.value.timeout_units << '\n';
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> ValidateDisplayedRobotSafety(
const rm_control::common::RobotSafetyStatus &status) {
if (!status.real_mode_enabled || !status.arm_power_on ||
!status.all_joints_enabled || !status.arm_error_free ||
!status.controller_error_free || status.controller_error_code != 0) {
return rm_control::common::Result<void>::failure(
"RM75 safety state rejected: real_mode=" +
std::to_string(status.real_mode_enabled) + ", arm_power=" +
std::to_string(status.arm_power_on) + ", joints_enabled=" +
std::to_string(status.all_joints_enabled) + ", arm_error_free=" +
std::to_string(status.arm_error_free) + ", controller_error_free=" +
std::to_string(status.controller_error_free) + ", controller_error_code=" +
std::to_string(status.controller_error_code) + ", virtual_wall=" +
std::to_string(status.virtual_wall_enabled) + ", self_collision=" +
std::to_string(status.self_collision_enabled));
}
if (status.emergency_stop_state_available && !status.emergency_stop_normal) {
return rm_control::common::Result<void>::failure(
"RM75 reports an abnormal emergency-stop state");
}
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> ValidateDisplayedForce(
const rm_control::common::ForceData &force,
const rm_control::config::DragTeachSafetyConfig &safety) {
for (std::size_t index = 0; index < force.zero.size(); ++index) {
const double value = std::fabs(static_cast<double>(force.zero[index]));
const double limit = index < 3U
? safety.maximum_absolute_force_newton
: safety.maximum_absolute_torque_newton_meter;
if (!std::isfinite(value) || value > limit) {
return rm_control::common::Result<void>::failure(
"RM75 six-axis force/torque exceeds the configured preflight limit");
}
}
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> VerifyLiveReadOnlyState(
flight_robot::realman::RealmanArmAdapter &robot,
flight_robot::linker_hand::LinkerHandAdapter &hand,
const rm_control::config::DragTeachSafetyConfig &safety_config,
const rm_control::config::TrajectoryO6TaskCliConfig &cli) {
const auto info = robot.getRobotInfo();
const auto software = robot.getRobotSoftwareInfo();
const auto current = robot.getCurrentArmState();
const auto safety = robot.getRobotSafetyStatus();
const auto limits = robot.getJointLimits();
const auto force = robot.getForceData();
const auto program_slot =
robot.isControllerProgramSlotAvailable(cli.program_id);
auto return_program_slot = rm_control::common::Result<bool>::success(true);
if (cli.return_from_trajectory_end) {
return_program_slot =
robot.isControllerProgramSlotAvailable(cli.return_program_id);
}
const auto readiness = hand.ReadReadiness();
if (!info.ok || !software.ok || !current.ok || !safety.ok || !limits.ok ||
!force.ok || !program_slot.ok || !return_program_slot.ok || !readiness.ok) {
const std::string error = !info.ok ? info.error_message
: !software.ok ? software.error_message
: !current.ok ? current.error_message
: !safety.ok ? safety.error_message
: !limits.ok ? limits.error_message
: !force.ok ? force.error_message
: !program_slot.ok ? program_slot.error_message
: !return_program_slot.ok
? return_program_slot.error_message
: readiness.error_message;
return rm_control::common::Result<void>::failure(error);
}
const auto safety_validation = ValidateDisplayedRobotSafety(safety.value);
const auto force_validation = ValidateDisplayedForce(force.value, safety_config);
if (!safety_validation.ok || !force_validation.ok || !program_slot.value ||
!return_program_slot.value) {
return rm_control::common::Result<void>::failure(
!safety_validation.ok ? safety_validation.error_message
: !force_validation.ok ? force_validation.error_message
: !program_slot.value
? "forward controller program slot is occupied; choose an unused --program-id"
: "return controller program slot is occupied; choose an unused --return-program-id");
}
std::cout << "RM75 confirmed: dof=" << info.value.arm_dof
<< ", SDK=" << software.value.sdk_version
<< ", firmware=" << software.value.firmware_version << '\n'
<< "Current RM75 joints (deg): ";
PrintJointValues(current.value.joints_deg);
std::cout << '\n';
PrintO6ProtocolValues("O6 current torque values", readiness.value.joint_torques);
PrintO6ProtocolValues("O6 current speed values", readiness.value.joint_speeds);
PrintO6Readiness(readiness.value);
return rm_control::common::Result<void>::success();
}
rm_control::modules::trajectory::PlaybackReportO6Values ToPlaybackReportValues(
const rm_control::interfaces::O6JointPose &values) {
rm_control::modules::trajectory::PlaybackReportO6Values output{};
std::copy(values.begin(), values.end(), output.begin());
return output;
}
rm_control::modules::trajectory::ControllerManagedPlaybackReport BuildPlaybackReport(
const rm_control::config::TrajectoryO6TaskCliConfig &cli,
const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
const rm_control::modules::trajectory::DragTrajectoryValidationReport &validation,
const rm_control::core::ArmO6TrajectoryTaskReport &task_report) {
rm_control::modules::trajectory::ControllerManagedPlaybackReport report;
report.report_identifier =
rm_control::modules::trajectory::timestampString();
report.source_file = trajectory.source_file;
report.source_file_sha256 = trajectory.source_file_sha256;
report.trajectory_point_count = trajectory.trajectory_point_count;
report.sample_period_ms = trajectory.sample_period_ms;
report.source_duration_ms = validation.duration_ms;
report.timestamp_is_derived = trajectory.timestamp_is_derived;
report.plan_speed_percent = cli.plan_speed_percent;
report.controller_program_id = cli.program_id;
report.returned_from_trajectory_end = task_report.returned_from_trajectory_end;
if (task_report.returned_from_trajectory_end) {
report.return_source_file = return_trajectory.source_file;
report.return_source_file_sha256 = return_trajectory.source_file_sha256;
report.return_controller_program_id = cli.return_program_id;
report.return_observed_execution_duration_ms =
task_report.return_controller_timing.observed_execution_duration_ms;
report.return_program_status_poll_count =
task_report.return_controller_timing.poll_count;
report.return_minimum_poll_period_ms =
task_report.return_controller_timing.minimum_poll_period_ms;
report.return_maximum_poll_period_ms =
task_report.return_controller_timing.maximum_poll_period_ms;
report.return_average_poll_period_ms =
task_report.return_controller_timing.average_poll_period_ms;
report.return_maximum_absolute_poll_jitter_ms =
task_report.return_controller_timing.maximum_absolute_poll_jitter_ms;
report.return_final_controller_state =
task_report.return_final_program_state.raw_state;
}
report.observed_execution_duration_ms =
task_report.controller_timing.observed_execution_duration_ms;
report.program_status_poll_count = task_report.controller_timing.poll_count;
report.minimum_poll_period_ms = task_report.controller_timing.minimum_poll_period_ms;
report.maximum_poll_period_ms = task_report.controller_timing.maximum_poll_period_ms;
report.average_poll_period_ms = task_report.controller_timing.average_poll_period_ms;
report.maximum_absolute_poll_jitter_ms =
task_report.controller_timing.maximum_absolute_poll_jitter_ms;
report.final_controller_state = task_report.final_program_state.raw_state;
report.o6_device_id = task_report.final_hand_readiness.device_id;
report.target_positions = ToPlaybackReportValues(BuildO6ProtocolValues(cli.grip_pose));
report.speed_commands = ToPlaybackReportValues(BuildO6ProtocolValues(cli.grip_speed));
report.torque_commands = ToPlaybackReportValues(BuildO6ProtocolValues(cli.grip_torque));
report.final_positions =
ToPlaybackReportValues(task_report.final_hand_readiness.joint_positions);
return report;
}
void PrintTaskSuccess(const rm_control::core::ArmO6TrajectoryTaskReport &report,
const std::filesystem::path &report_path) {
std::cout << "Task completed: trajectory_points=" << report.trajectory_point_count
<< ", returned_from_trajectory_end="
<< (report.returned_from_trajectory_end ? "true" : "false");
if (report.returned_from_trajectory_end) {
std::cout << ", return_program_state="
<< report.return_final_program_state.raw_state
<< ", return_program_status_polls="
<< report.return_controller_timing.poll_count
<< ", return_execution_duration_ms="
<< report.return_controller_timing.observed_execution_duration_ms;
}
std::cout
<< ", observed_running_state="
<< (report.observed_running_state ? "true" : "false")
<< ", final_program_state=" << report.final_program_state.raw_state
<< ", program_status_polls=" << report.controller_timing.poll_count
<< ", average_poll_period_ms="
<< report.controller_timing.average_poll_period_ms
<< ", maximum_poll_period_ms="
<< report.controller_timing.maximum_poll_period_ms
<< ", maximum_poll_jitter_ms="
<< report.controller_timing.maximum_absolute_poll_jitter_ms
<< ", observed_execution_duration_ms="
<< report.controller_timing.observed_execution_duration_ms
<< "\nFinal O6 positions: ";
std::vector<float> positions(report.final_hand_readiness.joint_positions.begin(),
report.final_hand_readiness.joint_positions.end());
PrintJointValues(positions);
std::cout << "\nPlayback report: " << report_path << '\n';
}
void PrintRequestedO6Motion(
const rm_control::config::TrajectoryO6TaskCliConfig &cli,
const rm_control::config::DragTeachSafetyConfig &safety) {
PrintO6ProtocolValues(
"Requested O6 target positions", BuildO6ProtocolValues(cli.grip_pose));
PrintO6ProtocolValues(
"Requested O6 speed commands", BuildO6ProtocolValues(cli.grip_speed));
PrintO6ProtocolValues(
"Requested O6 torque commands", BuildO6ProtocolValues(cli.grip_torque));
std::cout << "Configured O6 maximum speed command: "
<< safety.maximum_o6_speed_command << '\n'
<< "Configured O6 maximum torque command: "
<< safety.maximum_o6_torque_command << '\n';
}
rm_control::common::Result<rm_control::core::HardwareExecutionGateInput>
ConfirmLiveExecution(
flight_robot::realman::RealmanArmAdapter &robot,
flight_robot::linker_hand::LinkerHandAdapter &hand,
const rm_control::config::TrajectoryO6TaskCliConfig &cli,
const rm_control::config::DragTeachSafetyConfig &safety,
const std::string &forward_source_sha256,
const std::string &return_source_sha256) {
const auto read_only_state = VerifyLiveReadOnlyState(
robot, hand, safety, cli);
if (!read_only_state.ok) {
return rm_control::common::Result<
rm_control::core::HardwareExecutionGateInput>::failure(
"live readiness failed before motion: " +
read_only_state.error_message);
}
auto gate = BuildExecutionGate(
cli, safety, robot.isConnected(), true, false);
rm_control::core::HardwareExecutionGateRequirements requirements;
requirements.required_speed_percent =
std::max(cli.origin_speed_percent, cli.plan_speed_percent);
requirements.require_confirmed_trajectory_sample_period = true;
requirements.require_confirmed_vendor_raw_units_per_degree = true;
const auto preliminary =
rm_control::core::EvaluateHardwareExecutionGate(gate, requirements);
if (PrintPreConfirmationRejections(preliminary)) {
return rm_control::common::Result<
rm_control::core::HardwareExecutionGateInput>::failure(
"pre-confirmation hardware safety gate rejected execution");
}
if (StopWasRequestedByProcessSignal()) {
return rm_control::common::Result<
rm_control::core::HardwareExecutionGateInput>::failure(
"stop requested before operator confirmation");
}
gate.operator_confirmed = RequestOperatorConfirmation(
BuildConfirmationPhrase(forward_source_sha256, return_source_sha256));
if (!gate.operator_confirmed) {
return rm_control::common::Result<
rm_control::core::HardwareExecutionGateInput>::failure(
"operator confirmation mismatch");
}
return rm_control::common::Result<
rm_control::core::HardwareExecutionGateInput>::success(gate);
}
int ExecuteConfirmedTask(
flight_robot::realman::RealmanArmAdapter &robot,
flight_robot::linker_hand::LinkerHandAdapter &hand,
const rm_control::config::TrajectoryO6TaskCliConfig &cli,
const OfflineTaskPreparation &preparation,
const rm_control::core::HardwareExecutionGateInput &gate) {
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = cli.return_from_trajectory_end
? task.RunFromTrajectoryEnd(
preparation.return_trajectory, preparation.trajectory,
preparation.task_config, gate)
: task.Run(preparation.trajectory, preparation.task_config, gate);
if (!result.ok) {
std::cerr << "RM75/O6 task failed: " << result.error_message << '\n';
return kTaskFailureExitCode;
}
const auto playback_report = BuildPlaybackReport(
cli, preparation.trajectory, preparation.return_trajectory,
preparation.validation, result.value);
const auto report_path =
rm_control::modules::trajectory::
WriteControllerManagedPlaybackReportWithoutOverwrite(
playback_report,
rm_control::modules::trajectory::
DefaultTrajectoryPlaybackReportDirectory());
if (!report_path.ok) {
std::cerr << "RM75/O6 task completed, but report persistence failed: "
<< report_path.error_message << '\n';
return kTaskFailureExitCode;
}
PrintTaskSuccess(result.value, report_path.value);
return 0;
}
int RunLiveTask(
const rm_control::config::TrajectoryO6TaskCliConfig &cli,
const rm_control::config::DragTeachSafetyConfig &safety,
const OfflineTaskPreparation &preparation) {
const auto source_integrity =
rm_control::core::VerifySourceFileIntegrity(preparation.trajectory);
const auto return_integrity = !preparation.return_from_trajectory_end
? rm_control::common::Result<void>::success()
: rm_control::core::VerifySourceFileIntegrity(
preparation.return_trajectory);
if (!source_integrity.ok || !return_integrity.ok) {
std::cerr << "Hardware execution refused before connection: "
<< (!source_integrity.ok ? source_integrity.error_message
: return_integrity.error_message)
<< '\n';
return kHardwareExecutionRefusedExitCode;
}
if (!safety.hardware_execution_enabled) {
std::cerr << "Hardware execution refused: hardware_execution_enabled is false.\n";
return kHardwareExecutionRefusedExitCode;
}
const auto signals = InstallStopSignalHandlers();
if (!signals.ok) {
std::cerr << "Hardware execution refused: " << signals.error_message << '\n';
return kHardwareExecutionRefusedExitCode;
}
flight_robot::realman::RealmanArmAdapter robot;
const auto connected = robot.connect(rm_control::common::RobotConnectionConfig{});
if (!connected.ok) {
std::cerr << "Failed to connect RM75: " << connected.error_message << '\n';
return kConnectionFailureExitCode;
}
const auto tool_bus = InitializeO6ToolModbusSession(robot);
if (!tool_bus.ok) {
std::cerr << "Hardware execution refused: "
<< tool_bus.error_message << '\n';
return kHardwareExecutionRefusedExitCode;
}
flight_robot::linker_hand::LinkerHandAdapter hand(
robot, ConvertHandSide(cli.o6_side));
const auto gate = ConfirmLiveExecution(
robot, hand, cli, safety, preparation.trajectory.source_file_sha256,
preparation.return_from_trajectory_end
? preparation.return_trajectory.source_file_sha256
: std::string{});
if (!gate.ok) {
std::cerr << "Hardware execution refused: " << gate.error_message << '\n';
return kHardwareExecutionRefusedExitCode;
}
return ExecuteConfirmedTask(
robot, hand, cli, preparation, gate.value);
}
rm_control::common::Result<OfflineTaskPreparation> PrepareOfflineTask(
const rm_control::config::TrajectoryO6TaskCliConfig &cli,
const rm_control::config::DragTeachSafetyConfig &safety) {
std::error_code path_error;
const auto trajectory_path = BuildAbsoluteTrajectoryPath(
cli.trajectory_path, &path_error);
if (path_error) {
return rm_control::common::Result<OfflineTaskPreparation>::failure(
"failed to resolve trajectory path: " + path_error.message());
}
const auto trajectory =
rm_control::modules::trajectory::ParseVendorDragTrajectoryJsonLines(
trajectory_path, cli.sample_period_ms,
safety.vendor_raw_units_per_degree);
if (!trajectory.ok) {
return rm_control::common::Result<OfflineTaskPreparation>::failure(
"trajectory parse failed: " + trajectory.error_message);
}
const auto validation = rm_control::modules::trajectory::ValidateDragTrajectory(
trajectory.value.samples, BuildValidationLimits(safety));
if (!validation.ok) {
return rm_control::common::Result<OfflineTaskPreparation>::failure(
"trajectory validation failed: " + validation.error_message);
}
const auto task_config = BuildTaskConfig(
cli, safety, validation.value.duration_ms);
const auto config_validation = cli.return_from_trajectory_end
? rm_control::core::ValidateArmO6RoundTripTaskConfig(task_config)
: rm_control::core::ValidateArmO6TrajectoryTaskConfig(task_config);
if (!config_validation.ok) {
return rm_control::common::Result<OfflineTaskPreparation>::failure(
"task configuration validation failed: " +
config_validation.error_message);
}
OfflineTaskPreparation preparation;
preparation.trajectory = trajectory.value;
preparation.validation = validation.value;
preparation.task_config = task_config;
preparation.return_from_trajectory_end = cli.return_from_trajectory_end;
if (cli.return_from_trajectory_end) {
const auto return_trajectory = rm_control::modules::trajectory::
CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(
trajectory.value,
rm_control::modules::trajectory::
DefaultReversedVendorTrajectoryDirectory());
if (!return_trajectory.ok) {
return rm_control::common::Result<OfflineTaskPreparation>::failure(
"failed to create return trajectory: " +
return_trajectory.error_message);
}
const auto return_validation =
rm_control::modules::trajectory::ValidateDragTrajectory(
return_trajectory.value.samples, BuildValidationLimits(safety));
if (!return_validation.ok) {
return rm_control::common::Result<OfflineTaskPreparation>::failure(
"return trajectory validation failed: " +
return_validation.error_message);
}
preparation.return_trajectory = return_trajectory.value;
preparation.return_validation = return_validation.value;
}
return rm_control::common::Result<OfflineTaskPreparation>::success(preparation);
}
} // namespace
/**
* @brief 安全执行 RM75 本地轨迹与 O6 夹握集成任务。
* @reason 按离线校验、只读硬件确认、人工二次确认、统一状态机的顺序 fail-closed。
* @author 唐永康
* @date 2026-07-11
* @param argc 命令行参数数量。
* @param argv 命令行参数数组，全部外部输入在硬件连接前校验。
* @return 0 表示帮助、DryRun 或完整任务成功；其他值表示输入、门控、连接或任务失败。
* @calls ParseVendorDragTrajectoryJsonLines、RealmanArmAdapter、LinkerHandAdapter、ArmO6TrajectoryTask。
* @safety 默认 DryRun；只有 --live、配置、设备状态、轨迹校验和动态人工口令全部满足才运动。
* @note 本程序不会自动配置工具电压/RS485，不覆盖源轨迹或已有控制器程序，也不自动重试 O6 写入。
*/
int main(int argc, char **argv) {
const auto cli = rm_control::config::ParseTrajectoryO6TaskCliArguments(argc, argv);
if (!cli.ok) {
std::cerr << "Invalid arguments: " << cli.error_message << '\n';
rm_control::config::PrintTrajectoryO6TaskUsage(
argv == nullptr ? nullptr : argv[0], std::cerr);
return kInvalidArgumentsExitCode;
}
if (cli.value.help_requested) {
rm_control::config::PrintTrajectoryO6TaskUsage(
argv == nullptr ? nullptr : argv[0], std::cout);
return 0;
}
const auto safety = rm_control::config::LoadDragTeachSafetyConfig(
cli.value.safety_config_path);
if (!safety.ok) {
std::cerr << safety.error_message << '\n';
return kInvalidInputExitCode;
}
const auto preparation = PrepareOfflineTask(cli.value, safety.value);
if (!preparation.ok) {
std::cerr << preparation.error_message << '\n';
return kInvalidInputExitCode;
}
PrintOfflineReport(
"Forward trajectory", preparation.value.trajectory,
preparation.value.validation);
if (preparation.value.return_from_trajectory_end) {
PrintOfflineReport(
"Return trajectory", preparation.value.return_trajectory,
preparation.value.return_validation);
std::cout << "Round-trip controller programs: return="
<< cli.value.return_program_id
<< ", forward=" << cli.value.program_id << '\n';
}
PrintRequestedO6Motion(cli.value, safety.value);
if (!cli.value.live_requested) {
std::cout << "DryRun complete: no device was connected and no motion was executed.\n";
return 0;
}
return RunLiveTask(cli.value, safety.value, preparation.value);
}
RM/tests/mocks/mock_dexterous_hand.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/interfaces/dexterous_hand.h"
namespace rm_control::tests {
class MockDexterousHand final : public interfaces::IDexterousHand {
public:
common::Result<interfaces::DexterousHandReadiness> readiness_result =
common::Result<interfaces::DexterousHandReadiness>::failure(
"mock readiness was not configured");
common::Result<void> command_result = common::Result<void>::success();
int readiness_calls = 0;
int command_calls = 0;
interfaces::O6MotionCommand last_motion_command{};
/** @copydoc interfaces::IDexterousHand::ReadReadiness */
common::Result<interfaces::DexterousHandReadiness>
ReadReadiness() override {
++readiness_calls;
return readiness_result;
}
/** @copydoc interfaces::IDexterousHand::CommandMotion */
common::Result<void> CommandMotion(
const interfaces::O6MotionCommand &command) override {
++command_calls;
last_motion_command = command;
return command_result;
}
};
} // namespace rm_control::tests
RM/tests/mocks/mock_robot_arm.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/interfaces/robot_arm.h"
#include <filesystem>
#include <fstream>
#include <string>
#include <vector>
namespace rm_control::tests {
class MockRobotArm final : public interfaces::IRobotArm {
public:
bool connected = true;
common::RobotInfo robot_info;
common::RobotSoftwareInfo software_info;
common::ArmState arm_state;
common::ForceData force_data;
common::RobotSafetyStatus safety_status;
common::RobotJointLimits joint_limits;
int vendor_save_return_code = 0;
int vendor_point_count = 3;
bool create_vendor_file = true;
int fail_current_state_on_or_after_call = -1;
int start_recording_failures_remaining = 0;
int stop_recording_failures_remaining = 0;
int current_state_calls = 0;
int start_calls = 0;
int stop_recording_calls = 0;
int save_calls = 0;
int origin_calls = 0;
int replay_calls = 0;
int pause_calls = 0;
int continue_calls = 0;
int stop_replay_calls = 0;
int move_joint_calls = 0;
int execute_vendor_file_calls = 0;
int save_vendor_program_calls = 0;
int run_controller_program_calls = 0;
int program_state_calls = 0;
int slow_stop_calls = 0;
int program_slot_query_calls = 0;
bool last_move_joint_blocking = true;
bool apply_move_joint_target_immediately = true;
bool controller_program_slot_available = true;
bool controller_program_was_running = false;
common::VendorDragTrajectoryExecutionRequest last_vendor_execution_request;
common::VendorDragTrajectoryExecutionRequest last_vendor_program_save_request;
common::ControllerProgramRunRequest last_controller_program_run_request;
std::vector<common::VendorDragTrajectoryExecutionRequest>
vendor_program_save_requests;
std::vector<common::ControllerProgramRunRequest>
controller_program_run_requests;
common::Result<void> save_vendor_program_result = common::Result<void>::success();
common::Result<void> run_controller_program_result = common::Result<void>::success();
std::vector<common::Result<void>> save_vendor_program_results;
std::vector<common::Result<void>> run_controller_program_results;
std::vector<std::vector<float>> completed_program_joint_targets;
std::vector<std::string> operation_log;
std::vector<common::ControllerProgramRunState> program_run_states = {
common::ControllerProgramRunState{1, 1, 0, 5},
common::ControllerProgramRunState{0, 1, 0, 5},
};
MockRobotArm() {
robot_info.arm_dof = static_cast<int>(common::kRm75JointCount);
robot_info.controller_version = 3;
robot_info.is_rm75 = true;
robot_info.six_axis_force_sensor_available = true;
software_info.sdk_version = "test-sdk";
software_info.product_version = "RM75-MOCK";
software_info.firmware_version = "test-firmware";
arm_state.joints_deg.assign(common::kRm75JointCount, 0.0F);
safety_status.real_mode_enabled = true;
safety_status.arm_power_on = true;
safety_status.all_joints_enabled = true;
safety_status.arm_error_free = true;
safety_status.controller_error_free = true;
safety_status.virtual_wall_enabled = true;
safety_status.self_collision_enabled = true;
joint_limits.minimum_position_deg.fill(-360.0F);
joint_limits.maximum_position_deg.fill(360.0F);
joint_limits.maximum_speed_deg_per_second.fill(180.0F);
joint_limits.maximum_acceleration_deg_per_second_squared.fill(180.0F);
}
common::Result<void> connect(const common::RobotConnectionConfig &) override {
connected = true;
return common::Result<void>::success();
}
void disconnect() override {
connected = false;
}
bool isConnected() const noexcept override {
return connected;
}
common::Result<common::RobotInfo> getRobotInfo() override {
return common::Result<common::RobotInfo>::success(robot_info);
}
common::Result<common::RobotSoftwareInfo> getRobotSoftwareInfo() override {
return common::Result<common::RobotSoftwareInfo>::success(software_info);
}
common::Result<common::ArmState> getCurrentArmState() override {
++current_state_calls;
if (fail_current_state_on_or_after_call > 0 &&
current_state_calls >= fail_current_state_on_or_after_call) {
return common::Result<common::ArmState>::failure(
"mock current arm state failure");
}
return common::Result<common::ArmState>::success(arm_state);
}
common::Result<common::ForceData> getForceData() override {
return common::Result<common::ForceData>::success(force_data);
}
common::Result<common::RobotSafetyStatus> getRobotSafetyStatus() override {
return common::Result<common::RobotSafetyStatus>::success(safety_status);
}
common::Result<common::RobotJointLimits> getJointLimits() override {
return common::Result<common::RobotJointLimits>::success(joint_limits);
}
common::Result<void> moveJ(const std::vector<float> &joints_deg,
int,
int,
int,
bool blocking) override {
++move_joint_calls;
last_move_joint_blocking = blocking;
if (apply_move_joint_target_immediately) {
arm_state.joints_deg = joints_deg;
}
return common::Result<void>::success();
}
common::Result<void> moveL(const common::Pose &,
int,
int,
int,
bool) override {
return common::Result<void>::success();
}
common::Result<void> startDragTeach(
const common::DragTeachStartOptions &) override {
++start_calls;
if (start_recording_failures_remaining > 0) {
--start_recording_failures_remaining;
return common::Result<void>::failure("mock uncertain drag teach start");
}
return common::Result<void>::success();
}
common::Result<void> stopDragTeach() override {
++stop_recording_calls;
if (stop_recording_failures_remaining > 0) {
--stop_recording_failures_remaining;
return common::Result<void>::failure("mock stop drag teach failure");
}
return common::Result<void>::success();
}
common::VendorTrajectorySaveAttempt saveDragTeachTrajectory(
const std::filesystem::path &path) override {
++save_calls;
common::VendorTrajectorySaveAttempt attempt;
attempt.path = path;
attempt.api_called = true;
attempt.return_code = vendor_save_return_code;
attempt.point_count = vendor_point_count;
if (create_vendor_file && vendor_save_return_code == 0) {
std::ofstream output(path, std::ios::binary);
output << "mock rm75 raw trajectory";
}
return attempt;
}
common::Result<void> moveToDragTrajectoryOrigin(bool) override {
++origin_calls;
return common::Result<void>::success();
}
common::Result<void> replayLastDragTrajectory(bool) override {
++replay_calls;
return common::Result<void>::success();
}
common::Result<void> pauseDragTrajectoryReplay() override {
++pause_calls;
return common::Result<void>::success();
}
common::Result<void> continueDragTrajectoryReplay() override {
++continue_calls;
return common::Result<void>::success();
}
common::Result<void> stopDragTrajectoryReplay() override {
++stop_replay_calls;
return common::Result<void>::success();
}
common::Result<void> executeVendorDragTrajectoryFile(
const common::VendorDragTrajectoryExecutionRequest &request) override {
++execute_vendor_file_calls;
last_vendor_execution_request = request;
return common::Result<void>::success();
}
common::Result<void> saveVendorDragTrajectoryProgram(
const common::VendorDragTrajectoryExecutionRequest &request) override {
++save_vendor_program_calls;
last_vendor_program_save_request = request;
vendor_program_save_requests.push_back(request);
operation_log.push_back("saveVendorDragTrajectoryProgram");
if (!save_vendor_program_results.empty()) {
const auto result = save_vendor_program_results.front();
save_vendor_program_results.erase(save_vendor_program_results.begin());
return result;
}
return save_vendor_program_result;
}
common::Result<void> runControllerProgram(
const common::ControllerProgramRunRequest &request) override {
++run_controller_program_calls;
last_controller_program_run_request = request;
controller_program_run_requests.push_back(request);
operation_log.push_back("runControllerProgram");
if (!run_controller_program_results.empty()) {
const auto result = run_controller_program_results.front();
run_controller_program_results.erase(run_controller_program_results.begin());
return result;
}
return run_controller_program_result;
}
common::Result<common::ControllerProgramRunState>
getControllerProgramRunState() override {
++program_state_calls;
if (program_run_states.empty()) {
return common::Result<common::ControllerProgramRunState>::failure(
"mock program run state was not configured");
}
const auto state = program_run_states.front();
if (state.raw_state == 1) {
controller_program_was_running = true;
} else if (state.raw_state == 0 && controller_program_was_running) {
if (!completed_program_joint_targets.empty()) {
arm_state.joints_deg = completed_program_joint_targets.front();
completed_program_joint_targets.erase(
completed_program_joint_targets.begin());
}
controller_program_was_running = false;
}
if (program_run_states.size() > 1) {
program_run_states.erase(program_run_states.begin());
}
return common::Result<common::ControllerProgramRunState>::success(state);
}
common::Result<void> slowStopMotion() override {
++slow_stop_calls;
return common::Result<void>::success();
}
common::Result<bool> isControllerProgramSlotAvailable(int) override {
++program_slot_query_calls;
return common::Result<bool>::success(controller_program_slot_available);
}
};
} // namespace rm_control::tests
RM/tests/mocks/mock_tool_modbus_bus.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/interfaces/tool_modbus_bus.h"
#include <cstddef>
#include <cstdint>
#include <map>
#include <string>
#include <utility>
#include <vector>
namespace rm_control::tests {
struct InputRegisterReadCall {
std::uint8_t device_id = 0U;
std::uint16_t start_address = 0U;
std::size_t register_count = 0U;
};
struct HoldingRegisterReadCall {
std::uint8_t device_id = 0U;
std::uint16_t start_address = 0U;
std::size_t register_count = 0U;
};
struct HoldingRegisterWriteCall {
std::uint8_t device_id = 0U;
std::uint16_t start_address = 0U;
std::vector<std::uint16_t> values;
};
class MockToolModbusBus final : public interfaces::IToolModbusBus {
public:
common::Result<interfaces::ToolModbusBusStatus> status_result =
common::Result<interfaces::ToolModbusBusStatus>::success(
interfaces::ToolModbusBusStatus{true, 1, 1, 115200, 3, 10});
std::map<std::uint16_t, common::Result<std::vector<std::uint16_t>>>
input_register_results;
std::map<std::uint16_t, common::Result<std::uint16_t>>
single_input_register_results;
std::map<std::uint16_t, common::Result<std::vector<std::uint16_t>>>
holding_register_results;
std::map<std::uint16_t, common::Result<void>> write_results_by_start_address;
common::Result<void> write_result = common::Result<void>::success();
int status_calls = 0;
int configure_calls = 0;
interfaces::ToolModbusBusConfiguration last_configuration;
std::vector<InputRegisterReadCall> read_calls;
std::vector<InputRegisterReadCall> single_read_calls;
std::vector<HoldingRegisterReadCall> holding_read_calls;
std::vector<HoldingRegisterWriteCall> write_calls;
std::vector<std::string> operation_calls;
/** @copydoc interfaces::IToolModbusBus::Configure */
common::Result<interfaces::ToolModbusBusStatus> Configure(
const interfaces::ToolModbusBusConfiguration &configuration) override {
++configure_calls;
last_configuration = configuration;
operation_calls.push_back("configure");
return status_result;
}
/** @copydoc interfaces::IToolModbusBus::GetStatus */
common::Result<interfaces::ToolModbusBusStatus> GetStatus() override {
++status_calls;
operation_calls.push_back("get_status");
return status_result;
}
/** @copydoc interfaces::IToolModbusBus::ReadInputRegister */
common::Result<std::uint16_t> ReadInputRegister(
std::uint8_t device_id,
std::uint16_t address) override {
single_read_calls.push_back(InputRegisterReadCall{device_id, address, 1U});
operation_calls.push_back("read_single_input:" + std::to_string(address));
const auto configured = single_input_register_results.find(address);
if (configured != single_input_register_results.end()) {
return configured->second;
}
for (const auto &group : input_register_results) {
if (!group.second.ok && address == group.first) {
return common::Result<std::uint16_t>::failure(
group.second.error_message);
}
if (!group.second.ok || address < group.first) {
continue;
}
const std::size_t offset = address - group.first;
if (offset < group.second.value.size()) {
return common::Result<std::uint16_t>::success(
group.second.value[offset]);
}
}
return common::Result<std::uint16_t>::failure(
"mock single input register was not configured");
}
/** @copydoc interfaces::IToolModbusBus::ReadInputRegisters */
common::Result<std::vector<std::uint16_t>> ReadInputRegisters(
std::uint8_t device_id,
std::uint16_t start_address,
std::size_t register_count) override {
read_calls.push_back(
InputRegisterReadCall{device_id, start_address, register_count});
operation_calls.push_back(
"read_input:" + std::to_string(start_address));
const auto configured = input_register_results.find(start_address);
if (configured == input_register_results.end()) {
return common::Result<std::vector<std::uint16_t>>::failure(
"mock input register group was not configured");
}
return configured->second;
}
/** @copydoc interfaces::IToolModbusBus::ReadHoldingRegisters */
common::Result<std::vector<std::uint16_t>> ReadHoldingRegisters(
std::uint8_t device_id,
std::uint16_t start_address,
std::size_t register_count) override {
holding_read_calls.push_back(
HoldingRegisterReadCall{device_id, start_address, register_count});
operation_calls.push_back(
"read_holding:" + std::to_string(start_address));
const auto configured = holding_register_results.find(start_address);
if (configured == holding_register_results.end()) {
return common::Result<std::vector<std::uint16_t>>::failure(
"mock holding register group was not configured");
}
return configured->second;
}
/** @copydoc interfaces::IToolModbusBus::WriteHoldingRegisters */
common::Result<void> WriteHoldingRegisters(
std::uint8_t device_id,
std::uint16_t start_address,
const std::vector<std::uint16_t> &values) override {
write_calls.push_back(
HoldingRegisterWriteCall{device_id, start_address, values});
operation_calls.push_back(
"write_holding:" + std::to_string(start_address));
const auto configured = write_results_by_start_address.find(start_address);
if (configured != write_results_by_start_address.end()) {
return configured->second;
}
return write_result;
}
};
} // namespace rm_control::tests
RM/tests/test_arm_o6_trajectory_task.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/core/arm_o6_trajectory_task.h"
#include "rm_control/modules/trajectory/reversed_vendor_trajectory_store.h"
#include "mocks/mock_dexterous_hand.h"
#include "mocks/mock_robot_arm.h"
#include <chrono>
#include <cmath>
#include <cstddef>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <string>
#include <system_error>
#include <vector>
namespace {
struct TestContext {
int failures = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures;
std::cerr << "[FAIL] " << message << '\n';
}
}
};
const std::string &ValidTrajectoryJsonLines() {
static const std::string content =
"{\"point\":[0,0,0,0,0,0,0]}\n"
"{\"point\":[100,0,0,0,0,0,0]}\n"
"{\"point\":[200,0,0,0,0,0,0]}\n";
return content;
}
std::filesystem::path TestOutputRoot() {
static const std::filesystem::path output_root = [] {
const auto unique_suffix =
std::chrono::steady_clock::now().time_since_epoch().count();
const std::filesystem::path path = std::filesystem::temp_directory_path() /
("rm_arm_o6_trajectory_task_tests_" + std::to_string(unique_suffix));
std::error_code filesystem_error;
std::filesystem::create_directories(path, filesystem_error);
if (filesystem_error) {
std::cerr << "[FAIL] failed to create task test output: "
<< filesystem_error.message() << '\n';
}
return path;
}();
return output_root;
}
rm_control::modules::trajectory::VendorDragTrajectoryParseResult CreateTrajectoryFixture(
const std::string &filename,
bool make_read_only,
const std::string &replacement_after_parse = {}) {
const std::filesystem::path path = TestOutputRoot() / filename;
{
std::ofstream output(path, std::ios::binary | std::ios::trunc);
output << ValidTrajectoryJsonLines();
}
const auto parsed =
rm_control::modules::trajectory::ParseVendorDragTrajectoryJsonLines(path, 100);
if (!parsed.ok) {
std::cerr << "[FAIL] failed to parse task trajectory fixture: "
<< parsed.error_message << '\n';
return {};
}
if (!replacement_after_parse.empty()) {
std::ofstream replacement(path, std::ios::binary | std::ios::trunc);
replacement << replacement_after_parse;
}
if (make_read_only) {
std::error_code filesystem_error;
const auto read_only_permissions = std::filesystem::perms::owner_read |
std::filesystem::perms::group_read | std::filesystem::perms::others_read;
std::filesystem::permissions(
path, read_only_permissions, std::filesystem::perm_options::replace,
filesystem_error);
if (filesystem_error) {
std::cerr << "[FAIL] failed to make task fixture read-only: "
<< filesystem_error.message() << '\n';
}
}
return parsed.value;
}
rm_control::modules::trajectory::VendorDragTrajectoryParseResult MakeTrajectory() {
static const auto trajectory =
CreateTrajectoryFixture("valid_read_only_trajectory.jsonl", true);
return trajectory;
}
rm_control::modules::trajectory::VendorDragTrajectoryParseResult
MakeReturnTrajectory() {
static const auto return_trajectory = [] {
const auto created = rm_control::modules::trajectory::
CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(
MakeTrajectory(), TestOutputRoot() / "return_trajectories");
if (!created.ok) {
std::cerr << "[FAIL] failed to create return trajectory fixture: "
<< created.error_message << '\n';
return rm_control::modules::trajectory::
VendorDragTrajectoryParseResult{};
}
return created.value;
}();
return return_trajectory;
}
rm_control::core::ArmO6TrajectoryTaskConfig MakeTaskConfig() {
rm_control::core::ArmO6TrajectoryTaskConfig config;
config.validation_limits.maximum_joint_jump_deg = 1.0;
config.validation_limits.maximum_joint_velocity_deg_per_second = 10.0;
config.validation_limits.maximum_joint_acceleration_deg_per_second_squared = 100.0;
config.origin_speed_percent = 5;
config.plan_speed_percent = 5;
config.controller_program_id = 1;
config.grip_pose = {140U, 100U, 55U, 55U, 55U, 55U};
config.grip_speed_commands = {64U, 64U, 64U, 64U, 64U, 64U};
config.grip_torque_commands = {80U, 80U, 80U, 80U, 80U, 80U};
config.maximum_o6_speed_command = 64U;
config.maximum_o6_torque_command = 80U;
config.maximum_origin_delta_deg = 5.0;
config.origin_position_tolerance_deg = 0.5;
config.program_status_poll_period = std::chrono::milliseconds(0);
config.program_execution_timeout = std::chrono::milliseconds(50);
config.hand_status_poll_period = std::chrono::milliseconds(0);
config.hand_motion_timeout = std::chrono::milliseconds(0);
return config;
}
rm_control::core::HardwareExecutionGateInput MakeAllowedGate() {
rm_control::core::HardwareExecutionGateInput gate;
gate.live_requested = true;
gate.hardware_execution_enabled = true;
gate.arm_connected = true;
gate.hand_connected = true;
gate.emergency_stop_verified = true;
gate.safety_limits_loaded = true;
gate.operator_confirmed = true;
gate.trajectory_validated = true;
gate.trajectory_sample_period_confirmed = true;
gate.vendor_raw_units_per_degree_confirmed = true;
gate.workspace_and_virtual_wall_verified = true;
gate.configured_max_speed_percent = 10;
return gate;
}
void MakeRobotSafetyArraysExplicit(rm_control::tests::MockRobotArm *robot) {
robot->safety_status.joint_enable_states.fill(1);
robot->safety_status.joint_error_codes.fill(0);
robot->safety_status.arm_error_codes.clear();
robot->safety_status.controller_error_code = 0;
}
rm_control::interfaces::DexterousHandReadiness MakeReadyHand(
const rm_control::interfaces::O6JointPose &positions) {
rm_control::interfaces::DexterousHandReadiness readiness;
readiness.ready = true;
readiness.device_id = 0x27U;
readiness.freedom = 6U;
readiness.direction = 'R';
readiness.joint_positions = positions;
return readiness;
}
class SequencedDexterousHand final : public rm_control::interfaces::IDexterousHand {
public:
std::vector<rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>> readiness_results;
rm_control::common::Result<void> command_result =
rm_control::common::Result<void>::success();
bool *stop_requested_after_command = nullptr;
int readiness_calls = 0;
int command_calls = 0;
rm_control::interfaces::O6MotionCommand last_motion_command{};
rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>
ReadReadiness() override {
++readiness_calls;
if (readiness_results.empty()) {
return rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::failure(
"sequenced readiness exhausted");
}
const auto result = readiness_results.front();
if (readiness_results.size() > 1U) {
readiness_results.erase(readiness_results.begin());
}
return result;
}
rm_control::common::Result<void> CommandMotion(
const rm_control::interfaces::O6MotionCommand &command) override {
++command_calls;
last_motion_command = command;
if (stop_requested_after_command != nullptr) {
*stop_requested_after_command = true;
}
return command_result;
}
};
void TestCompletesTrajectoryThenCommandsAndVerifiesGrip(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
SequencedDexterousHand hand;
const auto config = MakeTaskConfig();
const rm_control::interfaces::O6JointPose open_pose{};
hand.readiness_results = {
rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(open_pose)),
rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(open_pose)),
rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(config.grip_pose)),
};
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(result.ok, "valid Mock trajectory and O6 task should complete");
context->Expect(robot.move_joint_calls == 1, "task should move to trajectory origin once");
context->Expect(!robot.last_move_joint_blocking,
"origin move must be nonblocking so stop requests remain observable");
context->Expect(robot.program_slot_query_calls == 1,
"task should prove the controller program slot is free before moveJ");
context->Expect(robot.execute_vendor_file_calls == 1,
"task should execute the vendor trajectory once");
context->Expect(robot.program_state_calls == 2,
"task should observe running followed by completed state");
context->Expect(hand.command_calls == 1, "task should command O6 only after trajectory end");
context->Expect(hand.last_motion_command.target_positions == config.grip_pose,
"O6 command should preserve all six target positions");
context->Expect(hand.last_motion_command.joint_speed_commands ==
config.grip_speed_commands,
"O6 command should preserve all six speed commands");
context->Expect(hand.last_motion_command.joint_torque_commands ==
config.grip_torque_commands,
"O6 command should preserve all six torque commands");
context->Expect(robot.slow_stop_calls == 0, "successful task should not request slow stop");
if (result.ok) {
context->Expect(result.value.observed_running_state,
"success report must prove controller running state was observed");
context->Expect(result.value.final_program_state.raw_state == 0,
"success report controller state must be completed");
const auto &timing = result.value.controller_timing;
context->Expect(timing.poll_count == 2U,
"controller timing report should count both state polls");
context->Expect(timing.minimum_poll_period_ms <= timing.average_poll_period_ms &&
timing.average_poll_period_ms <= timing.maximum_poll_period_ms,
"controller timing report periods should remain ordered");
context->Expect(timing.maximum_absolute_poll_jitter_ms >= 0.0 &&
timing.observed_execution_duration_ms >= 0.0,
"controller timing report durations should be nonnegative");
}
}
void ConfigureSuccessfulRoundTripRobot(
rm_control::tests::MockRobotArm *robot,
const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
const rm_control::modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory) {
MakeRobotSafetyArraysExplicit(robot);
robot->arm_state.joints_deg = return_trajectory.samples.front().joints_deg;
robot->program_run_states = {
rm_control::common::ControllerProgramRunState{0, 0, 0, 0},
rm_control::common::ControllerProgramRunState{1, 2, 0, 5},
rm_control::common::ControllerProgramRunState{0, 0, 0, 0},
rm_control::common::ControllerProgramRunState{0, 0, 0, 0},
rm_control::common::ControllerProgramRunState{1, 1, 0, 5},
rm_control::common::ControllerProgramRunState{0, 0, 0, 0},
};
robot->completed_program_joint_targets = {
return_trajectory.samples.back().joints_deg,
forward_trajectory.samples.back().joints_deg,
};
}
void ConfigureSuccessfulRoundTripHand(
SequencedDexterousHand *hand,
const rm_control::core::ArmO6TrajectoryTaskConfig &config) {
const rm_control::interfaces::O6JointPose open_pose{};
hand->readiness_results = {
rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(open_pose)),
rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(open_pose)),
rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(config.grip_pose)),
};
}
rm_control::core::ArmO6TrajectoryTaskConfig MakeRoundTripTaskConfig() {
auto config = MakeTaskConfig();
config.return_controller_program_id = 2;
config.origin_position_tolerance_deg = 0.05;
return config;
}
void TestRoundTripReturnsThenReplaysBeforeGrip(TestContext *context) {
const auto forward_trajectory = MakeTrajectory();
const auto return_trajectory = MakeReturnTrajectory();
rm_control::tests::MockRobotArm robot;
ConfigureSuccessfulRoundTripRobot(
&robot, return_trajectory, forward_trajectory);
SequencedDexterousHand hand;
const auto config = MakeRoundTripTaskConfig();
ConfigureSuccessfulRoundTripHand(&hand, config);
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.RunFromTrajectoryEnd(
return_trajectory, forward_trajectory, config, MakeAllowedGate());
context->Expect(result.ok, "valid reverse and forward trajectories should complete");
context->Expect(robot.move_joint_calls == 0,
"round trip must not use a large direct moveJ to the origin");
context->Expect(robot.execute_vendor_file_calls == 0,
"round trip must not use upload-and-immediately-run compatibility API");
context->Expect(robot.save_vendor_program_calls == 2 &&
robot.run_controller_program_calls == 2,
"round trip should save two programs before running two legs");
context->Expect(robot.vendor_program_save_requests.size() == 2U &&
robot.vendor_program_save_requests[0].controller_program_id == 2 &&
robot.vendor_program_save_requests[1].controller_program_id == 1,
"return program must be saved before the forward program with distinct ids");
context->Expect(robot.controller_program_run_requests.size() == 2U &&
robot.controller_program_run_requests[0].controller_program_id == 2 &&
robot.controller_program_run_requests[1].controller_program_id == 1 &&
!robot.controller_program_run_requests[0].blocking &&
!robot.controller_program_run_requests[1].blocking,
"return then forward programs must run nonblocking in strict order");
context->Expect(robot.program_state_calls == 6,
"both legs may observe idle before running and must still prove completion");
context->Expect(hand.command_calls == 1,
"O6 grip must be commanded only after both controller legs complete");
context->Expect(robot.slow_stop_calls == 0,
"successful round trip must not request slow stop");
if (result.ok) {
context->Expect(result.value.returned_from_trajectory_end &&
result.value.return_observed_running_state &&
result.value.observed_running_state,
"round-trip report must prove both running states were observed");
context->Expect(result.value.return_final_program_state.raw_state == 0 &&
result.value.final_program_state.raw_state == 0,
"round-trip report must contain two completed controller states");
}
}
void TestRoundTripRejectsCurrentStateAwayFromTrajectoryEnd(TestContext *context) {
const auto forward_trajectory = MakeTrajectory();
const auto return_trajectory = MakeReturnTrajectory();
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
robot.arm_state.joints_deg = forward_trajectory.samples[1].joints_deg;
rm_control::tests::MockDexterousHand hand;
hand.readiness_result = rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(MakeReadyHand({}));
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.RunFromTrajectoryEnd(
return_trajectory, forward_trajectory, MakeRoundTripTaskConfig(),
MakeAllowedGate());
context->Expect(!result.ok,
"round trip must reject a robot that is not at the original end point");
context->Expect(robot.save_vendor_program_calls == 0 &&
robot.run_controller_program_calls == 0 &&
robot.move_joint_calls == 0,
"endpoint mismatch must fail before every upload and motion command");
context->Expect(hand.command_calls == 0,
"endpoint mismatch must prevent O6 grip");
}
void TestRoundTripForwardSaveFailurePreventsMotion(TestContext *context) {
const auto forward_trajectory = MakeTrajectory();
const auto return_trajectory = MakeReturnTrajectory();
rm_control::tests::MockRobotArm robot;
ConfigureSuccessfulRoundTripRobot(
&robot, return_trajectory, forward_trajectory);
robot.save_vendor_program_results = {
rm_control::common::Result<void>::success(),
rm_control::common::Result<void>::failure("mock forward save failure"),
};
rm_control::tests::MockDexterousHand hand;
hand.readiness_result = rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(MakeReadyHand({}));
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.RunFromTrajectoryEnd(
return_trajectory, forward_trajectory, MakeRoundTripTaskConfig(),
MakeAllowedGate());
context->Expect(!result.ok, "forward program save failure must fail closed");
context->Expect(robot.save_vendor_program_calls == 2 &&
robot.run_controller_program_calls == 0,
"both saves may be attempted but no program may run after save failure");
context->Expect(robot.slow_stop_calls == 0,
"save-only failure before motion should not issue an unnecessary slow stop");
context->Expect(hand.command_calls == 0,
"save failure must prevent O6 grip");
}
void TestRoundTripPausedReturnProgramStopsBeforeForward(TestContext *context) {
const auto forward_trajectory = MakeTrajectory();
const auto return_trajectory = MakeReturnTrajectory();
rm_control::tests::MockRobotArm robot;
ConfigureSuccessfulRoundTripRobot(
&robot, return_trajectory, forward_trajectory);
robot.program_run_states = {
rm_control::common::ControllerProgramRunState{1, 2, 0, 5},
rm_control::common::ControllerProgramRunState{2, 2, 0, 5},
};
rm_control::tests::MockDexterousHand hand;
hand.readiness_result = rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(MakeReadyHand({}));
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.RunFromTrajectoryEnd(
return_trajectory, forward_trajectory, MakeRoundTripTaskConfig(),
MakeAllowedGate());
context->Expect(!result.ok, "paused return program must fail the round trip");
context->Expect(robot.run_controller_program_calls == 1 &&
robot.slow_stop_calls == 1,
"paused return must slow-stop and prevent the forward program");
context->Expect(hand.command_calls == 0,
"paused return must prevent O6 grip");
}
void TestOccupiedProgramSlotPreventsOriginMotion(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
robot.controller_program_slot_available = false;
rm_control::tests::MockDexterousHand hand;
const auto config = MakeTaskConfig();
hand.readiness_result = rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(config.grip_pose));
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!result.ok, "occupied controller program slot must fail closed");
context->Expect(robot.program_slot_query_calls == 1,
"program slot availability should be queried exactly once in preflight");
context->Expect(robot.move_joint_calls == 0 && robot.execute_vendor_file_calls == 0,
"occupied controller slot must be rejected before RM75 moves");
context->Expect(hand.command_calls == 0,
"occupied controller slot must prevent the O6 command");
}
void TestWritableSourceFileIsRejectedBeforeOriginMotion(TestContext *context) {
const auto trajectory =
CreateTrajectoryFixture("writable_trajectory.jsonl", false);
context->Expect(trajectory.samples.size() == 3U,
"writable source fixture should contain valid parsed metadata");
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
rm_control::tests::MockDexterousHand hand;
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(trajectory, MakeTaskConfig(), MakeAllowedGate());
context->Expect(!result.ok, "writable Live trajectory source must be rejected");
context->Expect(result.error_message.find("read-only") != std::string::npos,
"writable source rejection should identify its permission problem");
context->Expect(robot.move_joint_calls == 0 && robot.execute_vendor_file_calls == 0,
"writable source must be rejected before moveJ or upload");
}
void TestTamperedReadOnlySourceFileIsRejectedBeforeOriginMotion(TestContext *context) {
std::string tampered_content = ValidTrajectoryJsonLines();
const std::size_t value_position = tampered_content.find("100");
if (value_position != std::string::npos) {
tampered_content[value_position + 2U] = '1';
}
const auto trajectory = CreateTrajectoryFixture(
"tampered_read_only_trajectory.jsonl", true, tampered_content);
context->Expect(trajectory.samples.size() == 3U,
"tampered source fixture should retain original parsed metadata");
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
rm_control::tests::MockDexterousHand hand;
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(trajectory, MakeTaskConfig(), MakeAllowedGate());
context->Expect(!result.ok, "tampered read-only trajectory source must be rejected");
context->Expect(result.error_message.find("SHA-256") != std::string::npos,
"same-size source tampering should be detected by SHA-256");
context->Expect(robot.move_joint_calls == 0 && robot.execute_vendor_file_calls == 0,
"tampered source must be rejected before moveJ or upload");
}
void TestGateRejectionPreventsEveryMotionCommand(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
rm_control::tests::MockDexterousHand hand;
auto gate = MakeAllowedGate();
gate.operator_confirmed = false;
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), MakeTaskConfig(), gate);
context->Expect(!result.ok, "missing operator confirmation must reject task");
context->Expect(robot.move_joint_calls == 0 && robot.execute_vendor_file_calls == 0,
"gate rejection must occur before any RM75 motion command");
context->Expect(hand.command_calls == 0,
"gate rejection must occur before any O6 command");
}
void TestPausedProgramStateSlowStopsAndPreventsGrip(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
robot.program_run_states = {
rm_control::common::ControllerProgramRunState{1, 1, 0, 5},
rm_control::common::ControllerProgramRunState{2, 1, 0, 5},
};
rm_control::tests::MockDexterousHand hand;
const auto config = MakeTaskConfig();
hand.readiness_result = rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(config.grip_pose));
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!result.ok, "unexpected paused controller state must fail closed");
context->Expect(robot.slow_stop_calls == 1,
"program state failure should request RM75 slow stop");
context->Expect(hand.command_calls == 0,
"O6 must not move when trajectory completion was not proven");
}
void TestWrongProgramIdentityOrUnsafeSpeedSlowStops(TestContext *context) {
const auto run_case = [context](int program_id,
int plan_speed_percent,
const std::string &description) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
robot.program_run_states = {
rm_control::common::ControllerProgramRunState{
1, program_id, 0, plan_speed_percent},
};
rm_control::tests::MockDexterousHand hand;
const auto config = MakeTaskConfig();
hand.readiness_result = rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(config.grip_pose));
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!result.ok, description + " must fail closed");
context->Expect(robot.slow_stop_calls == 1,
description + " must request RM75 slow stop");
context->Expect(hand.command_calls == 0,
description + " must prevent the O6 command");
};
run_case(2, 5, "mismatched controller program id");
run_case(1, 6, "controller plan speed above configured request");
}
void TestPostTrajectoryHandReadinessFailurePreventsGrip(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
SequencedDexterousHand hand;
const auto config = MakeTaskConfig();
hand.readiness_results = {
rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(config.grip_pose)),
rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>::failure(
"mock O6 disappeared after trajectory"),
};
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!result.ok, "post-trajectory O6 readiness failure must fail task");
context->Expect(robot.execute_vendor_file_calls == 1,
"readiness failure should be exercised after completed trajectory");
context->Expect(robot.slow_stop_calls == 1,
"post-trajectory readiness failure should request slow stop");
context->Expect(hand.command_calls == 0,
"failed O6 readiness must prevent the grip command");
}
void TestPreGripRobotSafetyRecheckFailurePreventsGrip(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
robot.fail_current_state_on_or_after_call = 3;
rm_control::tests::MockDexterousHand hand;
const auto config = MakeTaskConfig();
hand.readiness_result = rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(config.grip_pose));
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!result.ok, "failed pre-grip RM75 state recheck must fail task");
context->Expect(robot.execute_vendor_file_calls == 1,
"RM75 state failure should occur after the trajectory completed");
context->Expect(robot.current_state_calls == 3,
"task should re-read RM75 state immediately before O6 grip");
context->Expect(robot.slow_stop_calls == 1,
"failed pre-grip RM75 recheck should request slow stop");
context->Expect(hand.command_calls == 0,
"failed pre-grip RM75 recheck must prevent the O6 command");
}
void ExpectInvalidO6ConfigRejected(
TestContext *context,
const rm_control::core::ArmO6TrajectoryTaskConfig &config,
const std::string &description) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
rm_control::tests::MockDexterousHand hand;
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto validation =
rm_control::core::ValidateArmO6TrajectoryTaskConfig(config);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!validation.ok,
description + " must be rejected by the public pure validator");
context->Expect(!result.ok, description + " must fail closed");
context->Expect(robot.move_joint_calls == 0 && robot.execute_vendor_file_calls == 0,
description + " must fail before RM75 motion");
context->Expect(hand.command_calls == 0,
description + " must fail before the O6 command");
}
void TestRejectsO6ProtocolRangeViolations(TestContext *context) {
auto invalid_position = MakeTaskConfig();
invalid_position.grip_pose[1] = 256U;
ExpectInvalidO6ConfigRejected(context, invalid_position, "O6 position above 255");
auto invalid_speed = MakeTaskConfig();
invalid_speed.grip_speed_commands[2] = 256U;
ExpectInvalidO6ConfigRejected(context, invalid_speed, "O6 speed above 255");
auto invalid_torque = MakeTaskConfig();
invalid_torque.grip_torque_commands[3] = 256U;
ExpectInvalidO6ConfigRejected(context, invalid_torque, "O6 torque above 255");
}
void TestAcceptsO6CommandsAtConfiguredMaximum(TestContext *context) {
const auto validation =
rm_control::core::ValidateArmO6TrajectoryTaskConfig(MakeTaskConfig());
context->Expect(validation.ok,
"O6 speed and torque equal to configured maxima should be valid");
}
void TestRejectsO6ConfiguredMaximumViolations(TestContext *context) {
auto speed_above_configured_maximum = MakeTaskConfig();
speed_above_configured_maximum.grip_speed_commands[4] = 65U;
ExpectInvalidO6ConfigRejected(
context, speed_above_configured_maximum, "O6 speed above configured maximum");
auto torque_above_configured_maximum = MakeTaskConfig();
torque_above_configured_maximum.grip_torque_commands[5] = 81U;
ExpectInvalidO6ConfigRejected(
context, torque_above_configured_maximum, "O6 torque above configured maximum");
}
void TestRejectsO6MaximumOutsideProtocolRange(TestContext *context) {
auto invalid_speed_maximum = MakeTaskConfig();
invalid_speed_maximum.maximum_o6_speed_command = 256U;
ExpectInvalidO6ConfigRejected(
context, invalid_speed_maximum, "configured O6 speed maximum above 255");
auto invalid_torque_maximum = MakeTaskConfig();
invalid_torque_maximum.maximum_o6_torque_command = 256U;
ExpectInvalidO6ConfigRejected(
context, invalid_torque_maximum, "configured O6 torque maximum above 255");
}
void TestO6MotionWriteFailureSlowStops(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
SequencedDexterousHand hand;
const auto config = MakeTaskConfig();
const rm_control::interfaces::O6JointPose open_pose{};
hand.readiness_results = {
rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(open_pose)),
};
hand.command_result = rm_control::common::Result<void>::failure(
"mock RS485 write failure");
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!result.ok, "O6 motion write failure must fail the task");
context->Expect(hand.command_calls == 1,
"O6 motion write failure should occur on one composite command");
context->Expect(robot.slow_stop_calls == 1,
"O6 motion write failure should request RM75 slow stop");
}
void TestO6GripReadbackTimeoutSlowStops(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
SequencedDexterousHand hand;
auto config = MakeTaskConfig();
config.hand_motion_timeout = std::chrono::milliseconds(0);
const rm_control::interfaces::O6JointPose open_pose{};
hand.readiness_results = {
rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(open_pose)),
};
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!result.ok, "unchanged O6 readback must time out");
context->Expect(result.error_message.find("timeout") != std::string::npos,
"O6 readback timeout should preserve timeout context");
context->Expect(hand.command_calls == 1 && hand.readiness_calls == 3,
"timeout path should read open, command once, then read open again");
context->Expect(robot.slow_stop_calls == 1,
"O6 readback timeout should request RM75 slow stop");
}
void TestStopRequestDuringProgramMonitorSlowStops(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
rm_control::tests::MockDexterousHand hand;
const auto ready = MakeReadyHand({});
hand.readiness_result = rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(ready);
auto config = MakeTaskConfig();
int stop_callback_calls = 0;
config.stop_requested = [&stop_callback_calls] {
++stop_callback_calls;
return stop_callback_calls >= 6;
};
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!result.ok, "stop request during controller monitor must fail task");
context->Expect(robot.program_state_calls == 1 && robot.slow_stop_calls == 1,
"monitor stop request should stop before the next poll and slow-stop RM75");
context->Expect(hand.command_calls == 0,
"monitor stop request must prevent the O6 command");
}
void TestStopRequestDuringGripReadbackSlowStops(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
SequencedDexterousHand hand;
const auto config_without_callback = MakeTaskConfig();
const rm_control::interfaces::O6JointPose open_pose{};
hand.readiness_results = {
rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(open_pose)),
};
bool stop_requested = false;
hand.stop_requested_after_command = &stop_requested;
auto config = config_without_callback;
config.stop_requested = [&stop_requested] { return stop_requested; };
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!result.ok, "stop request during O6 readback must fail task");
context->Expect(hand.command_calls == 1 && hand.readiness_calls == 2,
"grip stop request should occur after command and before readback poll");
context->Expect(robot.slow_stop_calls == 1,
"grip readback stop request should request RM75 slow stop");
}
void TestStopRequestDuringOriginMotionStopsBeforeUpload(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
robot.apply_move_joint_target_immediately = false;
rm_control::tests::MockDexterousHand hand;
auto config = MakeTaskConfig();
config.origin_status_poll_period = std::chrono::milliseconds(0);
int stop_callback_calls = 0;
config.stop_requested = [&stop_callback_calls] {
++stop_callback_calls;
return stop_callback_calls >= 3;
};
hand.readiness_result = rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(MakeReadyHand({}));
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(!result.ok, "stop request during origin motion must fail the task");
context->Expect(robot.move_joint_calls == 1 && robot.execute_vendor_file_calls == 0,
"origin stop must occur after move start and before trajectory upload");
context->Expect(robot.slow_stop_calls == 1,
"origin stop request must trigger one RM75 slow stop");
}
void TestControllerTimingReportMeasuresConfiguredPoll(TestContext *context) {
rm_control::tests::MockRobotArm robot;
MakeRobotSafetyArraysExplicit(&robot);
rm_control::tests::MockDexterousHand hand;
auto config = MakeTaskConfig();
config.program_status_poll_period = std::chrono::milliseconds(2);
hand.readiness_result = rm_control::common::Result<
rm_control::interfaces::DexterousHandReadiness>::success(
MakeReadyHand(config.grip_pose));
rm_control::core::ArmO6TrajectoryTask task(robot, hand);
const auto result = task.Run(MakeTrajectory(), config, MakeAllowedGate());
context->Expect(result.ok, "timed controller monitor should complete with Mock states");
if (!result.ok) {
return;
}
const auto &timing = result.value.controller_timing;
const double expected_jitter = std::fabs(timing.average_poll_period_ms - 2.0);
context->Expect(timing.poll_count == 2U && timing.average_poll_period_ms > 0.0,
"timing report should contain one positive measured poll interval");
context->Expect(timing.minimum_poll_period_ms == timing.average_poll_period_ms &&
timing.average_poll_period_ms == timing.maximum_poll_period_ms,
"one measured interval should have identical min, max, and average");
context->Expect(std::fabs(timing.maximum_absolute_poll_jitter_ms - expected_jitter) < 1e-9,
"timing report jitter should be absolute deviation from configured poll");
context->Expect(timing.observed_execution_duration_ms >= timing.average_poll_period_ms,
"observed execution duration should cover the measured poll interval");
}
} // namespace
int main() {
TestContext context;
TestCompletesTrajectoryThenCommandsAndVerifiesGrip(&context);
TestRoundTripReturnsThenReplaysBeforeGrip(&context);
TestRoundTripRejectsCurrentStateAwayFromTrajectoryEnd(&context);
TestRoundTripForwardSaveFailurePreventsMotion(&context);
TestRoundTripPausedReturnProgramStopsBeforeForward(&context);
TestOccupiedProgramSlotPreventsOriginMotion(&context);
TestWritableSourceFileIsRejectedBeforeOriginMotion(&context);
TestTamperedReadOnlySourceFileIsRejectedBeforeOriginMotion(&context);
TestGateRejectionPreventsEveryMotionCommand(&context);
TestPausedProgramStateSlowStopsAndPreventsGrip(&context);
TestWrongProgramIdentityOrUnsafeSpeedSlowStops(&context);
TestPostTrajectoryHandReadinessFailurePreventsGrip(&context);
TestPreGripRobotSafetyRecheckFailurePreventsGrip(&context);
TestRejectsO6ProtocolRangeViolations(&context);
TestAcceptsO6CommandsAtConfiguredMaximum(&context);
TestRejectsO6ConfiguredMaximumViolations(&context);
TestRejectsO6MaximumOutsideProtocolRange(&context);
TestO6MotionWriteFailureSlowStops(&context);
TestO6GripReadbackTimeoutSlowStops(&context);
TestStopRequestDuringProgramMonitorSlowStops(&context);
TestStopRequestDuringGripReadbackSlowStops(&context);
TestStopRequestDuringOriginMotionStopsBeforeUpload(&context);
TestControllerTimingReportMeasuresConfiguredPoll(&context);
if (context.failures == 0) {
std::cout << "[PASS] rm_arm_o6_trajectory_task_tests\n";
return 0;
}
std::cerr << context.failures << " test(s) failed\n";
return 1;
}
RM/tests/test_cli_config.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file test_cli_config.cpp
* @brief RM 子工程命令行配置解析单元测试。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 本文件测试 `rm_json_runner`、`rm_drag_teach_record` 和 `rm_trajectory_convert`
* 三个入口程序的 CLI 参数解析逻辑。测试只构造 `argc/argv`，不读取真实 JSON 或 CSV，
* 不连接 RM75，也不执行任何运动。
*
* 设计原则：
* - 每个测试只验证一个清晰行为；
* - 失败输出明确错误文本；
* - 不依赖绝对路径；
* - 不访问真实硬件或网络。
*
* @dependencies
* - rm_control/config/json_runner_cli_config.h
* - rm_control/config/drag_teach_cli_config.h
* - rm_control/config/trajectory_convert_cli_config.h
*/
#include "rm_control/config/drag_teach_cli_config.h"
#include "rm_control/config/json_runner_cli_config.h"
#include "rm_control/config/trajectory_convert_cli_config.h"
#include <iostream>
#include <string>
namespace {
class CliConfigTestSuite {
public:
int Run() {
testJsonRunnerValidateOnlyArguments();
testJsonRunnerRejectsUnknownOption();
testDragTeachRecordArguments();
testDragTeachLiveSafetyArguments();
testTrajectoryConvertDefaultOutput();
return failures_;
}
private:
int failures_ = 0;
/**
* @brief 记录一个布尔断言结果。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 当前 RM 子工程测试保持零外部依赖，不使用 GoogleTest。该函数提供最小断言能力：
* 条件不满足时累加失败计数并输出错误说明，主函数最终根据失败计数返回退出码。
*
* @param[in] condition 被验证的布尔条件。
* @param[in] message 断言失败时输出的错误说明。
*
* @return 无返回值。
*
* @throws 本函数不主动抛出异常；输出流异常由运行环境处理。
*
* @note
* 该函数只用于测试代码，不应被生产代码依赖。
*
* @warning
* 失败计数属于当前测试套件实例，不使用可变全局状态。
*
* @thread_safety
* 非线程安全；当前测试单线程顺序执行。
*
* @par 边界条件
* 空 message 仍会输出通用失败前缀。
*
* @par 后续维护
* 若未来引入 GoogleTest，应删除该轻量断言函数并迁移到标准断言宏。
*/
void expect(bool condition, const std::string &message) {
if (!condition) {
++failures_;
std::cerr << "[FAIL] " << message << '\n';
}
}
/**
* @brief 测试 JSON runner 的 validate-only 参数解析。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该测试构造 `rm_json_runner config.json --validate-only` 参数，确认解析结果包含
* JSON 文件路径并设置 validate-only 标志。该行为是新 JSON 文件安全验证的第一步。
*
* @param 无输入参数。
*
* @return 无返回值；失败通过 `expect()` 记录。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 测试不检查文件是否存在，因为 CLI 层不负责文件读取。
*
* @warning
* 如果未来改变 validate-only 参数名称，需要同步更新该测试。
*
* @thread_safety
* 非线程安全；依赖测试全局失败计数。
*
* @par 边界条件
* 参数数组使用普通字符串常量，模拟最常见调用路径。
*
* @par 后续维护
* 保持该测试覆盖“不连接硬件前就能解析参数”的安全路径。
*/
void testJsonRunnerValidateOnlyArguments() {
char program[] = "rm_json_runner";
char command_file[] = "config.json";
char validate_only[] = "--validate-only";
char *argv[] = {program, command_file, validate_only};
const auto config = rm_control::config::ParseJsonRunnerCliArguments(3, argv);
expect(config.ok, "json runner validate-only arguments should parse");
if (!config.ok) {
return;
}
expect(config.value.command_file_path == "config.json", "json runner command path parsed");
expect(config.value.validate_only, "json runner validate-only flag parsed");
}
/**
* @brief 测试 JSON runner 拒绝未知选项。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该测试确认 CLI 层会拒绝未知选项，避免用户拼错安全相关参数后程序继续进入
* JSON 加载或硬件连接阶段。
*
* @param 无输入参数。
*
* @return 无返回值；失败通过 `expect()` 记录。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 不访问文件系统。
*
* @warning
* 后续新增选项时应只放开明确支持的选项。
*
* @thread_safety
* 非线程安全；依赖测试全局失败计数。
*
* @par 边界条件
* 未知选项放在第二业务参数位置。
*
* @par 后续维护
* 保持错误路径测试，防止 CLI 解析过度宽松。
*/
void testJsonRunnerRejectsUnknownOption() {
char program[] = "rm_json_runner";
char command_file[] = "config.json";
char unknown_option[] = "--unsafe";
char *argv[] = {program, command_file, unknown_option};
const auto config = rm_control::config::ParseJsonRunnerCliArguments(3, argv);
expect(!config.ok, "json runner should reject unknown option");
}
/**
* @brief 测试拖动示教 CLI 的位置参数和选项解析。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该测试覆盖 session name、录制时长、采样周期、存储根目录和 `--no-vendor-save`
* 选项，确保入口文件可以拿到完整结构化配置，而不需要自己解析字符串。
*
* @param 无输入参数。
*
* @return 无返回值；失败通过 `expect()` 记录。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 测试不会创建轨迹目录，也不会连接 RM75。
*
* @warning
* 如果未来修改位置参数顺序，需要同步更新测试和 README。
*
* @thread_safety
* 非线程安全；依赖测试全局失败计数。
*
* @par 边界条件
* 使用相对存储根目录，避免绝对路径依赖。
*
* @par 后续维护
* 新增安全选项时应补充对应测试。
*/
void testDragTeachRecordArguments() {
char program[] = "rm_drag_teach_record";
char session[] = "demo";
char duration[] = "12";
char sample_period[] = "100";
char storage_root[] = "trajectory_output";
char no_vendor_save[] = "--no-vendor-save";
char *argv[] = {program, session, duration, sample_period, storage_root, no_vendor_save};
const auto config = rm_control::config::ParseDragTeachRecordCliArguments(6, argv);
expect(config.ok, "drag teach arguments should parse");
if (!config.ok) {
return;
}
expect(config.value.record_config.session_name == "demo", "drag teach session parsed");
expect(config.value.record_config.duration_seconds == 12, "drag teach duration parsed");
expect(config.value.record_config.sample_period_ms == 100, "drag teach sample period parsed");
expect(config.value.record_config.storage_root == "trajectory_output", "drag teach storage parsed");
expect(!config.value.record_config.save_vendor_trajectory, "drag teach no vendor save parsed");
expect(!config.value.live_requested, "drag teach must default to DryRun authorization");
expect(config.value.record_config.drag_options.mode ==
rm_control::common::DragTeachMode::OrdinaryRecorded,
"ordinary recorded mode should be the safe default");
}
void testDragTeachLiveSafetyArguments() {
char program[] = "rm_drag_teach_record";
char live[] = "--live";
char safety_option[] = "--safety-config";
char safety_path[] = "config/custom_drag_safety.json";
char six_dof[] = "--six-dof-force";
char *argv[] = {program, live, safety_option, safety_path, six_dof};
const auto config = rm_control::config::ParseDragTeachRecordCliArguments(5, argv);
expect(config.ok, "drag teach live safety arguments should parse");
if (!config.ok) {
return;
}
expect(config.value.live_requested, "--live should require explicit presence");
expect(config.value.safety_config_path == safety_path,
"custom safety config path should be preserved as filesystem path");
expect(config.value.record_config.drag_options.mode ==
rm_control::common::DragTeachMode::SixDofForcePositionAndOrientation,
"six-dof force mode should require its explicit option");
}
/**
* @brief 测试轨迹转换 CLI 的默认输出路径解析。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该测试只提供输入 CSV 路径，确认 CLI 层能够生成 session name 和默认输出路径。
* 默认路径实际写入能力由 trajectory 模块测试覆盖。
*
* @param 无输入参数。
*
* @return 无返回值；失败通过 `expect()` 记录。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 解析阶段可能确保默认轨迹目录存在，但不会读取输入 CSV。
*
* @warning
* 如果默认存储策略改变，需要同步更新断言粒度。
*
* @thread_safety
* 非线程安全；依赖测试全局失败计数。
*
* @par 边界条件
* 输入路径包含目录和扩展名，用于验证 session name 提取。
*
* @par 后续维护
* 保持该测试关注 CLI 输出，不把 CSV 内容校验混入其中。
*/
void testTrajectoryConvertDefaultOutput() {
char program[] = "rm_trajectory_convert";
char input_csv[] = "csv/demo-path.csv";
char *argv[] = {program, input_csv};
const auto config = rm_control::config::ParseTrajectoryConvertCliArguments(2, argv);
expect(config.ok, "trajectory convert arguments should parse");
if (!config.ok) {
return;
}
expect(config.value.input_csv_path == "csv/demo-path.csv", "trajectory input path parsed");
expect(config.value.session_name == "demo-path", "trajectory session name parsed");
expect(config.value.output_cpp_path.find("demo-path_tcp_path.cpp") != std::string::npos,
"trajectory default output path parsed");
}
};
} // namespace
/**
* @brief 运行 CLI 配置解析单元测试。
*
* @author mashiro
* @date 2026-07-09
* @last_modified 2026-07-09
*
* @details
* 该入口按顺序运行所有 CLI 解析测试，并根据失败计数返回进程状态码。
* 测试不访问真实硬件、不启动外部进程、不依赖网络。
*
* @param 无输入参数。
*
* @return 0 所有测试通过。
* @return 1 至少一个测试失败。
*
* @throws 本函数不主动抛出异常。
*
* @note
* 该测试程序由 CTest 调用。
*
* @warning
* 每个测试进程创建独立套件实例，不共享可变全局状态。
*
* @thread_safety
* 非线程安全；单线程测试入口。
*
* @par 边界条件
* 任一测试提前返回只影响该测试内部后续断言，不影响其他测试继续执行。
*
* @par 后续维护
* 新增 CLI 模块时应在此入口追加对应测试函数。
*/
int main() {
CliConfigTestSuite suite;
const int failures = suite.Run();
if (failures == 0) {
std::cout << "[PASS] rm_cli_config_tests\n";
return 0;
}
std::cerr << failures << " test(s) failed\n";
return 1;
}
RM/tests/test_drag_teach_safety_config.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/config/drag_teach_safety_config.h"
#include <filesystem>
#include <fstream>
#include <iostream>
#include <string>
#ifndef RM_TEST_OUTPUT_DIR
#define RM_TEST_OUTPUT_DIR "."
#endif
namespace {
struct TestContext {
int failures = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures;
std::cerr << "[FAIL] " << message << '\n';
}
}
};
std::filesystem::path WriteConfig(const std::string &name, const std::string &text) {
const std::filesystem::path directory =
std::filesystem::path(RM_TEST_OUTPUT_DIR) / "drag_teach_safety_config";
std::filesystem::create_directories(directory);
const std::filesystem::path path = directory / name;
std::ofstream output(path, std::ios::trunc);
output << text;
return path;
}
std::string ValidConfigText() {
return R"({
"hardware_execution_enabled": false,
"emergency_stop_verified": false,
"safety_limits_loaded": true,
"workspace_and_virtual_wall_verified": true,
"native_replay_without_speed_control_accepted": false,
"trajectory_sample_period_confirmed": false,
"vendor_raw_units_per_degree_confirmed": false,
"max_speed_percent": 20,
"vendor_raw_units_per_degree": 1000,
"maximum_origin_move_delta_deg": 30.0,
"max_joint_jump_deg": 5.0,
"max_joint_velocity_deg_per_second": 30.0,
"max_joint_acceleration_deg_per_second_squared": 100.0,
"maximum_absolute_force_newton": 100.0,
"maximum_absolute_torque_newton_meter": 20.0,
"maximum_o6_speed_command": 64,
"maximum_o6_torque_command": 80
})";
}
void TestLoadsExplicitDisabledConfiguration(TestContext *context) {
const auto path = WriteConfig("valid.json", ValidConfigText());
const auto result = rm_control::config::LoadDragTeachSafetyConfig(path);
context->Expect(result.ok, "valid safety config should load");
if (!result.ok) {
return;
}
context->Expect(!result.value.hardware_execution_enabled,
"hardware execution must remain explicitly disabled");
context->Expect(result.value.max_speed_percent == 20, "max speed should be parsed");
context->Expect(result.value.vendor_raw_units_per_degree == 1000,
"vendor raw-units-per-degree ratio should be parsed");
context->Expect(!result.value.trajectory_sample_period_confirmed &&
!result.value.vendor_raw_units_per_degree_confirmed,
"unconfirmed vendor timing and units must remain false");
context->Expect(result.value.maximum_origin_move_delta_deg == 30.0F,
"maximum origin move delta should be parsed");
context->Expect(result.value.maximum_o6_speed_command == 64U,
"maximum O6 speed command should be parsed");
context->Expect(result.value.maximum_o6_torque_command == 80U,
"maximum O6 torque command should be parsed");
}
void TestRejectsMissingRequiredField(TestContext *context) {
const auto path = WriteConfig(
"missing.json",
R"({"hardware_execution_enabled": false})");
const auto result = rm_control::config::LoadDragTeachSafetyConfig(path);
context->Expect(!result.ok, "missing safety fields must be rejected");
}
void TestRejectsInvalidLimit(TestContext *context) {
std::string text = ValidConfigText();
const std::string original = "\"max_speed_percent\": 20";
text.replace(text.find(original), original.size(), "\"max_speed_percent\": 0");
const auto path = WriteConfig("invalid_limit.json", text);
const auto result = rm_control::config::LoadDragTeachSafetyConfig(path);
context->Expect(!result.ok, "zero maximum speed must be rejected");
}
void TestRejectsO6CommandLimitAboveProtocolRange(TestContext *context) {
std::string text = ValidConfigText();
const std::string original = "\"maximum_o6_torque_command\": 80";
text.replace(text.find(original), original.size(),
"\"maximum_o6_torque_command\": 256");
const auto path = WriteConfig("invalid_o6_limit.json", text);
const auto result = rm_control::config::LoadDragTeachSafetyConfig(path);
context->Expect(!result.ok, "O6 torque command limit above 255 must be rejected");
}
} // namespace
int main() {
TestContext context;
TestLoadsExplicitDisabledConfiguration(&context);
TestRejectsMissingRequiredField(&context);
TestRejectsInvalidLimit(&context);
TestRejectsO6CommandLimitAboveProtocolRange(&context);
if (context.failures == 0) {
std::cout << "[PASS] rm_drag_teach_safety_config_tests\n";
return 0;
}
std::cerr << context.failures << " test(s) failed\n";
return 1;
}
RM/tests/test_drag_teach_service.cpp 0 → 100644
View file @ 8275ee2e
#include "mocks/mock_robot_arm.h"
#include "rm_control/core/drag_teach_service.h"
#include <chrono>
#include <filesystem>
#include <iostream>
#include <string>
#include <thread>
#ifndef RM_TEST_OUTPUT_DIR
#define RM_TEST_OUTPUT_DIR "."
#endif
namespace {
struct TestContext {
int failures = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures;
std::cerr << "[FAIL] " << message << '\n';
}
}
};
rm_control::core::HardwareExecutionGateInput AllowedGate() {
rm_control::core::HardwareExecutionGateInput gate;
gate.live_requested = true;
gate.hardware_execution_enabled = true;
gate.arm_connected = true;
gate.hand_connected = true;
gate.emergency_stop_verified = true;
gate.safety_limits_loaded = true;
gate.operator_confirmed = true;
gate.trajectory_validated = true;
gate.trajectory_sample_period_confirmed = true;
gate.vendor_raw_units_per_degree_confirmed = true;
gate.workspace_and_virtual_wall_verified = true;
gate.configured_max_speed_percent = 20;
gate.native_replay_without_speed_control_accepted = true;
gate.native_replay_speed_within_config_verified = true;
return gate;
}
rm_control::core::DragTeachServiceConfig ServiceConfig(const std::string &name) {
rm_control::core::DragTeachServiceConfig config;
config.storage_root =
std::filesystem::path(RM_TEST_OUTPUT_DIR) / "drag_teach_service";
config.session_name = name;
config.sample_period = std::chrono::milliseconds(10);
return config;
}
bool WaitForSampleCount(rm_control::core::DragTeachService *service,
std::size_t minimum_sample_count) {
const auto deadline = std::chrono::steady_clock::now() +
std::chrono::milliseconds(500);
while (std::chrono::steady_clock::now() < deadline) {
if (service->recordedSampleCount() >= minimum_sample_count) {
return true;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2));
}
return false;
}
bool WaitForState(rm_control::core::DragTeachService *service,
rm_control::core::DragTeachServiceState expected) {
const auto deadline = std::chrono::steady_clock::now() +
std::chrono::milliseconds(500);
while (std::chrono::steady_clock::now() < deadline) {
if (service->state() == expected) {
return true;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2));
}
return false;
}
void RecordAndSave(rm_control::core::DragTeachService *service,
const std::string &name,
TestContext *context) {
const auto start = service->StartRecording(ServiceConfig(name), AllowedGate());
context->Expect(start.ok, "StartRecording should succeed with complete gate");
if (!start.ok) {
return;
}
context->Expect(WaitForSampleCount(service, 3),
"recording should produce three samples before timeout");
const auto stop = service->StopRecording();
context->Expect(stop.ok, "StopRecording should validate stationary trajectory");
if (!stop.ok) {
return;
}
context->Expect(stop.value.recorded_sample_count >= 2,
"recording should contain at least two samples");
const auto artifact = service->SaveRawTrajectory();
context->Expect(artifact.ok, "SaveRawTrajectory should create manifest artifact");
if (!artifact.ok) {
return;
}
context->Expect(artifact.value.raw_file_present, "mock raw file should be present");
context->Expect(artifact.value.raw_file_sha256.size() == 64,
"raw file SHA-256 should have 64 hex characters");
context->Expect(std::filesystem::exists(artifact.value.paths.manifest),
"manifest should exist");
}
void TestStartRequiresHandConnection(TestContext *context) {
rm_control::tests::MockRobotArm robot;
rm_control::core::DragTeachService service(robot);
auto gate = AllowedGate();
gate.hand_connected = false;
const auto result = service.StartRecording(ServiceConfig("missing_hand"), gate);
context->Expect(!result.ok, "StartRecording must reject missing hand connection");
context->Expect(robot.start_calls == 0, "rejected start must not call hardware");
}
void TestFullLifecycleAndContinueRecheck(TestContext *context) {
rm_control::tests::MockRobotArm robot;
rm_control::core::DragTeachService service(robot);
RecordAndSave(&service, "full_lifecycle", context);
if (service.state() != rm_control::core::DragTeachServiceState::Saved) {
return;
}
const auto origin = service.MoveToTrajectoryOrigin(AllowedGate());
const auto replay = service.ReplayLastDragTrajectory(AllowedGate());
const auto pause = service.PauseReplay();
context->Expect(origin.ok && replay.ok && pause.ok,
"origin, replay and pause should follow valid lifecycle");
auto denied_gate = AllowedGate();
denied_gate.live_requested = false;
const auto denied_continue = service.ContinueReplay(denied_gate);
context->Expect(!denied_continue.ok, "ContinueReplay must recheck live gate");
context->Expect(robot.continue_calls == 0,
"denied ContinueReplay must not call hardware");
const auto continued = service.ContinueReplay(AllowedGate());
const auto stopped = service.StopReplay();
context->Expect(continued.ok && stopped.ok,
"authorized continue and safety stop should succeed");
context->Expect(robot.origin_calls == 1 && robot.replay_calls == 1 &&
robot.pause_calls == 1 && robot.continue_calls == 1 &&
robot.stop_replay_calls == 1,
"adapter replay methods should each be called once");
}
void TestWrongModelRejectedBeforeHardwareStart(TestContext *context) {
rm_control::tests::MockRobotArm robot;
robot.robot_info.is_rm75 = false;
rm_control::core::DragTeachService service(robot);
const auto result =
service.StartRecording(ServiceConfig("wrong_model"), AllowedGate());
context->Expect(!result.ok, "non-RM75 model must be rejected");
context->Expect(robot.start_calls == 0, "wrong model must not start drag teach");
}
void TestJointLimitViolationRejectedBeforeHardwareStart(TestContext *context) {
rm_control::tests::MockRobotArm robot;
robot.arm_state.joints_deg[0] = 400.0F;
rm_control::core::DragTeachService service(robot);
const auto result =
service.StartRecording(ServiceConfig("joint_limit"), AllowedGate());
context->Expect(!result.ok, "current joint outside controller limits must be rejected");
context->Expect(robot.start_calls == 0,
"joint limit violation must not start drag teach");
}
void TestSamplingFailureRetriesFailedStop(TestContext *context) {
rm_control::tests::MockRobotArm robot;
robot.fail_current_state_on_or_after_call = 2;
robot.stop_recording_failures_remaining = 1;
rm_control::core::DragTeachService service(robot);
const auto start = service.StartRecording(
ServiceConfig("sampling_failure"), AllowedGate());
context->Expect(start.ok, "sampling failure test should initially start");
if (!start.ok) {
return;
}
context->Expect(WaitForState(&service, rm_control::core::DragTeachServiceState::Faulted),
"sampling read failure should enter Faulted");
const auto stopped = service.StopRecording();
context->Expect(!stopped.ok, "sampling failure should be reported to caller");
context->Expect(robot.stop_recording_calls == 2,
"failed sampling-thread stop must be retried by StopRecording");
}
void TestUncertainStartAttemptsSafetyStop(TestContext *context) {
rm_control::tests::MockRobotArm robot;
robot.start_recording_failures_remaining = 1;
rm_control::core::DragTeachService service(robot);
const auto result = service.StartRecording(
ServiceConfig("uncertain_start"), AllowedGate());
context->Expect(!result.ok, "failed start must be reported");
context->Expect(robot.start_calls == 1,
"failed start should call the adapter exactly once");
context->Expect(robot.stop_recording_calls == 1,
"uncertain start must immediately attempt stopDragTeach");
}
} // namespace
int main() {
TestContext context;
TestStartRequiresHandConnection(&context);
TestFullLifecycleAndContinueRecheck(&context);
TestWrongModelRejectedBeforeHardwareStart(&context);
TestJointLimitViolationRejectedBeforeHardwareStart(&context);
TestSamplingFailureRetriesFailedStop(&context);
TestUncertainStartAttemptsSafetyStop(&context);
if (context.failures == 0) {
std::cout << "[PASS] rm_drag_teach_service_tests\n";
return 0;
}
std::cerr << context.failures << " test(s) failed\n";
return 1;
}
RM/tests/test_drag_trajectory_validator.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/modules/trajectory/drag_trajectory_validator.h"
#include <cstddef>
#include <iostream>
#include <limits>
#include <string>
#include <vector>
namespace {
struct TestContext {
int failures = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures;
std::cerr << "[FAIL] " << message << '\n';
}
}
};
rm_control::common::TrajectorySample MakeSample(std::size_t sample_index,
long long elapsed_ms,
float joint_1_deg) {
rm_control::common::TrajectorySample sample;
sample.sample_index = sample_index;
sample.elapsed_ms = elapsed_ms;
sample.joints_deg = {joint_1_deg, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F};
return sample;
}
rm_control::modules::trajectory::DragTrajectoryValidationLimits MakePermissiveLimits() {
rm_control::modules::trajectory::DragTrajectoryValidationLimits limits;
limits.maximum_joint_jump_deg = 100.0;
limits.maximum_joint_velocity_deg_per_second = 1000.0;
limits.maximum_joint_acceleration_deg_per_second_squared = 10000.0;
return limits;
}
void TestReportsPeakJointMotionForValidTrajectory(TestContext *context) {
const std::vector<rm_control::common::TrajectorySample> samples = {
MakeSample(0, 0, 0.0F),
MakeSample(1, 1000, 1.0F),
MakeSample(2, 2000, 3.0F),
};
const auto result = rm_control::modules::trajectory::ValidateDragTrajectory(
samples, MakePermissiveLimits());
context->Expect(result.ok, "valid seven-joint trajectory should pass");
if (!result.ok) {
return;
}
context->Expect(result.value.sample_count == 3, "report sample count should match input");
context->Expect(result.value.duration_ms == 2000,
"report duration should use elapsed milliseconds");
context->Expect(result.value.observed_maximum_joint_jump_deg == 2.0,
"report should contain two-degree maximum jump");
context->Expect(result.value.observed_maximum_joint_velocity_deg_per_second == 2.0,
"report should contain two-degree-per-second maximum velocity");
context->Expect(
result.value.observed_maximum_joint_acceleration_deg_per_second_squared == 1.0,
"report should calculate acceleration between interval midpoints");
}
void TestRejectsEmptyAndWrongJointCountTrajectories(TestContext *context) {
const std::vector<rm_control::common::TrajectorySample> empty_samples;
auto result = rm_control::modules::trajectory::ValidateDragTrajectory(
empty_samples, MakePermissiveLimits());
context->Expect(!result.ok, "empty trajectory must be rejected");
auto sample = MakeSample(0, 0, 0.0F);
sample.joints_deg.pop_back();
result = rm_control::modules::trajectory::ValidateDragTrajectory(
{sample}, MakePermissiveLimits());
context->Expect(!result.ok, "RM75 trajectory point with six joints must be rejected");
}
void TestRejectsNonMonotonicTimestamps(TestContext *context) {
const std::vector<rm_control::common::TrajectorySample> samples = {
MakeSample(0, 100, 0.0F),
MakeSample(1, 100, 1.0F),
};
const auto result = rm_control::modules::trajectory::ValidateDragTrajectory(
samples, MakePermissiveLimits());
context->Expect(!result.ok,
"equal timestamps must be rejected to prevent division by zero");
}
void TestRejectsNonFiniteJointAndTcpValues(TestContext *context) {
auto non_finite_joint = MakeSample(0, 0, 0.0F);
non_finite_joint.joints_deg[2] = std::numeric_limits<float>::quiet_NaN();
auto result = rm_control::modules::trajectory::ValidateDragTrajectory(
{non_finite_joint, MakeSample(1, 1000, 0.0F)}, MakePermissiveLimits());
context->Expect(!result.ok, "non-finite joint value must be rejected");
auto non_finite_pose = MakeSample(0, 0, 0.0F);
non_finite_pose.tcp_pose.z = std::numeric_limits<float>::infinity();
result = rm_control::modules::trajectory::ValidateDragTrajectory(
{non_finite_pose, MakeSample(1, 1000, 0.0F)}, MakePermissiveLimits());
context->Expect(!result.ok, "non-finite TCP pose value must be rejected");
}
void TestRejectsJumpVelocityAndAccelerationLimitViolations(TestContext *context) {
auto limits = MakePermissiveLimits();
limits.maximum_joint_jump_deg = 1.0;
auto result = rm_control::modules::trajectory::ValidateDragTrajectory(
{MakeSample(0, 0, 0.0F), MakeSample(1, 1000, 2.0F)}, limits);
context->Expect(!result.ok, "joint jump above configured maximum must be rejected");
limits = MakePermissiveLimits();
limits.maximum_joint_velocity_deg_per_second = 1.0;
result = rm_control::modules::trajectory::ValidateDragTrajectory(
{MakeSample(0, 0, 0.0F), MakeSample(1, 1000, 2.0F)}, limits);
context->Expect(!result.ok, "joint velocity above configured maximum must be rejected");
limits = MakePermissiveLimits();
limits.maximum_joint_acceleration_deg_per_second_squared = 1.0;
result = rm_control::modules::trajectory::ValidateDragTrajectory(
{MakeSample(0, 0, 0.0F), MakeSample(1, 1000, 0.0F),
MakeSample(2, 2000, 2.0F)},
limits);
context->Expect(!result.ok,
"joint acceleration above configured maximum must be rejected");
}
void TestRejectsInvalidLimits(TestContext *context) {
auto limits = MakePermissiveLimits();
limits.maximum_joint_jump_deg = 0.0;
const auto result = rm_control::modules::trajectory::ValidateDragTrajectory(
{MakeSample(0, 0, 0.0F)}, limits);
context->Expect(!result.ok, "zero safety limit must be rejected");
}
} // namespace
int main() {
TestContext context;
TestReportsPeakJointMotionForValidTrajectory(&context);
TestRejectsEmptyAndWrongJointCountTrajectories(&context);
TestRejectsNonMonotonicTimestamps(&context);
TestRejectsNonFiniteJointAndTcpValues(&context);
TestRejectsJumpVelocityAndAccelerationLimitViolations(&context);
TestRejectsInvalidLimits(&context);
if (context.failures == 0) {
std::cout << "[PASS] rm_drag_trajectory_validator_tests\n";
return 0;
}
std::cerr << context.failures << " test(s) failed\n";
return 1;
}
RM/tests/test_hardware_execution_gate.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/core/hardware_execution_gate.h"
#include <array>
#include <iostream>
#include <string>
namespace {
struct TestContext {
int failures = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures;
std::cerr << "[FAIL] " << message << '\n';
}
}
};
rm_control::core::HardwareExecutionGateInput MakeAllowedInput() {
rm_control::core::HardwareExecutionGateInput input;
input.live_requested = true;
input.hardware_execution_enabled = true;
input.arm_connected = true;
input.hand_connected = true;
input.emergency_stop_verified = true;
input.safety_limits_loaded = true;
input.operator_confirmed = true;
input.trajectory_validated = true;
input.trajectory_sample_period_confirmed = true;
input.vendor_raw_units_per_degree_confirmed = true;
input.workspace_and_virtual_wall_verified = true;
input.configured_max_speed_percent = 20;
input.native_replay_without_speed_control_accepted = true;
input.native_replay_speed_within_config_verified = true;
return input;
}
void TestAllowsExecutionWhenEveryBaseConditionIsSatisfied(TestContext *context) {
const auto decision =
rm_control::core::EvaluateHardwareExecutionGate(MakeAllowedInput());
context->Expect(decision.execution_allowed,
"complete base safety input should allow execution");
context->Expect(decision.rejection_reasons.empty(),
"allowed decision should contain no rejection");
}
void TestRejectsEachMissingBaseCondition(TestContext *context) {
using Input = rm_control::core::HardwareExecutionGateInput;
constexpr std::array<bool Input::*, 9> kRequiredConditions = {
&Input::live_requested,
&Input::hardware_execution_enabled,
&Input::arm_connected,
&Input::hand_connected,
&Input::emergency_stop_verified,
&Input::safety_limits_loaded,
&Input::operator_confirmed,
&Input::trajectory_validated,
&Input::workspace_and_virtual_wall_verified,
};
for (const auto required_condition : kRequiredConditions) {
auto input = MakeAllowedInput();
input.*required_condition = false;
const auto decision = rm_control::core::EvaluateHardwareExecutionGate(input);
context->Expect(!decision.execution_allowed,
"each missing base condition must reject execution");
context->Expect(!decision.rejection_reasons.empty(),
"a missing base condition must explain the rejection");
}
}
void TestRejectsUnconfirmedVendorMetadataWhenRequired(TestContext *context) {
rm_control::core::HardwareExecutionGateRequirements requirements;
requirements.require_confirmed_trajectory_sample_period = true;
requirements.require_confirmed_vendor_raw_units_per_degree = true;
auto input = MakeAllowedInput();
input.trajectory_sample_period_confirmed = false;
auto decision =
rm_control::core::EvaluateHardwareExecutionGate(input, requirements);
context->Expect(!decision.execution_allowed,
"required unconfirmed sample period must reject playback");
input = MakeAllowedInput();
input.vendor_raw_units_per_degree_confirmed = false;
decision = rm_control::core::EvaluateHardwareExecutionGate(input, requirements);
context->Expect(!decision.execution_allowed,
"required unconfirmed raw-unit ratio must reject playback");
}
void TestRejectsNativeReplayWithoutExplicitAcceptance(TestContext *context) {
auto input = MakeAllowedInput();
input.native_replay_without_speed_control_accepted = false;
const rm_control::core::HardwareExecutionGateRequirements requirements{
0, true};
const auto decision =
rm_control::core::EvaluateHardwareExecutionGate(input, requirements);
context->Expect(!decision.execution_allowed,
"native replay must require acceptance of missing speed control");
}
void TestRejectsNativeReplayWithoutIndependentSpeedVerification(
TestContext *context) {
auto input = MakeAllowedInput();
input.native_replay_speed_within_config_verified = false;
const rm_control::core::HardwareExecutionGateRequirements requirements{
0, true, true};
const auto decision =
rm_control::core::EvaluateHardwareExecutionGate(input, requirements);
context->Expect(!decision.execution_allowed,
"native replay must verify actual speed against the configured limit");
}
void TestAllowsNativeOperationsWhenTheirSpecificRequirementsAreSatisfied(
TestContext *context) {
const auto input = MakeAllowedInput();
const rm_control::core::HardwareExecutionGateRequirements origin_requirements{
rm_control::core::kNativeTrajectoryOriginRequiredSpeedPercent, false};
const rm_control::core::HardwareExecutionGateRequirements replay_requirements{0, true};
const auto origin_decision =
rm_control::core::EvaluateHardwareExecutionGate(input, origin_requirements);
const auto replay_decision =
rm_control::core::EvaluateHardwareExecutionGate(input, replay_requirements);
context->Expect(origin_decision.execution_allowed,
"native origin motion should pass at the configured 20 percent limit");
context->Expect(replay_decision.execution_allowed,
"native replay should pass after accepting its missing speed control");
}
void TestRejectsNativeOriginWhenRequiredSpeedExceedsConfiguredMaximum(
TestContext *context) {
auto input = MakeAllowedInput();
input.configured_max_speed_percent = 10;
const rm_control::core::HardwareExecutionGateRequirements requirements{
rm_control::core::kNativeTrajectoryOriginRequiredSpeedPercent, false};
const auto result =
rm_control::core::RequireHardwareExecutionPermission(input, requirements);
context->Expect(!result.ok,
"native 20 percent origin motion must fail below configured maximum");
context->Expect(result.error_message.find("exceeds configured maximum") !=
std::string::npos,
"speed rejection should include actionable context");
}
void TestRejectsInvalidSpeedRanges(TestContext *context) {
auto input = MakeAllowedInput();
input.configured_max_speed_percent = 101;
auto decision = rm_control::core::EvaluateHardwareExecutionGate(input);
context->Expect(!decision.execution_allowed,
"configured speed above 100 must be rejected");
input = MakeAllowedInput();
const rm_control::core::HardwareExecutionGateRequirements requirements{-1, false};
decision = rm_control::core::EvaluateHardwareExecutionGate(input, requirements);
context->Expect(!decision.execution_allowed,
"negative required speed must be rejected");
}
} // namespace
int main() {
TestContext context;
TestAllowsExecutionWhenEveryBaseConditionIsSatisfied(&context);
TestRejectsEachMissingBaseCondition(&context);
TestRejectsUnconfirmedVendorMetadataWhenRequired(&context);
TestRejectsNativeReplayWithoutExplicitAcceptance(&context);
TestRejectsNativeReplayWithoutIndependentSpeedVerification(&context);
TestAllowsNativeOperationsWhenTheirSpecificRequirementsAreSatisfied(&context);
TestRejectsNativeOriginWhenRequiredSpeedExceedsConfiguredMaximum(&context);
TestRejectsInvalidSpeedRanges(&context);
if (context.failures == 0) {
std::cout << "[PASS] rm_hardware_execution_gate_tests\n";
return 0;
}
std::cerr << context.failures << " test(s) failed\n";
return 1;
}
RM/tests/test_linker_hand_adapter.cpp 0 → 100644
View file @ 8275ee2e
#include "mocks/mock_dexterous_hand.h"
#include "mocks/mock_tool_modbus_bus.h"
#include "flight_robot/linker_hand/linker_hand_adapter.h"
#include <cstdint>
#include <initializer_list>
#include <iostream>
#include <string>
#include <vector>
namespace {
using rm_control::common::Result;
using rm_control::interfaces::DexterousHandSide;
using rm_control::interfaces::O6JointPose;
using rm_control::interfaces::O6MotionCommand;
using rm_control::tests::MockToolModbusBus;
constexpr std::uint16_t kPositionStartAddress = 0U;
constexpr std::uint16_t kTorqueStartAddress = 6U;
constexpr std::uint16_t kSpeedStartAddress = 12U;
constexpr std::uint16_t kTemperatureStartAddress = 18U;
constexpr std::uint16_t kFaultStartAddress = 24U;
constexpr std::uint16_t kIdentityStartAddress = 30U;
struct TestContext {
int failures = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures;
std::cerr << "[FAIL] " << message << '\n';
}
}
};
Result<std::vector<std::uint16_t>> RegisterValues(
std::initializer_list<std::uint16_t> values) {
return Result<std::vector<std::uint16_t>>::success(
std::vector<std::uint16_t>(values));
}
O6MotionCommand ValidMotionCommand() {
return O6MotionCommand{
O6JointPose{0U, 1U, 64U, 128U, 192U, 255U},
O6JointPose{10U, 11U, 12U, 13U, 14U, 15U},
O6JointPose{20U, 21U, 22U, 23U, 24U, 25U}};
}
void ConfigureValidRightHand(MockToolModbusBus *tool_bus) {
tool_bus->input_register_results[kIdentityStartAddress] =
RegisterValues({6U, 101U, 1U, 2U, 3U, static_cast<std::uint16_t>('R')});
tool_bus->input_register_results[kPositionStartAddress] =
RegisterValues({0U, 1U, 64U, 128U, 192U, 255U});
tool_bus->input_register_results[kTorqueStartAddress] =
RegisterValues({30U, 31U, 32U, 33U, 34U, 35U});
tool_bus->input_register_results[kSpeedStartAddress] =
RegisterValues({40U, 41U, 42U, 43U, 44U, 45U});
tool_bus->input_register_results[kTemperatureStartAddress] =
RegisterValues({20U, 21U, 22U, 23U, 24U, 25U});
tool_bus->input_register_results[kFaultStartAddress] =
RegisterValues({0U, 0U, 0U, 0U, 0U, 0U});
}
void TestValidRightHandReadiness(TestContext *context) {
MockToolModbusBus tool_bus;
ConfigureValidRightHand(&tool_bus);
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Right);
const auto result = adapter.ReadReadiness();
context->Expect(result.ok, "valid right-hand readiness should succeed");
if (!result.ok) {
return;
}
context->Expect(result.value.ready, "successful readiness must set ready=true");
context->Expect(result.value.device_id == 0x27U,
"right-hand device ID must be 0x27");
context->Expect(result.value.freedom == 6U && result.value.direction == 'R',
"identity must preserve freedom and right direction");
context->Expect(result.value.joint_positions[5] == 255U,
"position register order must be preserved");
context->Expect(result.value.joint_torques[0] == 30U &&
result.value.joint_torques[5] == 35U,
"torque input register order must be preserved");
context->Expect(result.value.joint_speeds[0] == 40U &&
result.value.joint_speeds[5] == 45U,
"speed input register order must be preserved");
context->Expect(tool_bus.status_calls == 1 &&
tool_bus.single_read_calls.size() == 36U &&
tool_bus.read_calls.empty(),
"readiness must query status and 36 single FC04 registers");
for (const auto &call : tool_bus.single_read_calls) {
context->Expect(call.device_id == 0x27U && call.register_count == 1U,
"each right-hand read must use ID 0x27 and one register");
}
}
void TestDirectionMismatchRejected(TestContext *context) {
MockToolModbusBus tool_bus;
ConfigureValidRightHand(&tool_bus);
tool_bus.input_register_results[kIdentityStartAddress] =
RegisterValues({6U, 101U, 1U, 2U, 3U, static_cast<std::uint16_t>('L')});
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Right);
const auto result = adapter.ReadReadiness();
context->Expect(!result.ok, "right-hand adapter must reject identity direction L");
context->Expect(result.error_message.find("direction") != std::string::npos,
"direction rejection should include field context");
context->Expect(tool_bus.single_read_calls.size() == 6U,
"identity mismatch must stop without extra Modbus reads");
}
void TestValidLeftHandUsesLeftIdentityAndDeviceId(TestContext *context) {
MockToolModbusBus tool_bus;
ConfigureValidRightHand(&tool_bus);
tool_bus.input_register_results[kIdentityStartAddress] =
RegisterValues({6U, 101U, 1U, 2U, 3U, static_cast<std::uint16_t>('L')});
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Left);
const auto result = adapter.ReadReadiness();
context->Expect(result.ok && result.value.device_id == 0x28U,
"valid left hand must use Modbus device ID 0x28");
context->Expect(result.ok && result.value.direction == 'L',
"valid left-hand identity must preserve direction L");
for (const auto &call : tool_bus.single_read_calls) {
context->Expect(call.device_id == 0x28U,
"each left-hand read must use device ID 0x28");
}
}
void TestFaultRejected(TestContext *context) {
MockToolModbusBus tool_bus;
ConfigureValidRightHand(&tool_bus);
tool_bus.input_register_results[kFaultStartAddress] =
RegisterValues({0U, 0U, 7U, 0U, 0U, 0U});
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Right);
const auto result = adapter.ReadReadiness();
context->Expect(!result.ok, "nonzero O6 fault register must reject readiness");
context->Expect(result.error_message.find("code=7") != std::string::npos,
"fault rejection should include the device fault code");
}
void TestTemperatureRejected(TestContext *context) {
MockToolModbusBus tool_bus;
ConfigureValidRightHand(&tool_bus);
tool_bus.input_register_results[kTemperatureStartAddress] =
RegisterValues({20U, 21U, 22U, 71U, 24U, 25U});
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Right);
const auto result = adapter.ReadReadiness();
context->Expect(!result.ok, "temperature above 70 Celsius must reject readiness");
context->Expect(result.error_message.find("temperature") != std::string::npos,
"temperature rejection should include field context");
}
void TestRegisterDimensionRejected(TestContext *context) {
MockToolModbusBus tool_bus;
ConfigureValidRightHand(&tool_bus);
tool_bus.input_register_results[kPositionStartAddress] =
RegisterValues({0U, 1U, 2U, 3U, 4U});
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Right);
const auto result = adapter.ReadReadiness();
context->Expect(!result.ok, "five position registers must reject O6 readiness");
context->Expect(result.error_message.find("address 5") !=
std::string::npos,
"missing sixth register should include its exact address");
}
void TestInvalidToolBusStatusRejectedBeforeRead(TestContext *context) {
MockToolModbusBus tool_bus;
ConfigureValidRightHand(&tool_bus);
tool_bus.status_result =
Result<rm_control::interfaces::ToolModbusBusStatus>::success(
rm_control::interfaces::ToolModbusBusStatus{
true, 1, 1, 57600, 3, 10});
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Right);
const auto result = adapter.ReadReadiness();
context->Expect(!result.ok, "non-115200 tool bus must reject readiness");
context->Expect(tool_bus.read_calls.empty() && tool_bus.single_read_calls.empty(),
"invalid tool bus must be rejected before Modbus reads");
}
bool WasHoldingRegisterAddressWritten(const MockToolModbusBus &tool_bus,
std::uint16_t address) {
for (const auto &call : tool_bus.write_calls) {
if (call.start_address == address) {
return true;
}
}
return false;
}
void TestValidMotionUsesVerifiedSafetyConfigurationBeforePosition(
TestContext *context) {
MockToolModbusBus tool_bus;
const O6MotionCommand command = ValidMotionCommand();
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Right);
const auto result = adapter.CommandMotion(command);
context->Expect(result.ok, "valid O6 motion command should succeed");
context->Expect(
tool_bus.operation_calls ==
(std::vector<std::string>{"write_holding:6", "write_holding:12",
"write_holding:0"}),
"motion must write torque, speed, then position without false FC04 command readback");
context->Expect(tool_bus.single_read_calls.empty(),
"command path must not confuse live FC04 state with FC16 command values");
context->Expect(tool_bus.write_calls.size() == 3U,
"valid motion must write each register group exactly once");
for (const auto &call : tool_bus.write_calls) {
context->Expect(call.device_id == 0x27U,
"all right-hand writes must use Modbus device ID 0x27");
}
if (tool_bus.write_calls.size() == 3U) {
context->Expect(
tool_bus.write_calls[0].values == std::vector<std::uint16_t>(
command.joint_torque_commands.begin(),
command.joint_torque_commands.end()),
"torque write must preserve all six values");
context->Expect(
tool_bus.write_calls[1].values == std::vector<std::uint16_t>(
command.joint_speed_commands.begin(),
command.joint_speed_commands.end()),
"speed write must preserve all six values");
context->Expect(
tool_bus.write_calls[2].values == std::vector<std::uint16_t>(
command.target_positions.begin(), command.target_positions.end()),
"position write must preserve all six values");
}
}
void TestOutOfRangeCommandRejectedWithoutWrite(TestContext *context) {
MockToolModbusBus tool_bus;
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Left);
O6MotionCommand command = ValidMotionCommand();
command.joint_speed_commands[2] = 256U;
const auto result = adapter.CommandMotion(command);
context->Expect(!result.ok, "O6 command value 256 must be rejected");
context->Expect(tool_bus.write_calls.empty(),
"out-of-range O6 command must not reach the bus");
context->Expect(tool_bus.read_calls.empty() && tool_bus.single_read_calls.empty(),
"out-of-range O6 command must not read the bus");
}
void TestTorqueWriteFailureStopsWithoutRetryOrPosition(TestContext *context) {
MockToolModbusBus tool_bus;
const O6MotionCommand command = ValidMotionCommand();
tool_bus.write_results_by_start_address[kTorqueStartAddress] =
Result<void>::failure("mock torque write timeout");
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Right);
const auto result = adapter.CommandMotion(command);
context->Expect(!result.ok, "torque write failure must be returned to the caller");
context->Expect(tool_bus.write_calls.size() == 1U,
"failed torque write must not be retried automatically");
context->Expect(!WasHoldingRegisterAddressWritten(tool_bus, kPositionStartAddress),
"torque write failure must prevent position write");
context->Expect(result.error_message.find("mock torque write timeout") !=
std::string::npos,
"torque write failure should preserve bus error context");
}
void TestSpeedWriteFailureStopsWithoutRetryOrPosition(TestContext *context) {
MockToolModbusBus tool_bus;
const O6MotionCommand command = ValidMotionCommand();
tool_bus.write_results_by_start_address[kSpeedStartAddress] =
Result<void>::failure("mock speed write timeout");
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Right);
const auto result = adapter.CommandMotion(command);
context->Expect(!result.ok, "speed write failure must be returned");
context->Expect(tool_bus.operation_calls ==
(std::vector<std::string>{"write_holding:6",
"write_holding:12"}),
"speed write failure must stop immediately without retry");
context->Expect(!WasHoldingRegisterAddressWritten(tool_bus, kPositionStartAddress),
"speed write failure must prevent position write");
}
void TestPositionWriteFailureIsNotRetried(TestContext *context) {
MockToolModbusBus tool_bus;
const O6MotionCommand command = ValidMotionCommand();
tool_bus.write_results_by_start_address[kPositionStartAddress] =
Result<void>::failure("mock position write timeout");
flight_robot::linker_hand::LinkerHandAdapter adapter(
tool_bus, DexterousHandSide::Right);
const auto result = adapter.CommandMotion(command);
context->Expect(!result.ok, "position write failure must be returned");
context->Expect(tool_bus.write_calls.size() == 3U,
"failed final position write must not be retried");
context->Expect(result.error_message.find("mock position write timeout") !=
std::string::npos,
"position write failure must preserve bus error context");
}
void TestMockDexterousHandRecordsCommand(TestContext *context) {
rm_control::tests::MockDexterousHand hand;
const O6MotionCommand command = ValidMotionCommand();
const auto result = hand.CommandMotion(command);
context->Expect(result.ok && hand.command_calls == 1,
"MockDexterousHand should report and count a configured command");
context->Expect(
hand.last_motion_command.target_positions == command.target_positions &&
hand.last_motion_command.joint_speed_commands ==
command.joint_speed_commands &&
hand.last_motion_command.joint_torque_commands ==
command.joint_torque_commands,
"MockDexterousHand should preserve the complete last motion command");
}
} // namespace
int main() {
TestContext context;
TestValidRightHandReadiness(&context);
TestDirectionMismatchRejected(&context);
TestValidLeftHandUsesLeftIdentityAndDeviceId(&context);
TestFaultRejected(&context);
TestTemperatureRejected(&context);
TestRegisterDimensionRejected(&context);
TestInvalidToolBusStatusRejectedBeforeRead(&context);
TestValidMotionUsesVerifiedSafetyConfigurationBeforePosition(&context);
TestOutOfRangeCommandRejectedWithoutWrite(&context);
TestTorqueWriteFailureStopsWithoutRetryOrPosition(&context);
TestSpeedWriteFailureStopsWithoutRetryOrPosition(&context);
TestPositionWriteFailureIsNotRetried(&context);
TestMockDexterousHandRecordsCommand(&context);
if (context.failures == 0) {
std::cout << "[PASS] rm_linker_hand_adapter_tests\n";
return 0;
}
std::cerr << context.failures << " test(s) failed\n";
return 1;
}
RM/tests/test_modbus_register_codec.cpp 0 → 100644
View file @ 8275ee2e
#include "flight_robot/realman/modbus_register_codec.h"
#include <cstdint>
#include <iostream>
#include <string>
#include <vector>
namespace {
struct TestContext {
int failures = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures;
std::cerr << "[FAIL] " << message << '\n';
}
}
};
void TestEncodePreservesZeroByteAndUint16Boundaries(TestContext *context) {
const std::vector<std::uint16_t> registers{
0x0000U, 0x00FFU, 0x0100U, 0xFFFFU};
const auto result =
flight_robot::realman::EncodeModbusRegistersAsRmApiHighLowBytes(
registers);
context->Expect(result.ok, "boundary registers should encode successfully");
context->Expect(
result.ok && result.value ==
(std::vector<int>{0x00, 0x00, 0x00, 0xFF,
0x01, 0x00, 0xFF, 0xFF}),
"registers must encode as num * 2 high-low bytes without truncation");
}
void TestDecodeUnsignedBytesPreservesRegisterBoundaries(TestContext *context) {
const std::vector<int> bytes{
0x00, 0x00, 0x00, 0xFF, 0x01, 0x00, 0xFF, 0xFF};
const auto result =
flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes(
bytes);
context->Expect(result.ok, "unsigned RM API bytes should decode");
context->Expect(
result.ok && result.value ==
(std::vector<std::uint16_t>{0x0000U, 0x00FFU, 0x0100U, 0xFFFFU}),
"unsigned high-low byte pairs must decode to exact uint16 values");
}
void TestDecodeSignedInt8BytesPreservesRawBits(TestContext *context) {
const std::vector<int> bytes{0, -1, -128, 0, -1, -1};
const auto result =
flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes(
bytes);
context->Expect(result.ok, "signed int8 RM API bytes should decode");
context->Expect(
result.ok && result.value ==
(std::vector<std::uint16_t>{0x00FFU, 0x8000U, 0xFFFFU}),
"signed int8 bytes must normalize without changing their raw bits");
}
void TestEmptyEncodeAndDecodeAreRejected(TestContext *context) {
const auto encoded =
flight_robot::realman::EncodeModbusRegistersAsRmApiHighLowBytes({});
const auto decoded =
flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes({});
context->Expect(!encoded.ok, "empty register sequence must not encode");
context->Expect(!decoded.ok, "empty byte sequence must not decode");
}
void TestOddByteCountIsRejected(TestContext *context) {
const auto result =
flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes(
std::vector<int>{0, 1, 2});
context->Expect(!result.ok, "odd RM API byte count must be rejected");
context->Expect(result.error_message.find("even byte count") !=
std::string::npos,
"odd byte count failure must include dimension context");
}
void TestByteBelowSignedInt8RangeIsRejected(TestContext *context) {
const auto result =
flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes(
std::vector<int>{-129, 0});
context->Expect(!result.ok, "RM API byte -129 must be rejected");
context->Expect(result.error_message.find("index 0") != std::string::npos,
"invalid low-bound byte must report its index");
}
void TestByteAboveUnsignedByteRangeIsRejected(TestContext *context) {
const auto result =
flight_robot::realman::DecodeModbusRegistersFromRmApiHighLowBytes(
std::vector<int>{0, 256});
context->Expect(!result.ok, "RM API byte 256 must be rejected");
context->Expect(result.error_message.find("index 1") != std::string::npos,
"invalid high-bound byte must report its index");
}
} // namespace
int main() {
TestContext context;
TestEncodePreservesZeroByteAndUint16Boundaries(&context);
TestDecodeUnsignedBytesPreservesRegisterBoundaries(&context);
TestDecodeSignedInt8BytesPreservesRawBits(&context);
TestEmptyEncodeAndDecodeAreRejected(&context);
TestOddByteCountIsRejected(&context);
TestByteBelowSignedInt8RangeIsRejected(&context);
TestByteAboveUnsignedByteRangeIsRejected(&context);
if (context.failures == 0) {
std::cout << "[PASS] rm_modbus_register_codec_tests\n";
return 0;
}
std::cerr << context.failures << " test(s) failed\n";
return 1;
}
RM/tests/test_o6_bus_scanner.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/core/o6_bus_scanner.h"
#include <cstdint>
#include <iostream>
#include <string>
#include <vector>
namespace {
class FakeScanningToolBus final : public rm_control::interfaces::IToolModbusBus {
public:
std::vector<std::uint8_t> requested_device_ids;
std::vector<std::string> operations;
rm_control::common::Result<rm_control::interfaces::ToolModbusBusStatus>
Configure(const rm_control::interfaces::ToolModbusBusConfiguration &) override {
return GetStatus();
}
rm_control::common::Result<rm_control::interfaces::ToolModbusBusStatus>
GetStatus() override {
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::success(
{true, 1, 1, 115200, 3, 1});
}
rm_control::common::Result<std::uint16_t> ReadInputRegister(
std::uint8_t device_id,
std::uint16_t) override {
operations.push_back("single:" + std::to_string(device_id));
if (device_id == 0x27U) {
return rm_control::common::Result<std::uint16_t>::success(6U);
}
return rm_control::common::Result<std::uint16_t>::failure(
"mock single read timeout");
}
rm_control::common::Result<std::vector<std::uint16_t>> ReadInputRegisters(
std::uint8_t device_id,
std::uint16_t,
std::size_t) override {
requested_device_ids.push_back(device_id);
operations.push_back("multiple:" + std::to_string(device_id));
if (device_id == 0x27U) {
return rm_control::common::Result<std::vector<std::uint16_t>>::success(
{6U, 101U, 1U, 2U, 3U, static_cast<std::uint16_t>('R')});
}
return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
"mock no response");
}
rm_control::common::Result<std::vector<std::uint16_t>> ReadHoldingRegisters(
std::uint8_t, std::uint16_t, std::size_t) override {
return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
"not used by scanner");
}
rm_control::common::Result<void> WriteHoldingRegisters(
std::uint8_t,
std::uint16_t,
const std::vector<std::uint16_t> &) override {
return rm_control::common::Result<void>::failure(
"scanner must never write");
}
};
int TestScanCollectsResponsesAndMarksO6() {
FakeScanningToolBus bus;
rm_control::core::O6BusScanConfig config;
const auto result = rm_control::core::ScanO6IdentityRegisters(bus, config);
if (!result.ok || result.value.attempted_device_count != 2U ||
result.value.failed_device_count != 1U ||
result.value.confirmed_o6_device_count != 1U ||
result.value.responses.size() != 2U ||
!result.value.responses[0].matches_o6_identity_shape ||
result.value.responses[1].matches_o6_identity_shape ||
result.value.responses[0].device_id != 0x27U ||
result.value.responses[1].device_id != 0x28U ||
bus.operations !=
(std::vector<std::string>{"multiple:39", "single:39",
"multiple:40", "single:40"})) {
std::cerr << "[FAIL] probe should use both FC04 paths once per O6 ID\n";
return 1;
}
return 0;
}
int TestInvalidRangeIsRejectedWithoutRead() {
FakeScanningToolBus bus;
rm_control::core::O6BusScanConfig config;
config.device_ids = {0x27U};
const auto result = rm_control::core::ScanO6IdentityRegisters(bus, config);
if (result.ok || !bus.requested_device_ids.empty()) {
std::cerr << "[FAIL] invalid scan range must fail before bus access\n";
return 1;
}
return 0;
}
} // namespace
int main() {
const int failures = TestScanCollectsResponsesAndMarksO6() +
TestInvalidRangeIsRejectedWithoutRead();
if (failures == 0) {
std::cout << "[PASS] rm_o6_bus_scanner_tests\n";
return 0;
}
return 1;
}
RM/tests/test_raw_trajectory_store.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/config/simple_json.h"
#include "rm_control/modules/trajectory/raw_trajectory_store.h"
#include "rm_control/modules/trajectory/sha256_file_checksum.h"
#include <filesystem>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#ifndef RM_TEST_OUTPUT_DIR
#error "RM_TEST_OUTPUT_DIR must be defined so tests cannot write into the source tree"
#endif
namespace {
namespace trajectory = rm_control::modules::trajectory;
class RawTrajectoryStoreTestSuite {
public:
int Run() {
std::error_code filesystem_error;
std::filesystem::create_directories(TestOutputRoot(), filesystem_error);
Expect(!filesystem_error,
"test output root should be created under RM_TEST_OUTPUT_DIR: " +
filesystem_error.message());
TestSanitizeSessionName();
TestSha256StandardVectors();
TestUniqueSessionLayoutDoesNotOverwrite();
TestFinalizeSuccessfulVendorArtifact();
TestFinalizeVendorSaveFailureStillWritesManifest();
return failures_;
}
private:
int failures_ = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures_;
std::cerr << "[FAIL] " << message << '\n';
}
}
std::filesystem::path TestOutputRoot() {
return std::filesystem::path(RM_TEST_OUTPUT_DIR) / "raw_trajectory_store_tests";
}
bool WriteBinaryFile(const std::filesystem::path &path, const std::string &content) {
std::ofstream output(path, std::ios::binary | std::ios::trunc);
output.write(content.data(), static_cast<std::streamsize>(content.size()));
return static_cast<bool>(output);
}
std::string ReadTextFile(const std::filesystem::path &path) {
std::ifstream input(path);
std::ostringstream contents;
contents << input.rdbuf();
return contents.str();
}
trajectory::RawTrajectorySessionPaths CreateTestSession(const std::string &group,
const std::string &name) {
const auto result = trajectory::CreateUniqueRawTrajectorySession(
TestOutputRoot() / group, name);
Expect(result.ok, "unique raw trajectory session should be created: " +
result.error_message);
return result.value;
}
void TestSanitizeSessionName() {
Expect(trajectory::SanitizeRawTrajectorySessionName(" flight::path // alpha ") ==
"flight_path_alpha",
"illegal character runs should collapse to one underscore");
Expect(trajectory::SanitizeRawTrajectorySessionName("con") == "trajectory_con",
"Windows reserved names should receive a safe prefix");
Expect(trajectory::SanitizeRawTrajectorySessionName("飞行轨迹") == "trajectory",
"a name without ASCII filename characters should use the fallback");
Expect(trajectory::SanitizeRawTrajectorySessionName(std::string(80U, 'a')).size() == 64U,
"sanitized session names should be limited to 64 characters");
}
void TestSha256StandardVectors() {
const auto paths = CreateTestSession("sha256", "standard vectors");
if (paths.directory.empty()) {
return;
}
const std::filesystem::path empty_file = paths.directory / "empty.bin";
const std::filesystem::path abc_file = paths.directory / "abc.bin";
Expect(WriteBinaryFile(empty_file, ""), "empty SHA-256 vector should be written");
Expect(WriteBinaryFile(abc_file, "abc"), "abc SHA-256 vector should be written");
const auto empty_digest = trajectory::CalculateFileSha256(empty_file);
Expect(empty_digest.ok, "empty file SHA-256 should succeed");
if (empty_digest.ok) {
Expect(empty_digest.value ==
"e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"empty file SHA-256 should match the standard vector");
}
const auto abc_digest = trajectory::CalculateFileSha256(abc_file);
Expect(abc_digest.ok, "abc SHA-256 should succeed");
if (abc_digest.ok) {
Expect(abc_digest.value ==
"ba7816bf8f01cfea414140de5dae2223b00361a396177a9cb410ff61f20015ad",
"abc SHA-256 should match the standard vector");
}
}
void TestUniqueSessionLayoutDoesNotOverwrite() {
const auto first = CreateTestSession("unique", "same / session");
const auto second = CreateTestSession("unique", "same / session");
if (first.directory.empty() || second.directory.empty()) {
return;
}
Expect(first.directory != second.directory,
"two reservations for the same name should use different directories");
Expect(std::filesystem::is_directory(first.directory),
"first unique session directory should exist");
Expect(std::filesystem::is_directory(second.directory),
"second unique session directory should exist");
Expect(first.raw_trajectory.filename() == "raw_trajectory.txt",
"raw trajectory filename should be fixed");
Expect(first.synchronized_csv.filename() == "synchronized_samples.csv",
"synchronized sample filename should be fixed");
Expect(first.manifest.filename() == "manifest.json",
"manifest filename should be fixed");
Expect(first.directory.parent_path().filename() == "sessions",
"session directories should be placed under the sessions folder");
}
void TestFinalizeSuccessfulVendorArtifact() {
const auto paths = CreateTestSession("finalize_success", "RM75 raw success");
if (paths.directory.empty()) {
return;
}
Expect(WriteBinaryFile(paths.raw_trajectory, "abc"),
"raw vendor trajectory should be written by the test fixture");
Expect(WriteBinaryFile(paths.synchronized_csv, "sample_index,timestamp\n"),
"synchronized CSV should be written by the test fixture");
trajectory::RawTrajectoryManifestMetadata metadata;
metadata.robot_model = "RM75-6F";
metadata.sdk_version = "RM_API2 test";
metadata.firmware_version = "controller test";
metadata.trajectory_point_count = 12U;
metadata.recorded_sample_count = 2U;
metadata.vendor_save_api_called = true;
metadata.vendor_save_return_code = 0;
metadata.synchronized_csv_write_succeeded = true;
const auto result = trajectory::FinalizeRawTrajectorySession(paths, metadata);
Expect(result.ok, "successful raw artifact should be finalized: " +
result.error_message);
if (!result.ok) {
return;
}
Expect(result.value.raw_file_present, "successful raw file should be marked present");
Expect(result.value.raw_file_size_bytes == 3U, "raw file size should be recorded");
Expect(result.value.raw_file_sha256 ==
"ba7816bf8f01cfea414140de5dae2223b00361a396177a9cb410ff61f20015ad",
"raw file SHA-256 should be recorded");
Expect(std::filesystem::is_regular_file(paths.manifest),
"manifest should be atomically published");
Expect(!std::filesystem::exists(paths.directory / "manifest.json.tmp"),
"temporary manifest should not remain after publication");
const std::string manifest = ReadTextFile(paths.manifest);
const auto parsed_manifest = rm_control::config::parseJson(manifest);
Expect(parsed_manifest.ok && parsed_manifest.value.isObject(),
"manifest should be valid JSON");
Expect(manifest.find("\"robot_model\": \"RM75-6F\"") != std::string::npos,
"manifest should record robot model");
Expect(manifest.find("\"trajectory_point_count\": 12") != std::string::npos,
"manifest should record trajectory point count");
Expect(manifest.find("\"raw_file_size_bytes\": 3") != std::string::npos,
"manifest should record raw file size");
Expect(manifest.find("\"save_interface_return_code\": 0") != std::string::npos,
"manifest should record vendor save return code");
Expect(manifest.find("\"synchronized_timestamp_is_derived\": false") !=
std::string::npos,
"manifest should record the synchronized timestamp provenance");
constexpr std::filesystem::perms kAllWritePermissions =
std::filesystem::perms::owner_write | std::filesystem::perms::group_write |
std::filesystem::perms::others_write;
const std::filesystem::perms raw_permissions =
std::filesystem::status(paths.raw_trajectory).permissions();
Expect((raw_permissions & kAllWritePermissions) == std::filesystem::perms::none,
"finalized raw trajectory should have all write permissions removed");
const auto second_finalize = trajectory::FinalizeRawTrajectorySession(paths, metadata);
Expect(!second_finalize.ok, "finalization should refuse to overwrite an existing manifest");
Expect(ReadTextFile(paths.manifest) == manifest,
"refused second finalization should leave the original manifest unchanged");
}
void TestFinalizeVendorSaveFailureStillWritesManifest() {
const auto paths = CreateTestSession("finalize_failure", "RM75 raw failure");
if (paths.directory.empty()) {
return;
}
trajectory::RawTrajectoryManifestMetadata metadata;
metadata.robot_model = "RM75-6F";
metadata.sdk_version = "RM_API2 test";
metadata.firmware_version = "controller test";
metadata.recorded_sample_count = 5U;
metadata.vendor_save_api_called = true;
metadata.vendor_save_return_code = -2;
metadata.vendor_save_error_message = "mock timeout";
const auto result = trajectory::FinalizeRawTrajectorySession(paths, metadata);
Expect(result.ok, "failed vendor save attempt should still finalize its manifest: " +
result.error_message);
if (!result.ok) {
return;
}
Expect(!result.value.raw_file_present,
"failed vendor save without output should be marked absent");
Expect(result.value.raw_file_size_bytes == 0U,
"absent raw file should have zero recorded size");
Expect(result.value.raw_file_sha256.empty(),
"absent raw file should have an empty SHA-256 field");
const std::string manifest = ReadTextFile(paths.manifest);
Expect(manifest.find("\"raw_file_present\": false") != std::string::npos,
"failure manifest should record that the raw file is absent");
Expect(manifest.find("\"save_interface_return_code\": -2") != std::string::npos,
"failure manifest should preserve the vendor return code");
}
};
} // namespace
int main() {
RawTrajectoryStoreTestSuite suite;
const int failures = suite.Run();
if (failures == 0) {
std::cout << "[PASS] rm_raw_trajectory_store_tests\n";
return 0;
}
std::cerr << failures << " test(s) failed\n";
return 1;
}
RM/tests/test_reversed_vendor_trajectory_store.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/modules/trajectory/reversed_vendor_trajectory_store.h"
#include <chrono>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <iterator>
#include <string>
#include <system_error>
#ifndef RM_TEST_OUTPUT_DIR
#define RM_TEST_OUTPUT_DIR "build/test_outputs"
#endif
namespace {
struct TestContext {
int failures = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures;
std::cerr << "[FAIL] " << message << '\n';
}
}
};
const std::string &FirstLine() {
static const std::string line = "{\"point\":[0,0,0,0,0,0,0]}";
return line;
}
const std::string &SecondLine() {
static const std::string line = "{\"point\":[100,200,300,400,500,600,700]}";
return line;
}
const std::string &ThirdLine() {
static const std::string line = "{\"point\":[1000,2000,3000,4000,5000,6000,7000]}";
return line;
}
std::string ForwardCrLfContent() {
return FirstLine() + "\r\n" + SecondLine() + "\r\n" + ThirdLine() + "\r\n";
}
std::string ReversedCrLfContent() {
return ThirdLine() + "\r\n" + SecondLine() + "\r\n" + FirstLine() + "\r\n";
}
std::filesystem::path TestOutputRoot() {
static const std::filesystem::path root = [] {
const auto unique_suffix =
std::chrono::steady_clock::now().time_since_epoch().count();
const std::filesystem::path path = std::filesystem::path(RM_TEST_OUTPUT_DIR) /
("reversed_vendor_trajectory_store_" + std::to_string(unique_suffix));
std::error_code filesystem_error;
std::filesystem::create_directories(path, filesystem_error);
return path;
}();
return root;
}
bool WriteBinaryFile(const std::filesystem::path &path, const std::string &content) {
std::ofstream output(path, std::ios::binary | std::ios::out | std::ios::trunc);
output.write(content.data(), static_cast<std::streamsize>(content.size()));
output.close();
return static_cast<bool>(output);
}
std::string ReadBinaryFile(const std::filesystem::path &path) {
std::ifstream input(path, std::ios::binary);
return std::string(std::istreambuf_iterator<char>(input),
std::istreambuf_iterator<char>());
}
bool SetReadOnly(const std::filesystem::path &path) {
std::error_code filesystem_error;
const auto permissions = std::filesystem::perms::owner_read |
std::filesystem::perms::group_read | std::filesystem::perms::others_read;
std::filesystem::permissions(
path, permissions, std::filesystem::perm_options::replace, filesystem_error);
return !filesystem_error;
}
bool HasNoWritePermission(const std::filesystem::path &path) {
std::error_code filesystem_error;
const auto status = std::filesystem::symlink_status(path, filesystem_error);
const auto writes = std::filesystem::perms::owner_write |
std::filesystem::perms::group_write | std::filesystem::perms::others_write;
return !filesystem_error && (status.permissions() & writes) == std::filesystem::perms::none;
}
rm_control::modules::trajectory::VendorDragTrajectoryParseResult CreateForwardFixture(
const std::string &filename,
const std::string &content = ForwardCrLfContent()) {
const auto path = TestOutputRoot() / filename;
if (!WriteBinaryFile(path, content)) {
std::cerr << "[FAIL] failed to write forward fixture " << path << '\n';
return {};
}
const auto parsed =
rm_control::modules::trajectory::ParseVendorDragTrajectoryJsonLines(path, 100);
if (!parsed.ok) {
std::cerr << "[FAIL] failed to parse forward fixture: "
<< parsed.error_message << '\n';
return {};
}
if (!SetReadOnly(path)) {
std::cerr << "[FAIL] failed to protect forward fixture " << path << '\n';
return {};
}
return parsed.value;
}
std::size_t DirectoryEntryCount(const std::filesystem::path &path) {
std::error_code filesystem_error;
std::size_t count = 0U;
for (std::filesystem::directory_iterator iterator(path, filesystem_error), end;
!filesystem_error && iterator != end;
iterator.increment(filesystem_error)) {
++count;
}
return filesystem_error ? static_cast<std::size_t>(-1) : count;
}
void TestCreatesExactReadOnlyReverseWithoutOverwrite(TestContext *context) {
const auto forward = CreateForwardFixture("success_forward.txt");
const auto output_root = TestOutputRoot() / "successful_artifacts";
const auto first = rm_control::modules::trajectory::
CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(forward, output_root);
context->Expect(first.ok, "valid read-only source should create a reversed artifact");
if (!first.ok) {
return;
}
context->Expect(first.value.source_file != forward.source_file,
"reversed artifact must not reuse or overwrite the source path");
context->Expect(ReadBinaryFile(first.value.source_file) == ReversedCrLfContent(),
"generated file must preserve CRLF and reverse exact physical lines");
context->Expect(HasNoWritePermission(first.value.source_file),
"generated reversed trajectory must have no write bits");
context->Expect(ReadBinaryFile(forward.source_file) == ForwardCrLfContent(),
"creating a reverse must leave the source byte-for-byte unchanged");
context->Expect(first.value.samples.front().joints_deg ==
forward.samples.back().joints_deg &&
first.value.samples.back().joints_deg ==
forward.samples.front().joints_deg,
"parsed reversed endpoints must exactly invert forward endpoints");
const auto second = rm_control::modules::trajectory::
CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(forward, output_root);
context->Expect(second.ok && second.value.source_file != first.value.source_file,
"a second call must allocate a unique path without overwriting the first");
context->Expect(ReadBinaryFile(first.value.source_file) == ReversedCrLfContent(),
"a later reverse must not change the first generated artifact");
}
void TestRejectsWritableAndTamperedSources(TestContext *context) {
const auto writable_path = TestOutputRoot() / "writable_forward.txt";
context->Expect(WriteBinaryFile(writable_path, ForwardCrLfContent()),
"writable source fixture should be created");
const auto writable =
rm_control::modules::trajectory::ParseVendorDragTrajectoryJsonLines(writable_path, 100);
const auto writable_result = rm_control::modules::trajectory::
CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(
writable.value, TestOutputRoot() / "writable_rejection");
context->Expect(!writable_result.ok &&
writable_result.error_message.find("read-only") != std::string::npos,
"a writable source must fail before creating a reverse");
auto tampered = CreateForwardFixture("tampered_forward.txt");
std::error_code filesystem_error;
std::filesystem::permissions(
tampered.source_file, std::filesystem::perms::owner_write,
std::filesystem::perm_options::add, filesystem_error);
std::string changed = ForwardCrLfContent();
changed[changed.find("1000") + 3U] = '1';
context->Expect(!filesystem_error && WriteBinaryFile(tampered.source_file, changed) &&
SetReadOnly(tampered.source_file),
"tampered source fixture should retain stale parsed metadata");
const auto tampered_result = rm_control::modules::trajectory::
CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(
tampered, TestOutputRoot() / "tampered_rejection");
context->Expect(!tampered_result.ok &&
tampered_result.error_message.find("SHA-256") != std::string::npos,
"same-size source tampering must be rejected by SHA-256");
}
void TestRejectsSymbolicLinkSource(TestContext *context) {
const auto parsed = CreateForwardFixture("symlink_target.txt");
const auto symbolic_link = TestOutputRoot() / "symlink_source.txt";
std::error_code filesystem_error;
std::filesystem::create_symlink(parsed.source_file, symbolic_link, filesystem_error);
auto linked_metadata = parsed;
linked_metadata.source_file = symbolic_link;
const auto result = rm_control::modules::trajectory::
CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(
linked_metadata, TestOutputRoot() / "symlink_rejection");
context->Expect(!filesystem_error && !result.ok &&
result.error_message.find("non-symlink regular file") != std::string::npos,
"symbolic-link source must fail the regular-file check");
}
void TestFailureCleansOnlyNewArtifact(TestContext *context) {
auto inconsistent = CreateForwardFixture("inconsistent_samples_forward.txt");
inconsistent.samples.front().joints_deg.front() = 99.0F;
const auto output_root = TestOutputRoot() / "cleanup_scope";
std::error_code filesystem_error;
std::filesystem::create_directories(output_root, filesystem_error);
const auto sentinel = output_root / "preexisting_sentinel.txt";
context->Expect(!filesystem_error && WriteBinaryFile(sentinel, "keep"),
"cleanup test sentinel should be created");
const std::size_t entries_before = DirectoryEntryCount(output_root);
const auto result = rm_control::modules::trajectory::
CreateReadOnlyReversedVendorTrajectoryWithoutOverwrite(inconsistent, output_root);
context->Expect(!result.ok && result.error_message.find("sample") != std::string::npos,
"inconsistent in-memory samples must fail after output verification");
context->Expect(DirectoryEntryCount(output_root) == entries_before &&
ReadBinaryFile(sentinel) == "keep",
"failure cleanup must remove only this call's new artifact");
context->Expect(ReadBinaryFile(inconsistent.source_file) == ForwardCrLfContent(),
"failure cleanup must never remove or modify the forward source");
}
void TestDefaultDirectoryIsDedicated(TestContext *context) {
const auto path =
rm_control::modules::trajectory::DefaultReversedVendorTrajectoryDirectory();
context->Expect(path.filename() == "reversed_vendor_trajectories",
"default reverse directory must be dedicated to derived artifacts");
}
} // namespace
int main() {
TestContext context;
TestCreatesExactReadOnlyReverseWithoutOverwrite(&context);
TestRejectsWritableAndTamperedSources(&context);
TestRejectsSymbolicLinkSource(&context);
TestFailureCleansOnlyNewArtifact(&context);
TestDefaultDirectoryIsDedicated(&context);
std::error_code cleanup_error;
std::filesystem::remove_all(TestOutputRoot(), cleanup_error);
if (context.failures == 0) {
std::cout << "[PASS] rm_reversed_vendor_trajectory_store_tests\n";
return 0;
}
std::cerr << context.failures << " test(s) failed\n";
return 1;
}
RM/tests/test_trajectory_file_store.cpp
View file @ 8275ee2e
#include "rm_control/modules/trajectory/trajectory_file_store.h" #include "rm_control/modules/trajectory/trajectory_file_store.h"
#include <filesystem>
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
...@@ -12,17 +13,22 @@ ...@@ -12,17 +13,22 @@
namespace { namespace {
int g_failures = 0; struct TestContext {
int failures = 0;
void expect(bool condition, const std::string &message) { void Expect(bool condition, const std::string &message) {
if (!condition) { if (!condition) {
++g_failures; ++failures;
std::cerr << "[FAIL] " << message << '\n'; std::cerr << "[FAIL] " << message << '\n';
}
} }
} };
rm_control::common::TrajectorySample makeSample(long long elapsed_ms, float j2_deg) { rm_control::common::TrajectorySample makeSample(std::size_t sample_index,
long long elapsed_ms,
float j2_deg) {
rm_control::common::TrajectorySample sample; rm_control::common::TrajectorySample sample;
sample.sample_index = sample_index;
sample.elapsed_ms = elapsed_ms; sample.elapsed_ms = elapsed_ms;
sample.joints_deg = {0.0F, j2_deg, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}; sample.joints_deg = {0.0F, j2_deg, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F};
sample.tcp_pose.x = 0.1F; sample.tcp_pose.x = 0.1F;
...@@ -41,63 +47,101 @@ std::string readTextFile(const std::string &path) { ...@@ -41,63 +47,101 @@ std::string readTextFile(const std::string &path) {
return buffer.str(); return buffer.str();
} }
void testStoragePathsAndCsvRoundTrip() { void testStoragePathsAndCsvRoundTrip(TestContext *context) {
const std::string root = std::string(RM_TEST_OUTPUT_DIR) + "/trajectory_store_test"; const std::string root = std::string(RM_TEST_OUTPUT_DIR) + "/trajectory_store_test";
auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(root, "demo session"); auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(root, "demo session");
expect(files.ok, "file set should be created"); context->Expect(files.ok, "file set should be created");
if (!files.ok) { if (!files.ok) {
return; return;
} }
expect(files.value.session_name == "demo_session", "session name should be sanitized"); context->Expect(files.value.session_name == "demo_session",
"session name should be sanitized");
std::vector<rm_control::common::TrajectorySample> samples; std::vector<rm_control::common::TrajectorySample> samples;
samples.push_back(makeSample(0, 0.0F)); samples.push_back(makeSample(0, 0, 0.0F));
samples.push_back(makeSample(100, 1.0F)); samples.push_back(makeSample(1, 100, 1.0F));
auto write_result = rm_control::modules::trajectory::writeTrajectoryCsv(files.value.csv_path, samples); auto write_result = rm_control::modules::trajectory::writeTrajectoryCsv(files.value.csv_path, samples);
expect(write_result.ok, "CSV should be written"); context->Expect(write_result.ok, "CSV should be written");
const auto overwrite_result =
rm_control::modules::trajectory::writeTrajectoryCsvWithoutOverwrite(
std::filesystem::path(files.value.csv_path), samples);
context->Expect(!overwrite_result.ok,
"new-session CSV writer must refuse an existing target");
auto loaded = rm_control::modules::trajectory::loadTrajectoryCsv(files.value.csv_path); auto loaded = rm_control::modules::trajectory::loadTrajectoryCsv(files.value.csv_path);
expect(loaded.ok, "CSV should be loaded"); context->Expect(loaded.ok, "CSV should be loaded");
if (!loaded.ok) { if (!loaded.ok) {
return; return;
} }
expect(loaded.value.size() == 2, "loaded sample count should match"); context->Expect(loaded.value.size() == 2, "loaded sample count should match");
expect(loaded.value[1].joints_deg[1] == 1.0F, "loaded J2 should match"); context->Expect(loaded.value[1].sample_index == 1,
"loaded sample index should match");
context->Expect(loaded.value[1].source == "realman_rm75_drag_teach",
"loaded trajectory source should match");
context->Expect(loaded.value[1].joints_deg[1] == 1.0F,
"loaded J2 should match");
} }
void testExportTcpPathCpp() { void testExportTcpPathCpp(TestContext *context) {
const std::string root = std::string(RM_TEST_OUTPUT_DIR) + "/trajectory_export_test"; const std::string root = std::string(RM_TEST_OUTPUT_DIR) + "/trajectory_export_test";
auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(root, "tcp demo"); auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(root, "tcp demo");
expect(files.ok, "file set should be created for export"); context->Expect(files.ok, "file set should be created for export");
if (!files.ok) { if (!files.ok) {
return; return;
} }
std::vector<rm_control::common::TrajectorySample> samples; std::vector<rm_control::common::TrajectorySample> samples;
samples.push_back(makeSample(0, 0.0F)); samples.push_back(makeSample(0, 0, 0.0F));
samples.push_back(makeSample(100, 1.0F)); samples.push_back(makeSample(1, 100, 1.0F));
auto export_result = rm_control::modules::trajectory::exportTcpPathCpp( auto export_result = rm_control::modules::trajectory::exportTcpPathCpp(
files.value.converted_cpp_path, samples, files.value.session_name); files.value.converted_cpp_path, samples, files.value.session_name);
expect(export_result.ok, "C++ TCP path should be exported"); context->Expect(export_result.ok, "C++ TCP path should be exported");
const std::string text = readTextFile(files.value.converted_cpp_path); const std::string text = readTextFile(files.value.converted_cpp_path);
expect(text.find("struct TcpWaypoint") != std::string::npos, "export should contain TcpWaypoint"); context->Expect(text.find("struct TcpWaypoint") != std::string::npos,
expect(text.find("kTcpPath_tcp_demo") != std::string::npos, "export should contain sanitized variable"); "export should contain TcpWaypoint");
context->Expect(text.find("kTcpPath_tcp_demo") != std::string::npos,
"export should contain sanitized variable");
}
void testLegacyCsvGetsExplicitLegacySource(TestContext *context) {
const std::filesystem::path directory =
std::filesystem::path(RM_TEST_OUTPUT_DIR) / "legacy_trajectory_store_test";
std::filesystem::create_directories(directory);
const std::filesystem::path csv_path = directory / "legacy.csv";
std::ofstream output(csv_path, std::ios::trunc);
output << "sample_index,elapsed_ms,"
"j1_deg,j2_deg,j3_deg,j4_deg,j5_deg,j6_deg,j7_deg,"
"tcp_x_m,tcp_y_m,tcp_z_m,tcp_rx_rad,tcp_ry_rad,tcp_rz_rad\n"
<< "0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n"
<< "1,100,0,0,0,0,0,0,0,0,0,0,0,0,0\n";
output.close();
const auto loaded =
rm_control::modules::trajectory::loadTrajectoryCsv(csv_path.string());
context->Expect(loaded.ok, "legacy 15-column CSV should remain readable");
if (loaded.ok) {
context->Expect(loaded.value[0].source == "legacy_rm75_csv",
"legacy CSV must not be mislabeled as a current capture");
}
} }
} // namespace } // namespace
int main() { int main() {
testStoragePathsAndCsvRoundTrip(); TestContext context;
testExportTcpPathCpp(); testStoragePathsAndCsvRoundTrip(&context);
testExportTcpPathCpp(&context);
testLegacyCsvGetsExplicitLegacySource(&context);
if (g_failures == 0) { if (context.failures == 0) {
std::cout << "[PASS] rm_trajectory_file_store_tests\n"; std::cout << "[PASS] rm_trajectory_file_store_tests\n";
return 0; return 0;
} }
std::cerr << g_failures << " test(s) failed\n"; std::cerr << context.failures << " test(s) failed\n";
return 1; return 1;
} }
RM/tests/test_trajectory_o6_task_cli_config.cpp 0 → 100644
View file @ 8275ee2e
/**
* @file test_trajectory_o6_task_cli_config.cpp
* @brief RM75 轨迹与 LinkerHand O6 抓握任务 CLI 纯解析单元测试。
*
* @author 唐永康
* @date 2026-07-11
*
* @details
* 测试只在内存中构造 argc/argv，覆盖正常参数、必填项、O6 六关节姿态、抓握速度、
* 抓握转矩、重复选项和帮助文本。测试不读取轨迹或安全配置，不连接 RM75/O6，
* 也不执行任何运动。
*/
#include "rm_control/config/trajectory_o6_task_cli_config.h"
#include <array>
#include <cstddef>
#include <cstdint>
#include <filesystem>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
namespace {
using CliConfigResult =
rm_control::common::Result<rm_control::config::TrajectoryO6TaskCliConfig>;
CliConfigResult ParseArguments(std::vector<std::string> owned_arguments) {
std::vector<char *> argv;
argv.reserve(owned_arguments.size());
for (std::string &argument : owned_arguments) {
argv.push_back(argument.data());
}
return rm_control::config::ParseTrajectoryO6TaskCliArguments(
static_cast<int>(argv.size()), argv.data());
}
std::vector<std::string> MakeValidArguments() {
return {
"rm_trajectory_o6_grasp",
"trajectories/vendor/source.txt",
"--sample-period-ms",
"20",
"--grip-pose",
"1,2,3,4,5,255",
"--grip-speed",
"0,10,20,30,40,255",
"--grip-torque",
"255,200,150,100,50,0",
};
}
bool ReplaceOptionValue(std::vector<std::string> *arguments,
const std::string &option_name,
const std::string &replacement_value) {
if (arguments == nullptr) {
return false;
}
for (std::size_t index = 0; index + 1 < arguments->size(); ++index) {
if ((*arguments)[index] == option_name) {
(*arguments)[index + 1] = replacement_value;
return true;
}
}
return false;
}
bool RemoveOptionAndValue(std::vector<std::string> *arguments,
const std::string &option_name) {
if (arguments == nullptr) {
return false;
}
for (std::size_t index = 0; index + 1 < arguments->size(); ++index) {
if ((*arguments)[index] == option_name) {
arguments->erase(arguments->begin() + static_cast<std::ptrdiff_t>(index),
arguments->begin() + static_cast<std::ptrdiff_t>(index + 2));
return true;
}
}
return false;
}
class TrajectoryO6TaskCliConfigTestSuite {
public:
int Run() {
testNormalArgumentsUseSafeDefaults();
testAllOptionalArgumentsAndLeftHand();
testMissingRequiredArgumentsAreRejected();
testInvalidO6JointValueDimensionsAreRejected();
testInvalidO6JointValueRangesAreRejected();
testNonDecimalO6JointValuesAreRejected();
testDuplicateO6JointValueOptionsAreRejected();
testRoundTripArgumentsUseTwoDistinctProgramSlots();
testRoundTripArgumentsRejectMissingOrUnpairedOptions();
testDuplicateRoundTripOptionsAreRejected();
testRoundTripProgramIdsMustDiffer();
testInvalidSpeedIsRejected();
testInvalidSideIsRejected();
testNumericBoundariesAreValidated();
testHelpDoesNotRequireTaskArguments();
testUsageListsAllRequiredO6JointValues();
return failures_;
}
private:
int failures_ = 0;
void expect(bool condition, const std::string &message) {
if (!condition) {
++failures_;
std::cerr << "[FAIL] " << message << '\n';
}
}
void testNormalArgumentsUseSafeDefaults() {
const auto parsed = ParseArguments(MakeValidArguments());
expect(parsed.ok, "normal task arguments should parse");
if (!parsed.ok) {
return;
}
expect(parsed.value.trajectory_path ==
std::filesystem::path("trajectories/vendor/source.txt"),
"trajectory source path should be preserved");
expect(parsed.value.sample_period_ms == 20, "sample period should be parsed in ms");
expect(parsed.value.origin_speed_percent == 5, "origin speed should safely default to 5%");
expect(parsed.value.plan_speed_percent == 5, "plan speed should safely default to 5%");
expect(parsed.value.program_id == 1, "program id should default to one");
expect(!parsed.value.return_from_trajectory_end,
"reverse return should remain disabled by default");
expect(parsed.value.return_program_id == 0,
"disabled reverse return should keep its program id unset");
expect(parsed.value.o6_side == rm_control::config::O6HandSide::Right,
"O6 side should default to right");
expect(static_cast<std::uint8_t>(parsed.value.o6_side) == 0x27,
"right O6 side should map to device address 0x27");
expect(parsed.value.grip_pose ==
(std::array<std::uint8_t, rm_control::config::kLinkerHandO6JointCount>{
1, 2, 3, 4, 5, 255}),
"six O6 grip pose values should be parsed without reordering");
expect(parsed.value.grip_speed ==
(std::array<std::uint8_t, rm_control::config::kLinkerHandO6JointCount>{
0, 10, 20, 30, 40, 255}),
"six O6 grip speed values should be parsed without reordering");
expect(parsed.value.grip_torque ==
(std::array<std::uint8_t, rm_control::config::kLinkerHandO6JointCount>{
255, 200, 150, 100, 50, 0}),
"six O6 grip torque values should be parsed without reordering");
expect(!parsed.value.live_requested, "task should default to DryRun authorization");
expect(parsed.value.safety_config_path.filename() == "drag_teach_safety.json",
"default safety config filename should be stable");
}
void testAllOptionalArgumentsAndLeftHand() {
const auto parsed = ParseArguments({
"rm_trajectory_o6_grasp",
"--sample-period-ms",
"1000",
"--origin-speed-percent",
"10",
"--plan-speed-percent",
"1",
"--program-id",
"100",
"--return-from-trajectory-end",
"--return-program-id",
"1",
"--o6-side",
"left",
"--grip-pose",
"0,10,20,30,40,255",
"--grip-speed",
"255,40,30,20,10,0",
"--grip-torque",
"1,2,3,4,5,6",
"--live",
"--safety-config",
"config/custom_safety.json",
"trajectory.txt",
});
expect(parsed.ok, "complete optional task arguments should parse");
if (!parsed.ok) {
return;
}
expect(parsed.value.origin_speed_percent == 10, "origin speed upper boundary parsed");
expect(parsed.value.plan_speed_percent == 1, "plan speed lower boundary parsed");
expect(parsed.value.program_id == 100, "program id upper boundary parsed");
expect(parsed.value.return_from_trajectory_end,
"explicit reverse-return flag should enable round-trip execution");
expect(parsed.value.return_program_id == 1,
"return program id lower boundary should parse");
expect(parsed.value.o6_side == rm_control::config::O6HandSide::Left,
"left O6 side should parse");
expect(static_cast<std::uint8_t>(parsed.value.o6_side) == 0x28,
"left O6 side should map to device address 0x28");
expect(parsed.value.live_requested, "--live should require explicit presence");
expect(parsed.value.safety_config_path ==
std::filesystem::path("config/custom_safety.json"),
"custom safety path should be preserved");
}
void testMissingRequiredArgumentsAreRejected() {
expectRequiredOptionRejected("--sample-period-ms", "sample period");
expectRequiredOptionRejected("--grip-pose", "grip pose");
expectRequiredOptionRejected("--grip-speed", "grip speed");
expectRequiredOptionRejected("--grip-torque", "grip torque");
std::vector<std::string> arguments = MakeValidArguments();
arguments.erase(arguments.begin() + 1);
const auto missing_trajectory = ParseArguments(arguments);
expect(!missing_trajectory.ok, "missing trajectory path should be rejected");
}
void expectRequiredOptionRejected(const std::string &option_name,
const std::string &case_name) {
std::vector<std::string> arguments = MakeValidArguments();
const bool removed = RemoveOptionAndValue(&arguments, option_name);
expect(removed, "test baseline should contain " + option_name);
const auto parsed = ParseArguments(arguments);
expect(!parsed.ok && parsed.error_message.find(option_name) != std::string::npos,
"missing " + case_name + " should be rejected with option context");
}
void testInvalidO6JointValueDimensionsAreRejected() {
expectO6JointValueOptionRejected("--grip-pose", "1,2,3,4,5", "five-value pose");
expectO6JointValueOptionRejected("--grip-pose", "1,2,3,4,5,6,7", "seven-value pose");
expectO6JointValueOptionRejected("--grip-speed", "1,2,3,4,5", "five-value speed");
expectO6JointValueOptionRejected("--grip-speed", "1,2,3,4,5,6,7", "seven-value speed");
expectO6JointValueOptionRejected("--grip-torque", "1,2,3,4,5", "five-value torque");
expectO6JointValueOptionRejected(
"--grip-torque", "1,2,3,4,5,6,7", "seven-value torque");
}
void testInvalidO6JointValueRangesAreRejected() {
expectO6JointValueOptionRejected("--grip-pose", "1,2,3,4,5,256", "high pose");
expectO6JointValueOptionRejected("--grip-pose", "1,2,3,4,-1,6", "negative pose");
expectO6JointValueOptionRejected("--grip-speed", "256,2,3,4,5,6", "high speed");
expectO6JointValueOptionRejected("--grip-speed", "1,-1,3,4,5,6", "negative speed");
expectO6JointValueOptionRejected("--grip-torque", "1,2,256,4,5,6", "high torque");
expectO6JointValueOptionRejected(
"--grip-torque", "1,2,3,-1,5,6", "negative torque");
}
void testNonDecimalO6JointValuesAreRejected() {
expectO6JointValueOptionRejected(
"--grip-pose", "1,2,three,4,5,6", "non-decimal pose");
expectO6JointValueOptionRejected(
"--grip-speed", "1,2,3,4,5,6.0", "floating-point speed");
expectO6JointValueOptionRejected(
"--grip-torque", "1,2,3,4,5,0x10", "hexadecimal torque");
}
void expectO6JointValueOptionRejected(const std::string &option_name,
const std::string &value,
const std::string &case_name) {
std::vector<std::string> arguments = MakeValidArguments();
const bool replaced = ReplaceOptionValue(&arguments, option_name, value);
expect(replaced, "test baseline should contain " + option_name);
const auto parsed = ParseArguments(arguments);
expect(!parsed.ok, case_name + " should be rejected");
}
void testDuplicateO6JointValueOptionsAreRejected() {
expectDuplicateO6JointValueOptionRejected("--grip-pose", "1,2,3,4,5,6");
expectDuplicateO6JointValueOptionRejected("--grip-speed", "6,5,4,3,2,1");
expectDuplicateO6JointValueOptionRejected("--grip-torque", "10,20,30,40,50,60");
}
void testRoundTripArgumentsUseTwoDistinctProgramSlots() {
std::vector<std::string> arguments = MakeValidArguments();
arguments.push_back("--return-from-trajectory-end");
arguments.push_back("--return-program-id");
arguments.push_back("2");
const auto parsed = ParseArguments(arguments);
expect(parsed.ok, "paired round-trip arguments with distinct program ids should parse");
if (parsed.ok) {
expect(parsed.value.return_from_trajectory_end,
"round-trip flag should be preserved in parsed config");
expect(parsed.value.program_id == 1 && parsed.value.return_program_id == 2,
"forward and return program ids should be preserved without reordering");
}
}
void testRoundTripArgumentsRejectMissingOrUnpairedOptions() {
std::vector<std::string> missing_return_id = MakeValidArguments();
missing_return_id.push_back("--return-from-trajectory-end");
const auto missing_id = ParseArguments(missing_return_id);
expect(!missing_id.ok &&
missing_id.error_message.find("--return-program-id") != std::string::npos,
"reverse-return flag without a return program id should be rejected");
std::vector<std::string> return_id_without_flag = MakeValidArguments();
return_id_without_flag.push_back("--return-program-id");
return_id_without_flag.push_back("2");
const auto unpaired_id = ParseArguments(return_id_without_flag);
expect(!unpaired_id.ok && unpaired_id.error_message.find(
"--return-from-trajectory-end") != std::string::npos,
"return program id without the reverse-return flag should be rejected");
}
void testDuplicateRoundTripOptionsAreRejected() {
std::vector<std::string> duplicate_flag = MakeValidArguments();
duplicate_flag.push_back("--return-from-trajectory-end");
duplicate_flag.push_back("--return-program-id");
duplicate_flag.push_back("2");
duplicate_flag.push_back("--return-from-trajectory-end");
const auto duplicate_flag_result = ParseArguments(duplicate_flag);
expect(!duplicate_flag_result.ok && duplicate_flag_result.error_message.find(
"duplicate option") != std::string::npos,
"duplicate reverse-return flag should be rejected explicitly");
std::vector<std::string> duplicate_id = MakeValidArguments();
duplicate_id.push_back("--return-from-trajectory-end");
duplicate_id.push_back("--return-program-id");
duplicate_id.push_back("2");
duplicate_id.push_back("--return-program-id");
duplicate_id.push_back("3");
const auto duplicate_id_result = ParseArguments(duplicate_id);
expect(!duplicate_id_result.ok && duplicate_id_result.error_message.find(
"duplicate option") != std::string::npos,
"duplicate return program id should be rejected explicitly");
}
void testRoundTripProgramIdsMustDiffer() {
std::vector<std::string> arguments = MakeValidArguments();
arguments.push_back("--return-from-trajectory-end");
arguments.push_back("--return-program-id");
arguments.push_back("1");
const auto parsed = ParseArguments(arguments);
expect(!parsed.ok && parsed.error_message.find("must differ") != std::string::npos,
"return program id must differ from the default forward program id");
}
void expectDuplicateO6JointValueOptionRejected(const std::string &option_name,
const std::string &value) {
std::vector<std::string> arguments = MakeValidArguments();
arguments.push_back(option_name);
arguments.push_back(value);
const auto parsed = ParseArguments(arguments);
expect(!parsed.ok && parsed.error_message.find("duplicate option") != std::string::npos,
"duplicate " + option_name + " should be rejected explicitly");
}
void testInvalidSpeedIsRejected() {
expectIntegerOptionRejected("--origin-speed-percent", "0", "zero origin speed");
expectIntegerOptionRejected("--origin-speed-percent", "11", "high origin speed");
expectIntegerOptionRejected("--plan-speed-percent", "0", "zero plan speed");
expectIntegerOptionRejected("--plan-speed-percent", "11", "high plan speed");
}
void expectIntegerOptionRejected(const std::string &option,
const std::string &value,
const std::string &case_name) {
std::vector<std::string> arguments = MakeValidArguments();
if (!ReplaceOptionValue(&arguments, option, value)) {
arguments.push_back(option);
arguments.push_back(value);
}
const auto parsed = ParseArguments(arguments);
expect(!parsed.ok, case_name + " should be rejected");
}
void testInvalidSideIsRejected() {
std::vector<std::string> arguments = MakeValidArguments();
arguments.push_back("--o6-side");
arguments.push_back("center");
const auto parsed = ParseArguments(arguments);
expect(!parsed.ok, "unknown O6 side should be rejected");
}
void testNumericBoundariesAreValidated() {
expectIntegerOptionRejected("--sample-period-ms", "0", "zero sample period");
expectIntegerOptionRejected("--sample-period-ms", "1001", "high sample period");
expectIntegerOptionRejected("--program-id", "0", "zero program id");
expectIntegerOptionRejected("--program-id", "101", "high program id");
expectReturnProgramIdRejected("0", "zero return program id");
expectReturnProgramIdRejected("101", "high return program id");
}
void expectReturnProgramIdRejected(const std::string &value,
const std::string &case_name) {
std::vector<std::string> arguments = MakeValidArguments();
arguments.push_back("--return-from-trajectory-end");
arguments.push_back("--return-program-id");
arguments.push_back(value);
const auto parsed = ParseArguments(arguments);
expect(!parsed.ok && parsed.error_message.find("--return-program-id") != std::string::npos,
case_name + " should be rejected with return option context");
}
void testHelpDoesNotRequireTaskArguments() {
const auto short_help = ParseArguments({"rm_trajectory_o6_grasp", "-h"});
expect(short_help.ok && short_help.value.help_requested,
"short help should not require task arguments");
const auto long_help = ParseArguments({"rm_trajectory_o6_grasp", "--help"});
expect(long_help.ok && long_help.value.help_requested,
"long help should not require task arguments");
}
void testUsageListsAllRequiredO6JointValues() {
std::ostringstream usage;
rm_control::config::PrintTrajectoryO6TaskUsage("rm_trajectory_o6_grasp", usage);
const std::string text = usage.str();
expect(text.find("--grip-pose v1,v2,v3,v4,v5,v6") != std::string::npos,
"usage should list the required six-value grip pose");
expect(text.find("--grip-speed v1,v2,v3,v4,v5,v6") != std::string::npos,
"usage should list the required six-value grip speed");
expect(text.find("--grip-torque v1,v2,v3,v4,v5,v6") != std::string::npos,
"usage should list the required six-value grip torque");
expect(text.find("all required; no execution defaults") != std::string::npos,
"usage should state that O6 action values have no execution defaults");
expect(text.find("--return-from-trajectory-end --return-program-id N") !=
std::string::npos,
"usage should document paired reverse-return arguments");
expect(text.find("two controller slots") != std::string::npos,
"usage should explain that round-trip execution reserves two program slots");
}
};
} // namespace
/**
* @brief 运行 RM75 轨迹与 O6 抓握任务 CLI 单元测试。
*
* @reason 使用独立无外部依赖测试程序，让 CTest 能在没有 RM75、O6 或厂商 SDK 时验证解析。
*
* @author 唐永康
* @date 2026-07-11
*
* @return 0 表示全部断言通过；1 表示至少一个断言失败。
*
* @calls 只调用纯命令行解析函数，不访问文件系统内容、网络或硬件。
* @safety 本测试永远不连接设备，也不会执行真实运动。
* @note 测试失败信息写入标准错误流，便于 CTest 输出定位。
*/
int main() {
TrajectoryO6TaskCliConfigTestSuite suite;
const int failures = suite.Run();
if (failures == 0) {
std::cout << "[PASS] rm_trajectory_o6_task_cli_config_tests\n";
return 0;
}
std::cerr << failures << " test(s) failed\n";
return 1;
}
RM/tests/test_trajectory_playback_report_store.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/modules/trajectory/trajectory_playback_report_store.h"
#include <filesystem>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#ifndef RM_TEST_OUTPUT_DIR
#define RM_TEST_OUTPUT_DIR "."
#endif
namespace {
struct TestContext {
int failures = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures;
std::cerr << "[FAIL] " << message << '\n';
}
}
};
rm_control::modules::trajectory::ControllerManagedPlaybackReport ValidReport() {
using Report =
rm_control::modules::trajectory::ControllerManagedPlaybackReport;
Report report;
report.report_identifier = "20260711_120000";
report.source_file = "source.txt";
report.source_file_sha256 = std::string(64U, 'a');
report.trajectory_point_count = 3U;
report.sample_period_ms = 100;
report.source_duration_ms = 200;
report.plan_speed_percent = 5;
report.controller_program_id = 7;
report.observed_execution_duration_ms = 450.0;
report.program_status_poll_count = 4U;
report.minimum_poll_period_ms = 99.0;
report.maximum_poll_period_ms = 102.0;
report.average_poll_period_ms = 100.3;
report.maximum_absolute_poll_jitter_ms = 2.0;
report.final_controller_state = 0;
report.o6_device_id = 0x27U;
report.target_positions = {140U, 100U, 55U, 55U, 55U, 55U};
report.speed_commands = {20U, 20U, 20U, 20U, 20U, 20U};
report.torque_commands = {30U, 30U, 30U, 30U, 30U, 30U};
report.final_positions = report.target_positions;
return report;
}
std::string ReadText(const std::filesystem::path &path) {
std::ifstream input(path);
std::ostringstream text;
text << input.rdbuf();
return text.str();
}
void TestWritesStructuredReportWithoutOverwrite(TestContext *context) {
const std::filesystem::path directory =
std::filesystem::path(RM_TEST_OUTPUT_DIR) / "playback_report_store";
std::filesystem::remove_all(directory);
const auto first =
rm_control::modules::trajectory::WriteControllerManagedPlaybackReportWithoutOverwrite(
ValidReport(), directory);
context->Expect(first.ok, "valid controller-managed report should be written");
if (!first.ok) {
return;
}
const std::string text = ReadText(first.value);
context->Expect(text.find("host_program_state_polling_not_point_dispatch") !=
std::string::npos,
"report must state the scope of polling jitter");
context->Expect(text.find("\"target_positions\": [140, 100, 55") !=
std::string::npos,
"report must preserve the O6 target positions");
const auto duplicate =
rm_control::modules::trajectory::WriteControllerManagedPlaybackReportWithoutOverwrite(
ValidReport(), directory);
context->Expect(!duplicate.ok,
"same report identifier must not overwrite an existing file");
}
void TestRejectsInvalidMetadataBeforeWriting(TestContext *context) {
auto report = ValidReport();
report.source_file_sha256 = "not-a-sha256";
const std::filesystem::path directory =
std::filesystem::path(RM_TEST_OUTPUT_DIR) / "invalid_playback_report";
std::filesystem::remove_all(directory);
const auto result =
rm_control::modules::trajectory::WriteControllerManagedPlaybackReportWithoutOverwrite(
report, directory);
context->Expect(!result.ok, "invalid report metadata must be rejected");
context->Expect(!std::filesystem::exists(directory),
"invalid report must not create an output directory");
}
void TestWritesRoundTripMetadata(TestContext *context) {
auto report = ValidReport();
report.report_identifier = "20260711_120001";
report.returned_from_trajectory_end = true;
report.return_source_file = "return_source.txt";
report.return_source_file_sha256 = std::string(64U, 'b');
report.return_controller_program_id = 8;
report.return_observed_execution_duration_ms = 430.0;
report.return_program_status_poll_count = 5U;
report.return_minimum_poll_period_ms = 98.0;
report.return_maximum_poll_period_ms = 103.0;
report.return_average_poll_period_ms = 100.1;
report.return_maximum_absolute_poll_jitter_ms = 3.0;
report.return_final_controller_state = 0;
const std::filesystem::path directory =
std::filesystem::path(RM_TEST_OUTPUT_DIR) / "round_trip_playback_report";
std::filesystem::remove_all(directory);
const auto result =
rm_control::modules::trajectory::WriteControllerManagedPlaybackReportWithoutOverwrite(
report, directory);
context->Expect(result.ok, "valid round-trip report should be written");
if (!result.ok) {
return;
}
const std::string text = ReadText(result.value);
context->Expect(text.find("\"returned_from_trajectory_end\": true") !=
std::string::npos,
"report must identify the reverse-return workflow");
context->Expect(text.find("\"return_controller_program_id\": 8") !=
std::string::npos,
"report must preserve the return controller program id");
context->Expect(text.find(std::string(64U, 'b')) != std::string::npos,
"report must preserve the return trajectory SHA-256");
}
} // namespace
int main() {
TestContext context;
TestWritesStructuredReportWithoutOverwrite(&context);
TestRejectsInvalidMetadataBeforeWriting(&context);
TestWritesRoundTripMetadata(&context);
if (context.failures == 0) {
std::cout << "[PASS] rm_trajectory_playback_report_store_tests\n";
return 0;
}
std::cerr << context.failures << " test(s) failed\n";
return 1;
}
RM/tests/test_vendor_drag_trajectory_parser.cpp 0 → 100644
View file @ 8275ee2e
#include "rm_control/modules/trajectory/vendor_drag_trajectory_parser.h"
#include <cmath>
#include <cstdint>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <limits>
#include <string>
#include <utility>
#include <vector>
#ifndef RM_TEST_OUTPUT_DIR
#error "RM_TEST_OUTPUT_DIR must be defined so tests cannot write into the source tree"
#endif
namespace {
namespace trajectory = rm_control::modules::trajectory;
class VendorDragTrajectoryParserTestSuite {
public:
int Run() {
std::error_code filesystem_error;
std::filesystem::create_directories(TestOutputRoot(), filesystem_error);
Expect(!filesystem_error,
"test output root should be created under RM_TEST_OUTPUT_DIR: " +
filesystem_error.message());
TestParsesStrictSevenJointJsonLines();
TestUsesExplicitRawUnitScale();
TestRejectsInvalidArgumentsAndTimestampOverflow();
TestRejectsMalformedVendorLines();
TestRejectsEmptyAndNonRegularInputs();
return failures_;
}
private:
int failures_ = 0;
void Expect(bool condition, const std::string &message) {
if (!condition) {
++failures_;
std::cerr << "[FAIL] " << message << '\n';
}
}
std::filesystem::path TestOutputRoot() const {
return std::filesystem::path(RM_TEST_OUTPUT_DIR) /
"vendor_drag_trajectory_parser_tests";
}
std::filesystem::path TestFile(const std::string &name) const {
return TestOutputRoot() / name;
}
bool WriteBinaryFile(const std::filesystem::path &path, const std::string &content) {
std::ofstream output(path, std::ios::binary | std::ios::trunc);
output.write(content.data(), static_cast<std::streamsize>(content.size()));
return static_cast<bool>(output);
}
bool NearlyEqual(float actual, float expected) const {
return std::abs(actual - expected) <= 0.00001F;
}
void TestParsesStrictSevenJointJsonLines() {
const std::string content =
"{\"point\":[1000,-2000,0,500,250,-750,3000]}\r\n"
" { \"point\" : [2000,-1000,1000,1500,1250,250,4000] } \r\n";
const auto path = TestFile("valid_crlf.txt");
Expect(WriteBinaryFile(path, content), "valid CRLF fixture should be written");
const auto result = trajectory::ParseVendorDragTrajectoryJsonLines(path, 20);
Expect(result.ok, "valid vendor JSONL should parse: " + result.error_message);
if (!result.ok) {
return;
}
Expect(result.value.source_file == path, "result should preserve the source path");
Expect(result.value.trajectory_point_count == 2U,
"result should report two trajectory points");
Expect(result.value.samples.size() == 2U, "result should contain two samples");
Expect(result.value.source_file_size_bytes == content.size(),
"result should report the original byte size");
Expect(result.value.source_file_sha256 ==
"b2274aa15734a7373e528fef19d4f3676b1e8f22a03d60dd56c36a2aa8997080",
"result should report the SHA-256 of the exact CRLF bytes");
Expect(result.value.timestamp_is_derived,
"vendor timestamps should be explicitly marked derived");
Expect(result.value.sample_period_ms == 20,
"result should preserve the explicit sample period");
Expect(result.value.raw_units_per_degree == 1000,
"default vendor scale should be 1000 raw units per degree");
Expect(result.value.source == trajectory::kRealmanVendorDragTrajectorySource,
"result should identify the vendor source format");
const auto &first = result.value.samples[0];
const auto &second = result.value.samples[1];
Expect(first.sample_index == 0U && second.sample_index == 1U,
"sample indexes should be contiguous and start at zero");
Expect(first.elapsed_ms == 0 && second.elapsed_ms == 20,
"timestamps should be derived from the explicit period");
Expect(first.joints_deg.size() == 7U && second.joints_deg.size() == 7U,
"every RM75 sample should contain seven joints");
Expect(NearlyEqual(first.joints_deg[0], 1.0F) &&
NearlyEqual(first.joints_deg[1], -2.0F) &&
NearlyEqual(first.joints_deg[6], 3.0F),
"raw integer joints should convert to degree without changing order");
Expect(first.source == trajectory::kRealmanVendorDragTrajectorySource &&
second.source == trajectory::kRealmanVendorDragTrajectorySource,
"every sample should retain vendor source provenance");
}
void TestUsesExplicitRawUnitScale() {
const auto path = TestFile("explicit_scale.txt");
Expect(WriteBinaryFile(path, "{\"point\":[2000,0,0,0,0,0,0]}\n"),
"explicit-scale fixture should be written");
const auto result = trajectory::ParseVendorDragTrajectoryJsonLines(path, 5, 2000);
Expect(result.ok, "positive explicit raw unit scale should parse");
if (result.ok) {
Expect(result.value.raw_units_per_degree == 2000,
"result should preserve the explicit raw scale");
Expect(NearlyEqual(result.value.samples[0].joints_deg[0], 1.0F),
"explicit raw scale should control degree conversion");
}
}
void TestRejectsInvalidArgumentsAndTimestampOverflow() {
const auto path = TestFile("arguments.txt");
const std::string three_points =
"{\"point\":[0,0,0,0,0,0,0]}\n"
"{\"point\":[0,0,0,0,0,0,0]}\n"
"{\"point\":[0,0,0,0,0,0,0]}\n";
Expect(WriteBinaryFile(path, three_points), "argument fixture should be written");
Expect(!trajectory::ParseVendorDragTrajectoryJsonLines({}, 1).ok,
"empty input path should be rejected");
Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(path, 0).ok,
"zero sample period should be rejected");
Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(path, -1).ok,
"negative sample period should be rejected");
Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(path, 1, 0).ok,
"zero raw units per degree should be rejected");
Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(path, 1, -1000).ok,
"negative raw units per degree should be rejected");
Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(
path, std::numeric_limits<std::int64_t>::max()).ok,
"derived timestamp multiplication overflow should be rejected");
}
void TestRejectsMalformedVendorLines() {
const std::vector<std::pair<std::string, std::string>> invalid_files = {
{"blank_line.txt",
"{\"point\":[0,0,0,0,0,0,0]}\n\n{\"point\":[0,0,0,0,0,0,0]}\n"},
{"unknown_key.txt", "{\"joints\":[0,0,0,0,0,0,0]}\n"},
{"six_joints.txt", "{\"point\":[0,0,0,0,0,0]}\n"},
{"eight_joints.txt", "{\"point\":[0,0,0,0,0,0,0,0]}\n"},
{"floating_joint.txt", "{\"point\":[0,0,0,0,0,0,1.5]}\n"},
{"leading_zero.txt", "{\"point\":[00,0,0,0,0,0,0]}\n"},
{"positive_sign.txt", "{\"point\":[+1,0,0,0,0,0,0]}\n"},
{"extra_field.txt", "{\"point\":[0,0,0,0,0,0,0],\"time\":1}\n"},
{"array_root.txt", "[[0,0,0,0,0,0,0]]\n"},
{"int64_overflow.txt",
"{\"point\":[9223372036854775808,0,0,0,0,0,0]}\n"},
{"negative_int64_overflow.txt",
"{\"point\":[-9223372036854775809,0,0,0,0,0,0]}\n"},
{"trailing_object.txt",
"{\"point\":[0,0,0,0,0,0,0]}{\"point\":[0,0,0,0,0,0,0]}\n"},
};
for (const auto &fixture : invalid_files) {
const auto path = TestFile(fixture.first);
Expect(WriteBinaryFile(path, fixture.second),
"malformed fixture should be written: " + fixture.first);
const auto result = trajectory::ParseVendorDragTrajectoryJsonLines(path, 10);
Expect(!result.ok, "malformed vendor line should be rejected: " + fixture.first);
if (!result.ok && fixture.first != "blank_line.txt") {
Expect(result.error_message.find("line 1") != std::string::npos,
"format error should include physical line context: " + fixture.first);
}
}
}
void TestRejectsEmptyAndNonRegularInputs() {
const auto empty_path = TestFile("empty.txt");
Expect(WriteBinaryFile(empty_path, ""), "empty fixture should be written");
Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(empty_path, 10).ok,
"empty vendor trajectory should be rejected");
Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(TestOutputRoot(), 10).ok,
"directory input should be rejected");
Expect(!trajectory::ParseVendorDragTrajectoryJsonLines(
TestFile("missing.txt"), 10).ok,
"missing input should be rejected");
}
};
} // namespace
int main() {
VendorDragTrajectoryParserTestSuite suite;
const int failures = suite.Run();
if (failures == 0) {
std::cout << "[PASS] rm_vendor_drag_trajectory_parser_tests\n";
return 0;
}
std::cerr << failures << " test(s) failed\n";
return 1;
}
include/flight_robot/linker_hand/linker_hand_adapter.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/interfaces/dexterous_hand.h"
namespace flight_robot::linker_hand {
/**
* @brief 通过 RM75 工具端 Modbus RTU 实现 O6 统一灵巧手接口。
*
* @reason 将 O6 功能码、寄存器地址、方向和安全值校验集中在厂商适配边界，业务状态机只依赖
* IDexterousHand，且不会接触 RM_API2 类型。
*
* @author 唐永康
* @date 2026-07-11
*
* @calls IToolModbusBus 的状态、输入/保持寄存器读取和保持寄存器写入能力。
* @safety ReadReadiness 只读；CommandMotion 可能触发真实 O6 运动并必须受上层门控。
* @note 协议字段来源为 `docs/O6机械手485协议简要说明.xlsx`，当前只支持 O6 六自由度手。
*/
class LinkerHandAdapter final : public rm_control::interfaces::IDexterousHand {
public:
/**
* @brief 构造指定左右手的 O6 工具端 Modbus 适配器。
*
* @reason 左右手决定从站地址和 identity direction，构造时固定可防止运行中错误切换设备。
*
* @author 唐永康
* @date 2026-07-10
*
* @param tool_bus 已由 Application 注入的 RM75 工具端总线；适配器不拥有该对象。
* @param side 目标 O6 手侧；Right 对应 0x27/'R'，Left 对应 0x28/'L'。
* @return 构造函数无显式返回值。
*
* @calls 不访问总线，首次通信发生在 ReadReadiness 或 CommandMotion。
* @safety 构造过程不触发硬件通信或运动。
* @note tool_bus 生命周期必须长于本适配器。
*/
LinkerHandAdapter(
rm_control::interfaces::IToolModbusBus &tool_bus,
rm_control::interfaces::DexterousHandSide side);
LinkerHandAdapter(const LinkerHandAdapter &) = delete;
LinkerHandAdapter &operator=(const LinkerHandAdapter &) = delete;
LinkerHandAdapter(LinkerHandAdapter &&) = delete;
LinkerHandAdapter &operator=(LinkerHandAdapter &&) = delete;
/**
* @brief 销毁 O6 协议适配器。
*
* @reason 适配器只借用总线引用，因此析构不应改变工具电压、通信模式或手部姿态。
*
* @author 唐永康
* @date 2026-07-10
*
* @return 析构函数无返回值且不抛出异常。
*
* @calls 不调用总线或厂商接口。
* @safety 不触发硬件动作，也不自动断电或松手。
* @note 总线资源由其所有者负责清理。
*/
~LinkerHandAdapter() noexcept override = default;
/**
* @brief 读取并验证当前 O6 的工具总线和全部 readiness 输入寄存器。
*
* @reason 适配器集中执行 O6 地址、身份方向、六自由度、位置、转矩、速度、温度和
* 故障规则，使上层只接收完整合规的 readiness，而不会误用部分读取结果。
*
* @author 唐永康
* @date 2026-07-11
*
* @return 全部读取与校验成功时返回 `ready=true` 的快照；否则返回首个带上下文的失败。
*
* @calls IToolModbusBus::GetStatus 和 IToolModbusBus::ReadInputRegister。
* @safety 仅使用功能码 04 读取状态，不触发 O6 或 RM75 运动。
* @note 不自动重试；调用方不得把失败前读取的部分字段作为安全门控依据。
*/
rm_control::common::Result<rm_control::interfaces::DexterousHandReadiness>
ReadReadiness() override;
/**
* @brief 按转矩、速度、最后位置的顺序下发 O6 FC16 动作命令。
*
* @reason O6 的 FC04 地址返回实时转矩和实时速度，并不是 FC16 命令回读；因此只校验
* 三组命令范围和每次 FC16 返回码，动作后的实际位置由上层 ReadReadiness 确认。
*
* @author 唐永康
* @date 2026-07-11
*
* @param command 六关节位置、速度和转矩命令，每项有效范围 0..255；调用期间只读。
* @return 全部输入合规且三次 FC16 写入均成功时成功，否则返回首个错误。
*
* @calls IToolModbusBus::WriteHoldingRegisters。
* @safety 可能立即触发真实 O6 动作，只能在上层 Live、安全限制和操作者确认门控后调用。
* @note 不读取 readiness 且不自动重试；转矩或速度任一步失败时不会下发位置。
*/
rm_control::common::Result<void> CommandMotion(
const rm_control::interfaces::O6MotionCommand &command) override;
private:
rm_control::interfaces::IToolModbusBus &tool_bus_;
rm_control::interfaces::DexterousHandSide side_;
};
} // namespace flight_robot::linker_hand
include/flight_robot/realman/modbus_register_codec.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/common/result.h"
#include <cstdint>
#include <vector>
namespace flight_robot::realman {
/**
* @brief 将 Modbus 16 位寄存器编码为 RM_API2 使用的高字节、低字节 int 序列。
*
* @reason RM_API2 的多寄存器接口以 `int*` 承载 `num * 2` 个字节；集中编码可避免每个
* 厂商调用各自实现位移和掩码，并允许完全脱离硬件验证边界值。
*
* @author 唐永康
* @date 2026-07-11
*
* @param register_values 按 Modbus 地址递增排列的 16 位寄存器值；不得为空。
* @return 成功时每个寄存器生成两个 0..255 的 int，顺序为高字节后低字节；输入为空或
* 长度无法安全扩展为两倍时返回失败。
*
* @calls 只使用标准库容器和整数位运算，不调用 RM_API2。
* @safety 纯数据转换，不连接设备，也不触发机械臂或 O6 动作。
* @note 返回缓冲区长度严格等于 `register_values.size() * 2`。
*/
rm_control::common::Result<std::vector<int>>
EncodeModbusRegistersAsRmApiHighLowBytes(
const std::vector<std::uint16_t> &register_values);
/**
* @brief 将 RM_API2 返回的高低字节 int 序列解码为 Modbus 16 位寄存器。
*
* @reason RM_API2 文档把 `int*` 元素描述为 int8，不同实现可能返回有符号 int8 或
* 0..255 原始字节；统一验证和归一化可拒绝掩码会静默截断的非法整数。
*
* @author 唐永康
* @date 2026-07-11
*
* @param rm_api_high_low_bytes 高字节、低字节交替排列的 RM_API2 int 序列；不得为空。
* @return 成功时返回按地址递增的寄存器；长度为奇数或元素超出 -128..255 时返回失败。
*
* @calls 只使用标准库容器和整数位运算，不调用 RM_API2。
* @safety 纯数据转换，不连接设备，也不触发机械臂或 O6 动作。
* @note -128..-1 按有符号 int8 的同位无符号字节归一化，编码函数始终输出 0..255。
*/
rm_control::common::Result<std::vector<std::uint16_t>>
DecodeModbusRegistersFromRmApiHighLowBytes(
const std::vector<int> &rm_api_high_low_bytes);
} // namespace flight_robot::realman
include/flight_robot/realman/realman_arm_adapter.h 0 → 100644
View file @ 8275ee2e
#pragma once
#include "rm_control/interfaces/robot_arm.h"
#include "rm_control/interfaces/tool_modbus_bus.h"
#include <memory>
namespace flight_robot::realman {
/**
* @brief 将统一机械臂接口映射到睿尔曼 RM_API2。
*
* @reason 使用 PImpl 隔离厂商 SDK 类型，确保业务层和兼容头不包含 rm_interface.h。
*
* @author 唐永康
* @date 2026-07-10
*
* @calls RM_API2 连接、状态、运动、拖动示教和原生轨迹复现接口。
* @safety 本类包含真实运动能力；调用方必须先通过上层安全门控。
* @note 当前只支持七自由度 RM75-6F，不提供其他机械臂型号兼容承诺。
*/
class RealmanArmAdapter final : public rm_control::interfaces::IRobotArm,
public rm_control::interfaces::IToolModbusBus {
public:
/**
* @brief 构造未连接的睿尔曼适配器。
* @reason PImpl 在构造期准备私有状态，同时不初始化 SDK 或连接设备。
* @author 唐永康
* @date 2026-07-10
* @return 构造函数无返回值。
* @calls 不调用厂商接口。
* @safety 不触发运动或硬件通信。
* @note 调用 connect 前所有硬件接口都会返回未连接错误。
*/
RealmanArmAdapter();
RealmanArmAdapter(const RealmanArmAdapter &) = delete;
RealmanArmAdapter &operator=(const RealmanArmAdapter &) = delete;
RealmanArmAdapter(RealmanArmAdapter &&) = delete;
RealmanArmAdapter &operator=(RealmanArmAdapter &&) = delete;
/**
* @brief 析构适配器并释放可能存在的 SDK 连接。
* @reason RAII 保证异常路径和提前返回路径不会泄漏厂商句柄。
* @author 唐永康
* @date 2026-07-10
* @return 析构函数无返回值且不抛出异常。
* @calls disconnect、厂商断开与 SDK 销毁接口。
* @safety 只执行资源清理，不主动发送运动指令。
* @note SDK 清理错误仅记录到标准错误流。
*/
~RealmanArmAdapter() noexcept override;
/** @copydoc rm_control::interfaces::IRobotArm::connect */
rm_control::common::Result<void>
connect(const rm_control::common::RobotConnectionConfig &config) override;
/** @copydoc rm_control::interfaces::IRobotArm::disconnect */
void disconnect() override;
/** @copydoc rm_control::interfaces::IRobotArm::isConnected */
bool isConnected() const noexcept override;
/** @copydoc rm_control::interfaces::IRobotArm::getRobotInfo */
rm_control::common::Result<rm_control::common::RobotInfo> getRobotInfo() override;
/** @copydoc rm_control::interfaces::IRobotArm::getRobotSoftwareInfo */
rm_control::common::Result<rm_control::common::RobotSoftwareInfo>
getRobotSoftwareInfo() override;
/** @copydoc rm_control::interfaces::IRobotArm::getCurrentArmState */
rm_control::common::Result<rm_control::common::ArmState> getCurrentArmState() override;
/** @copydoc rm_control::interfaces::IRobotArm::getForceData */
rm_control::common::Result<rm_control::common::ForceData> getForceData() override;
/** @copydoc rm_control::interfaces::IRobotArm::getRobotSafetyStatus */
rm_control::common::Result<rm_control::common::RobotSafetyStatus>
getRobotSafetyStatus() override;
/** @copydoc rm_control::interfaces::IRobotArm::getJointLimits */
rm_control::common::Result<rm_control::common::RobotJointLimits>
getJointLimits() override;
/** @copydoc rm_control::interfaces::IRobotArm::moveJ */
rm_control::common::Result<void> moveJ(const std::vector<float> &joints_deg,
int speed_percent,
int blend_radius,
int trajectory_connect,
bool blocking) override;
/** @copydoc rm_control::interfaces::IRobotArm::moveL */
rm_control::common::Result<void> moveL(const rm_control::common::Pose &pose,
int speed_percent,
int blend_radius,
int trajectory_connect,
bool blocking) override;
/** @copydoc rm_control::interfaces::IRobotArm::startDragTeach */
rm_control::common::Result<void>
startDragTeach(const rm_control::common::DragTeachStartOptions &options) override;
/** @copydoc rm_control::interfaces::IRobotArm::stopDragTeach */
rm_control::common::Result<void> stopDragTeach() override;
/** @copydoc rm_control::interfaces::IRobotArm::saveDragTeachTrajectory */
rm_control::common::VendorTrajectorySaveAttempt
saveDragTeachTrajectory(const std::filesystem::path &path) override;
/** @copydoc rm_control::interfaces::IRobotArm::moveToDragTrajectoryOrigin */
rm_control::common::Result<void> moveToDragTrajectoryOrigin(bool blocking) override;
/** @copydoc rm_control::interfaces::IRobotArm::replayLastDragTrajectory */
rm_control::common::Result<void> replayLastDragTrajectory(bool blocking) override;
/** @copydoc rm_control::interfaces::IRobotArm::pauseDragTrajectoryReplay */
rm_control::common::Result<void> pauseDragTrajectoryReplay() override;
/** @copydoc rm_control::interfaces::IRobotArm::continueDragTrajectoryReplay */
rm_control::common::Result<void> continueDragTrajectoryReplay() override;
/** @copydoc rm_control::interfaces::IRobotArm::stopDragTrajectoryReplay */
rm_control::common::Result<void> stopDragTrajectoryReplay() override;
/** @copydoc rm_control::interfaces::IRobotArm::saveVendorDragTrajectoryProgram */
rm_control::common::Result<void> saveVendorDragTrajectoryProgram(
const rm_control::common::VendorDragTrajectoryExecutionRequest &request) override;
/** @copydoc rm_control::interfaces::IRobotArm::runControllerProgram */
rm_control::common::Result<void> runControllerProgram(
const rm_control::common::ControllerProgramRunRequest &request) override;
/** @copydoc rm_control::interfaces::IRobotArm::executeVendorDragTrajectoryFile */
rm_control::common::Result<void> executeVendorDragTrajectoryFile(
const rm_control::common::VendorDragTrajectoryExecutionRequest &request) override;
/** @copydoc rm_control::interfaces::IRobotArm::getControllerProgramRunState */
rm_control::common::Result<rm_control::common::ControllerProgramRunState>
getControllerProgramRunState() override;
/** @copydoc rm_control::interfaces::IRobotArm::slowStopMotion */
rm_control::common::Result<void> slowStopMotion() override;
/** @copydoc rm_control::interfaces::IRobotArm::isControllerProgramSlotAvailable */
rm_control::common::Result<bool> isControllerProgramSlotAvailable(
int controller_program_id) override;
/** @copydoc rm_control::interfaces::IToolModbusBus::GetStatus */
rm_control::common::Result<rm_control::interfaces::ToolModbusBusStatus>
GetStatus() override;
/** @copydoc rm_control::interfaces::IToolModbusBus::Configure */
rm_control::common::Result<rm_control::interfaces::ToolModbusBusStatus>
Configure(
const rm_control::interfaces::ToolModbusBusConfiguration &configuration) override;
/** @copydoc rm_control::interfaces::IToolModbusBus::ReadInputRegister */
rm_control::common::Result<std::uint16_t> ReadInputRegister(
std::uint8_t device_id,
std::uint16_t address) override;
/** @copydoc rm_control::interfaces::IToolModbusBus::ReadInputRegisters */
rm_control::common::Result<std::vector<std::uint16_t>> ReadInputRegisters(
std::uint8_t device_id,
std::uint16_t start_address,
std::size_t register_count) override;
/** @copydoc rm_control::interfaces::IToolModbusBus::ReadHoldingRegisters */
rm_control::common::Result<std::vector<std::uint16_t>> ReadHoldingRegisters(
std::uint8_t device_id,
std::uint16_t start_address,
std::size_t register_count) override;
/** @copydoc rm_control::interfaces::IToolModbusBus::WriteHoldingRegisters */
rm_control::common::Result<void> WriteHoldingRegisters(
std::uint8_t device_id,
std::uint16_t start_address,
const std::vector<std::uint16_t> &values) override;
private:
class Implementation;
std::unique_ptr<Implementation> implementation_;
};
} // namespace flight_robot::realman
src/linker_hand/linker_hand_adapter.cpp 0 → 100644
View file @ 8275ee2e
#include "flight_robot/linker_hand/linker_hand_adapter.h"
#include <algorithm>
#include <string>
#include <vector>
namespace flight_robot::linker_hand {
namespace {
using rm_control::common::Result;
using rm_control::interfaces::DexterousHandReadiness;
using rm_control::interfaces::DexterousHandSide;
using rm_control::interfaces::IToolModbusBus;
using rm_control::interfaces::O6JointPose;
using rm_control::interfaces::O6MotionCommand;
using rm_control::interfaces::ToolModbusBusStatus;
constexpr std::uint8_t kRightHandDeviceId = 0x27U;
constexpr std::uint8_t kLeftHandDeviceId = 0x28U;
constexpr int kRequiredToolPort = 1;
constexpr int kRequiredModbusMode = 1;
constexpr int kRequiredBaudrate = 115200;
constexpr int kRequiredVoltageType = 3;
constexpr std::uint16_t kPositionStartAddress = 0U;
constexpr std::uint16_t kTorqueStartAddress = 6U;
constexpr std::uint16_t kSpeedStartAddress = 12U;
constexpr std::uint16_t kTemperatureStartAddress = 18U;
constexpr std::uint16_t kFaultStartAddress = 24U;
constexpr std::uint16_t kIdentityStartAddress = 30U;
constexpr std::uint16_t kMaximumJointPosition = 255U;
constexpr std::uint16_t kMaximumTemperatureCelsius = 70U;
constexpr std::uint16_t kExpectedFreedom = 6U;
std::uint8_t DeviceIdForSide(DexterousHandSide side) {
return side == DexterousHandSide::Right ? kRightHandDeviceId
: kLeftHandDeviceId;
}
char DirectionForSide(DexterousHandSide side) {
return side == DexterousHandSide::Right ? 'R' : 'L';
}
Result<void> ValidateToolBusStatus(const ToolModbusBusStatus &status) {
if (!status.connected) {
return Result<void>::failure("O6 readiness rejected: tool bus is disconnected");
}
if (status.port != kRequiredToolPort || status.mode != kRequiredModbusMode ||
status.baudrate != kRequiredBaudrate ||
status.voltage_type != kRequiredVoltageType || status.timeout_units <= 0) {
return Result<void>::failure(
"O6 readiness rejected: expected port=1, mode=1, baudrate=115200, "
"voltage_type=3 and timeout_units>0; actual port=" +
std::to_string(status.port) + ", mode=" + std::to_string(status.mode) +
", baudrate=" + std::to_string(status.baudrate) +
", voltage_type=" + std::to_string(status.voltage_type) +
", timeout_units=" + std::to_string(status.timeout_units));
}
return Result<void>::success();
}
Result<std::vector<std::uint16_t>> ReadSixInputRegisters(
IToolModbusBus &tool_bus,
std::uint8_t device_id,
std::uint16_t start_address,
const std::string &group_name) {
std::vector<std::uint16_t> registers;
registers.reserve(rm_control::interfaces::kO6JointCount);
for (std::size_t offset = 0U;
offset < rm_control::interfaces::kO6JointCount; ++offset) {
const auto address = static_cast<std::uint16_t>(start_address + offset);
const auto result = tool_bus.ReadInputRegister(device_id, address);
if (!result.ok) {
return Result<std::vector<std::uint16_t>>::failure(
"failed to read O6 " + group_name + " input register address " +
std::to_string(address) + ": " + result.error_message);
}
registers.push_back(result.value);
}
return Result<std::vector<std::uint16_t>>::success(registers);
}
Result<void> CopyBoundedRegisterGroup(
const std::vector<std::uint16_t> &registers,
std::uint16_t maximum_value,
const std::string &group_name,
O6JointPose *destination) {
for (std::size_t index = 0; index < registers.size(); ++index) {
if (registers[index] > maximum_value) {
return Result<void>::failure(
"invalid O6 " + group_name + " at index " +
std::to_string(index) + ": " + std::to_string(registers[index]) +
" exceeds " + std::to_string(maximum_value));
}
}
std::copy(registers.begin(), registers.end(), destination->begin());
return Result<void>::success();
}
Result<void> ReadAndCopyBoundedInputRegisterGroup(
IToolModbusBus &tool_bus,
std::uint8_t device_id,
std::uint16_t start_address,
std::uint16_t maximum_value,
const std::string &group_name,
O6JointPose *destination) {
const auto registers = ReadSixInputRegisters(
tool_bus, device_id, start_address, group_name);
if (!registers.ok) {
return Result<void>::failure(registers.error_message);
}
return CopyBoundedRegisterGroup(
registers.value, maximum_value, group_name, destination);
}
Result<void> CopyFaultRegisters(const std::vector<std::uint16_t> &registers,
O6JointPose *destination) {
for (std::size_t index = 0; index < registers.size(); ++index) {
if (registers[index] != 0U) {
return Result<void>::failure(
"O6 fault at joint index " + std::to_string(index) +
": code=" + std::to_string(registers[index]));
}
}
std::copy(registers.begin(), registers.end(), destination->begin());
return Result<void>::success();
}
Result<void> CopyAndValidateIdentity(
const std::vector<std::uint16_t> &registers,
char expected_direction,
DexterousHandReadiness *readiness) {
if (registers[0] != kExpectedFreedom) {
return Result<void>::failure(
"invalid O6 freedom: expected 6, got " +
std::to_string(registers[0]));
}
if (registers[5] != static_cast<std::uint16_t>(expected_direction)) {
return Result<void>::failure(
"invalid O6 direction: expected " + std::string(1, expected_direction) +
", got register value " + std::to_string(registers[5]));
}
readiness->freedom = registers[0];
readiness->hand_version = registers[1];
std::copy_n(registers.begin() + 2, readiness->hand_number_parts.size(),
readiness->hand_number_parts.begin());
readiness->direction = expected_direction;
return Result<void>::success();
}
Result<void> ReadAndValidateO6Registers(IToolModbusBus &tool_bus,
char expected_direction,
DexterousHandReadiness *readiness) {
const auto identity = ReadSixInputRegisters(
tool_bus, readiness->device_id, kIdentityStartAddress, "identity");
if (!identity.ok) {
return Result<void>::failure(identity.error_message);
}
auto validation =
CopyAndValidateIdentity(identity.value, expected_direction, readiness);
if (!validation.ok) {
return validation;
}
validation = ReadAndCopyBoundedInputRegisterGroup(
tool_bus, readiness->device_id, kPositionStartAddress,
kMaximumJointPosition, "position", &readiness->joint_positions);
if (!validation.ok) return validation;
validation = ReadAndCopyBoundedInputRegisterGroup(
tool_bus, readiness->device_id, kTorqueStartAddress,
kMaximumJointPosition, "torque", &readiness->joint_torques);
if (!validation.ok) return validation;
validation = ReadAndCopyBoundedInputRegisterGroup(
tool_bus, readiness->device_id, kSpeedStartAddress,
kMaximumJointPosition, "speed", &readiness->joint_speeds);
if (!validation.ok) return validation;
validation = ReadAndCopyBoundedInputRegisterGroup(
tool_bus, readiness->device_id, kTemperatureStartAddress,
kMaximumTemperatureCelsius, "temperature",
&readiness->temperatures_celsius);
if (!validation.ok) return validation;
const auto faults = ReadSixInputRegisters(
tool_bus, readiness->device_id, kFaultStartAddress, "faults");
if (!faults.ok) {
return Result<void>::failure(faults.error_message);
}
return CopyFaultRegisters(faults.value, &readiness->fault_codes);
}
Result<void> ValidateMotionCommandGroup(const O6JointPose &values,
const std::string &group_name) {
for (std::size_t index = 0; index < values.size(); ++index) {
if (values[index] > kMaximumJointPosition) {
return Result<void>::failure(
"invalid O6 " + group_name + " command at joint index " +
std::to_string(index) + ": " + std::to_string(values[index]) +
" exceeds 255");
}
}
return Result<void>::success();
}
Result<void> WriteCommandRegisterGroup(
IToolModbusBus &tool_bus,
std::uint8_t device_id,
std::uint16_t start_address,
const O6JointPose &values,
const std::string &group_name) {
const std::vector<std::uint16_t> register_values(values.begin(), values.end());
const auto write = tool_bus.WriteHoldingRegisters(
device_id, start_address, register_values);
if (!write.ok) {
return Result<void>::failure(
"failed to write O6 " + group_name + " commands: " +
write.error_message);
}
return Result<void>::success();
}
} // namespace
LinkerHandAdapter::LinkerHandAdapter(IToolModbusBus &tool_bus,
DexterousHandSide side)
: tool_bus_(tool_bus), side_(side) {}
Result<DexterousHandReadiness> LinkerHandAdapter::ReadReadiness() {
const auto status = tool_bus_.GetStatus();
if (!status.ok) {
return Result<DexterousHandReadiness>::failure(
"failed to read O6 tool bus status: " + status.error_message);
}
const auto status_validation = ValidateToolBusStatus(status.value);
if (!status_validation.ok) {
return Result<DexterousHandReadiness>::failure(
status_validation.error_message);
}
DexterousHandReadiness readiness;
readiness.side = side_;
readiness.device_id = DeviceIdForSide(side_);
readiness.tool_bus_status = status.value;
const auto registers = ReadAndValidateO6Registers(
tool_bus_, DirectionForSide(side_), &readiness);
if (!registers.ok) {
return Result<DexterousHandReadiness>::failure(registers.error_message);
}
readiness.ready = true;
return Result<DexterousHandReadiness>::success(readiness);
}
Result<void> LinkerHandAdapter::CommandMotion(const O6MotionCommand &command) {
auto validation = ValidateMotionCommandGroup(
command.target_positions, "position");
if (!validation.ok) {
return validation;
}
validation = ValidateMotionCommandGroup(
command.joint_speed_commands, "speed");
if (!validation.ok) {
return validation;
}
validation = ValidateMotionCommandGroup(
command.joint_torque_commands, "torque");
if (!validation.ok) {
return validation;
}
const std::uint8_t device_id = DeviceIdForSide(side_);
auto configured = WriteCommandRegisterGroup(
tool_bus_, device_id, kTorqueStartAddress,
command.joint_torque_commands, "torque");
if (!configured.ok) {
return configured;
}
configured = WriteCommandRegisterGroup(
tool_bus_, device_id, kSpeedStartAddress,
command.joint_speed_commands, "speed");
if (!configured.ok) {
return configured;
}
const std::vector<std::uint16_t> positions(
command.target_positions.begin(), command.target_positions.end());
const auto position_write = tool_bus_.WriteHoldingRegisters(
device_id, kPositionStartAddress, positions);
if (!position_write.ok) {
return Result<void>::failure("failed to write O6 joint pose: " +
position_write.error_message);
}
return Result<void>::success();
}
} // namespace flight_robot::linker_hand
src/realman/modbus_register_codec.cpp 0 → 100644
View file @ 8275ee2e
#include "flight_robot/realman/modbus_register_codec.h"
#include <cstddef>
#include <string>
namespace flight_robot::realman {
namespace {
constexpr int kMinimumSignedRmApiByte = -128;
constexpr int kMaximumUnsignedRmApiByte = 255;
constexpr int kUnsignedByteValueCount = 256;
constexpr std::size_t kBytesPerModbusRegister = 2U;
bool IsValidRmApiByte(int value) {
return value >= kMinimumSignedRmApiByte &&
value <= kMaximumUnsignedRmApiByte;
}
std::uint16_t NormalizeRmApiByte(int value) {
const int normalized = value < 0 ? value + kUnsignedByteValueCount : value;
return static_cast<std::uint16_t>(normalized);
}
} // namespace
rm_control::common::Result<std::vector<int>>
EncodeModbusRegistersAsRmApiHighLowBytes(
const std::vector<std::uint16_t> &register_values) {
if (register_values.empty()) {
return rm_control::common::Result<std::vector<int>>::failure(
"cannot encode an empty Modbus register sequence");
}
std::vector<int> encoded_bytes;
if (register_values.size() >
encoded_bytes.max_size() / kBytesPerModbusRegister) {
return rm_control::common::Result<std::vector<int>>::failure(
"Modbus register sequence is too large to encode as num * 2 bytes");
}
encoded_bytes.reserve(register_values.size() * kBytesPerModbusRegister);
for (const std::uint16_t value : register_values) {
encoded_bytes.push_back(static_cast<int>((value >> 8U) & 0xFFU));
encoded_bytes.push_back(static_cast<int>(value & 0xFFU));
}
return rm_control::common::Result<std::vector<int>>::success(encoded_bytes);
}
rm_control::common::Result<std::vector<std::uint16_t>>
DecodeModbusRegistersFromRmApiHighLowBytes(
const std::vector<int> &rm_api_high_low_bytes) {
if (rm_api_high_low_bytes.empty() ||
rm_api_high_low_bytes.size() % kBytesPerModbusRegister != 0U) {
return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
"RM_API2 register byte sequence must contain a nonzero even byte count");
}
std::vector<std::uint16_t> register_values;
register_values.reserve(rm_api_high_low_bytes.size() /
kBytesPerModbusRegister);
for (std::size_t byte_index = 0;
byte_index < rm_api_high_low_bytes.size();
byte_index += kBytesPerModbusRegister) {
const int high = rm_api_high_low_bytes[byte_index];
const int low = rm_api_high_low_bytes[byte_index + 1U];
if (!IsValidRmApiByte(high) || !IsValidRmApiByte(low)) {
const std::size_t invalid_index =
!IsValidRmApiByte(high) ? byte_index : byte_index + 1U;
return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
"invalid RM_API2 byte at index " + std::to_string(invalid_index) +
": " + std::to_string(rm_api_high_low_bytes[invalid_index]));
}
register_values.push_back(static_cast<std::uint16_t>(
(NormalizeRmApiByte(high) << 8U) | NormalizeRmApiByte(low)));
}
return rm_control::common::Result<std::vector<std::uint16_t>>::success(
register_values);
}
} // namespace flight_robot::realman
src/realman/realman_arm_adapter.cpp 0 → 100644
View file @ 8275ee2e
#include "flight_robot/realman/realman_arm_adapter.h"
#include "flight_robot/realman/modbus_register_codec.h"
#include "rm_control/common/rm75_constants.h"
#include "rm_interface.h"
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdarg>
#include <cstdint>
#include <cstdio>
#include <cstring>
#include <exception>
#include <filesystem>
#include <string>
#include <thread>
namespace flight_robot::realman {
namespace {
constexpr int kOrdinaryDragTrajectoryRecordEnabled = 1;
constexpr int kSixDofPositionAndOrientationDragMode = 3;
constexpr std::size_t kMaximumVendorTrajectoryPathLength = 299;
constexpr int kMaximumArmErrorCount = 24;
constexpr int kMinimumControllerProgramId = 1;
constexpr int kMaximumControllerProgramId = 100;
constexpr int kMinimumPlanSpeedPercent = 1;
constexpr int kMaximumPlanSpeedPercent = 100;
constexpr int kSaveAndRunControllerProject = 0;
constexpr int kOnlySaveControllerProject = 1;
constexpr int kToolRs485Port = 1;
constexpr int kThirdGenerationController = 3;
constexpr int kToolModbusMasterMode = 1;
constexpr std::chrono::milliseconds kToolModbusModeSettlingDelay(300);
constexpr std::size_t kMinimumMultipleRegisterReadCount = 3;
constexpr std::size_t kMaximumMultipleRegisterReadCount = 12;
constexpr std::size_t kMaximumMultipleRegisterWriteCount = 10;
void apiLogCallback(const char *message, va_list arguments) {
if (message == nullptr) {
return;
}
std::array<char, 1024> buffer{};
std::vsnprintf(buffer.data(), buffer.size(), message, arguments);
std::fprintf(stderr, "[rm_api] %s\n", buffer.data());
}
int destroyApi() noexcept {
#if defined(REALMAN_RM_API_HAS_RM_DESTORY)
return rm_destory();
#else
return rm_destroy();
#endif
}
rm_control::common::Result<void> sdkResult(const char *api_name, int return_code) {
if (return_code == 0) {
return rm_control::common::Result<void>::success();
}
return rm_control::common::Result<void>::failure(
std::string(api_name) + " failed, return_code=" + std::to_string(return_code));
}
rm_control::common::Result<void> ensureConnected(bool initialized,
const rm_robot_handle *handle) {
if (!initialized || handle == nullptr || handle->id <= 0) {
return rm_control::common::Result<void>::failure(
"RealmanArmAdapter requires an active RM75 connection");
}
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> ValidateToolModbusConfiguration(
const rm_control::interfaces::ToolModbusBusConfiguration &configuration) {
const bool supported_baudrate = configuration.baudrate == 9600 ||
configuration.baudrate == 115200 || configuration.baudrate == 460800;
const bool valid_delays = configuration.power_off_delay_ms >= 0 &&
configuration.power_off_delay_ms <= 10000 &&
configuration.startup_delay_ms >= 0 &&
configuration.startup_delay_ms <= 10000;
if (configuration.port != kToolRs485Port || !supported_baudrate ||
configuration.voltage_type < 0 || configuration.voltage_type > 3 ||
configuration.timeout_units <= 0 || configuration.timeout_units > 1000 ||
!valid_delays) {
return rm_control::common::Result<void>::failure(
"tool Modbus configuration requires port=1, supported baudrate, "
"voltage_type=0..3, timeout_units=1..1000, and delays=0..10000 ms");
}
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> VerifyThirdGenerationController(
rm_robot_handle *handle) {
rm_robot_info_t information{};
const int return_code = rm_get_robot_info(handle, &information);
if (return_code != 0) {
return sdkResult("rm_get_robot_info", return_code);
}
if (static_cast<int>(information.robot_controller_version) !=
kThirdGenerationController) {
return rm_control::common::Result<void>::failure(
"legacy tool Modbus initialization requires controller generation 3; actual=" +
std::to_string(static_cast<int>(information.robot_controller_version)));
}
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> SetToolVoltageAndVerify(
rm_robot_handle *handle,
int voltage_type) {
auto result = sdkResult(
"rm_set_tool_voltage", rm_set_tool_voltage(handle, voltage_type));
if (!result.ok) {
return result;
}
int actual_voltage_type = -1;
result = sdkResult(
"rm_get_tool_voltage", rm_get_tool_voltage(handle, &actual_voltage_type));
if (!result.ok) {
return result;
}
if (actual_voltage_type != voltage_type) {
return rm_control::common::Result<void>::failure(
"tool voltage readback mismatch: requested=" +
std::to_string(voltage_type) + ", actual=" +
std::to_string(actual_voltage_type));
}
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> ResetToolModbusModeIfNeeded(
rm_robot_handle *handle,
int port,
int current_mode,
bool reset_existing_mode) {
if (!reset_existing_mode || current_mode != kToolModbusMasterMode) {
return rm_control::common::Result<void>::success();
}
return sdkResult("rm_close_modbus_mode", rm_close_modbus_mode(handle, port));
}
using RmMultipleRegisterReadFunction = int (*)(
rm_robot_handle *, rm_peripheral_read_write_params_t, int *);
struct MultipleRegisterReadRequest {
std::uint8_t device_id = 0U;
std::uint16_t start_address = 0U;
std::size_t register_count = 0U;
const char *api_name = nullptr;
RmMultipleRegisterReadFunction read_function = nullptr;
};
rm_control::common::Result<std::vector<std::uint16_t>>
ReadMultipleRegistersFromRmApi(bool initialized,
rm_robot_handle *handle,
const MultipleRegisterReadRequest &request) {
const auto connected = ensureConnected(initialized, handle);
if (!connected.ok) {
return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
connected.error_message);
}
if (request.device_id == 0U || request.device_id > 247U ||
request.register_count < kMinimumMultipleRegisterReadCount ||
request.register_count > kMaximumMultipleRegisterReadCount ||
request.api_name == nullptr || request.read_function == nullptr) {
return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
"RM_API2 multiple-register read requires device 1..247, count 3..12, "
"and a valid SDK function");
}
rm_peripheral_read_write_params_t parameters{};
parameters.port = kToolRs485Port;
parameters.address = static_cast<int>(request.start_address);
parameters.device = static_cast<int>(request.device_id);
parameters.num = static_cast<int>(request.register_count);
std::vector<int> bytes(request.register_count * 2U, 0);
const int return_code = request.read_function(handle, parameters, bytes.data());
if (return_code != 0) {
return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
std::string(request.api_name) + " failed, return_code=" +
std::to_string(return_code) + ", device=" +
std::to_string(static_cast<int>(request.device_id)) + ", address=" +
std::to_string(request.start_address) + ", count=" +
std::to_string(request.register_count));
}
const auto decoded = DecodeModbusRegistersFromRmApiHighLowBytes(bytes);
if (!decoded.ok) {
return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
std::string(request.api_name) + " returned invalid bytes: " +
decoded.error_message);
}
return decoded;
}
template <std::size_t Size>
std::string boundedString(const char (&text)[Size]) {
const auto end = std::find(text, text + Size, '\0');
return std::string(text, end);
}
rm_control::common::Pose convertPose(const rm_pose_t &pose) {
rm_control::common::Pose result;
result.x = pose.position.x;
result.y = pose.position.y;
result.z = pose.position.z;
result.rx = pose.euler.rx;
result.ry = pose.euler.ry;
result.rz = pose.euler.rz;
return result;
}
rm_pose_t convertPose(const rm_control::common::Pose &pose) {
rm_pose_t result{};
result.position.x = pose.x;
result.position.y = pose.y;
result.position.z = pose.z;
result.euler.rx = pose.rx;
result.euler.ry = pose.ry;
result.euler.rz = pose.rz;
result.quaternion.w = 1.0F;
return result;
}
void appendNonZeroErrors(const rm_err_t &source, std::vector<int> *destination) {
if (destination == nullptr) {
return;
}
const int count = std::min(static_cast<int>(source.err_len), kMaximumArmErrorCount);
for (int index = 0; index < count; ++index) {
if (source.err[index] != 0) {
destination->push_back(source.err[index]);
}
}
}
rm_control::common::Result<void> readRunModeAndPower(
rm_robot_handle *handle,
rm_control::common::RobotSafetyStatus *status) {
int run_mode = 0;
int return_code = rm_get_arm_run_mode(handle, &run_mode);
if (return_code != 0) {
return sdkResult("rm_get_arm_run_mode", return_code);
}
int power_state = 0;
return_code = rm_get_arm_power_state(handle, &power_state);
if (return_code != 0) {
return sdkResult("rm_get_arm_power_state", return_code);
}
status->real_mode_enabled = run_mode == 1;
status->arm_power_on = power_state == 1;
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> readJointAndArmSafety(
rm_robot_handle *handle,
rm_control::common::RobotSafetyStatus *status) {
rm_arm_all_state_t arm_state{};
const int return_code = rm_get_arm_all_state(handle, &arm_state);
if (return_code != 0) {
return sdkResult("rm_get_arm_all_state", return_code);
}
status->all_joints_enabled = true;
for (std::size_t index = 0; index < rm_control::common::kRm75JointCount; ++index) {
status->joint_enable_states[index] = arm_state.joint_en_flag[index];
status->joint_error_codes[index] = arm_state.joint_err_code[index];
status->all_joints_enabled =
status->all_joints_enabled && arm_state.joint_en_flag[index] == 1;
}
appendNonZeroErrors(arm_state.err, &status->arm_error_codes);
status->arm_error_free = status->arm_error_codes.empty() &&
std::all_of(status->joint_error_codes.begin(),
status->joint_error_codes.end(),
[](int code) { return code == 0; });
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> readControllerSafety(
rm_robot_handle *handle,
rm_control::common::RobotSafetyStatus *status) {
const int return_code = rm_get_controller_state(handle,
&status->controller_voltage,
&status->controller_current,
&status->controller_temperature,
&status->controller_error_code);
if (return_code != 0) {
return sdkResult("rm_get_controller_state", return_code);
}
status->controller_error_free = status->controller_error_code == 0;
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> readConfiguredSafetyFeatures(
rm_robot_handle *handle,
rm_control::common::RobotSafetyStatus *status) {
rm_electronic_fence_enable_t virtual_wall{};
int return_code = rm_get_virtual_wall_enable(handle, &virtual_wall);
if (return_code != 0) {
return sdkResult("rm_get_virtual_wall_enable", return_code);
}
bool self_collision_enabled = false;
return_code = rm_get_self_collision_enable(handle, &self_collision_enabled);
if (return_code != 0) {
return sdkResult("rm_get_self_collision_enable", return_code);
}
status->virtual_wall_enabled = virtual_wall.enable_state;
status->self_collision_enabled = self_collision_enabled;
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<bool> queryControllerProgramSlotAvailable(
rm_robot_handle *handle,
int controller_program_id) {
rm_program_trajectorys_t programs{};
const int return_code = rm_get_program_trajectory_list(
handle, 1, 100, "", &programs);
if (return_code != 0) {
return rm_control::common::Result<bool>::failure(
sdkResult("rm_get_program_trajectory_list", return_code).error_message);
}
const int count = std::clamp(programs.list_size, 0, 100);
for (int index = 0; index < count; ++index) {
if (programs.trajectory_list[index].id == controller_program_id) {
return rm_control::common::Result<bool>::success(false);
}
}
return rm_control::common::Result<bool>::success(true);
}
rm_control::common::Result<std::string> validateVendorDragTrajectoryRequest(
const rm_control::common::VendorDragTrajectoryExecutionRequest &request) {
std::error_code filesystem_error;
if (!std::filesystem::is_regular_file(request.trajectory_path, filesystem_error) ||
filesystem_error) {
return rm_control::common::Result<std::string>::failure(
"RM75 vendor drag trajectory must be an existing regular file: " +
request.trajectory_path.string());
}
const std::string path = request.trajectory_path.string();
if (path.empty() || path.size() > kMaximumVendorTrajectoryPathLength) {
return rm_control::common::Result<std::string>::failure(
"rm_send_project path must contain 1..299 bytes, actual=" +
std::to_string(path.size()));
}
if (request.plan_speed_percent < kMinimumPlanSpeedPercent ||
request.plan_speed_percent > kMaximumPlanSpeedPercent) {
return rm_control::common::Result<std::string>::failure(
"rm_send_project plan speed percent must be in the range 1..100");
}
if (request.controller_program_id < kMinimumControllerProgramId ||
request.controller_program_id > kMaximumControllerProgramId) {
return rm_control::common::Result<std::string>::failure(
"rm_send_project controller program id must be in the range 1..100");
}
return rm_control::common::Result<std::string>::success(path);
}
rm_control::common::Result<void> requireControllerProgramSlotAvailable(
rm_robot_handle *handle,
int controller_program_id) {
const auto available = queryControllerProgramSlotAvailable(handle, controller_program_id);
if (!available.ok) {
return rm_control::common::Result<void>::failure(available.error_message);
}
if (!available.value) {
return rm_control::common::Result<void>::failure(
"refusing to overwrite existing controller program id=" +
std::to_string(controller_program_id));
}
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> requireControllerProgramSlotOccupied(
rm_robot_handle *handle,
int controller_program_id) {
const auto available = queryControllerProgramSlotAvailable(handle, controller_program_id);
if (!available.ok) {
return rm_control::common::Result<void>::failure(
"failed to confirm saved controller program id=" +
std::to_string(controller_program_id) + ": " + available.error_message);
}
if (available.value) {
return rm_control::common::Result<void>::failure(
"rm_send_project reported success but controller program id=" +
std::to_string(controller_program_id) + " is absent");
}
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> sendVendorDragTrajectoryProject(
rm_robot_handle *handle,
const rm_control::common::VendorDragTrajectoryExecutionRequest &request,
const std::string &path,
int only_save) {
rm_send_project_t project{};
std::memcpy(project.project_path, path.data(), path.size());
project.project_path[path.size()] = '\0';
project.project_path_len = static_cast<int>(path.size());
project.plan_speed = request.plan_speed_percent;
project.only_save = only_save;
project.save_id = request.controller_program_id;
project.step_flag = 0;
project.auto_start = 0;
project.project_type = 1;
int error_line = -1;
const int return_code = rm_send_project(handle, project, &error_line);
if (return_code != 0 || error_line != -1) {
return rm_control::common::Result<void>::failure(
"rm_send_project(project_type=1, only_save=" + std::to_string(only_save) +
") failed, return_code=" + std::to_string(return_code) +
", error_line=" + std::to_string(error_line));
}
return rm_control::common::Result<void>::success();
}
rm_control::common::Result<void> validateControllerProgramRunRequest(
const rm_control::common::ControllerProgramRunRequest &request) {
if (request.controller_program_id < kMinimumControllerProgramId ||
request.controller_program_id > kMaximumControllerProgramId) {
return rm_control::common::Result<void>::failure(
"rm_set_program_id_run controller program id must be in the range 1..100");
}
if (request.speed_percent < kMinimumPlanSpeedPercent ||
request.speed_percent > kMaximumPlanSpeedPercent) {
return rm_control::common::Result<void>::failure(
"rm_set_program_id_run speed percent must be in the range 1..100");
}
return rm_control::common::Result<void>::success();
}
} // namespace
class RealmanArmAdapter::Implementation {
public:
rm_robot_handle *handle = nullptr;
bool initialized = false;
int dof = 0;
};
static_assert(ARM_DOF == rm_control::common::kRm75JointCount,
"RM_API2 ARM_DOF must match the RM75 seven-joint contract");
RealmanArmAdapter::RealmanArmAdapter()
: implementation_(std::make_unique<Implementation>()) {}
RealmanArmAdapter::~RealmanArmAdapter() noexcept {
disconnect();
}
rm_control::common::Result<void> RealmanArmAdapter::connect(
const rm_control::common::RobotConnectionConfig &config) {
if (config.ip.empty() || config.port <= 0 || config.port > 65535 || config.timeout_ms <= 0) {
return rm_control::common::Result<void>::failure(
"RealmanArmAdapter connection config has invalid IP, port, or timeout");
}
disconnect();
rm_set_log_call_back(apiLogCallback, 2);
rm_set_timeout(config.timeout_ms);
const int init_return_code = rm_init(RM_TRIPLE_MODE_E);
if (init_return_code != 0) {
return sdkResult("rm_init", init_return_code);
}
implementation_->initialized = true;
implementation_->handle = rm_create_robot_arm(config.ip.c_str(), config.port);
if (implementation_->handle == nullptr || implementation_->handle->id <= 0) {
disconnect();
return rm_control::common::Result<void>::failure(
"rm_create_robot_arm failed for " + config.ip + ":" + std::to_string(config.port));
}
const auto info = getRobotInfo();
if (!info.ok || info.value.arm_dof != static_cast<int>(rm_control::common::kRm75JointCount) ||
!info.value.is_rm75) {
const std::string context = info.ok ? "connected robot is not an RM75 with seven joints"
: info.error_message;
disconnect();
return rm_control::common::Result<void>::failure(context);
}
implementation_->dof = info.value.arm_dof;
return rm_control::common::Result<void>::success();
}
void RealmanArmAdapter::disconnect() {
if (implementation_ == nullptr) {
return;
}
if (implementation_->handle != nullptr) {
const int delete_return_code = rm_delete_robot_arm(implementation_->handle);
if (delete_return_code != 0) {
std::fprintf(stderr, "[rm_api] rm_delete_robot_arm failed, return_code=%d\n",
delete_return_code);
}
implementation_->handle = nullptr;
}
if (implementation_->initialized) {
const int destroy_return_code = destroyApi();
if (destroy_return_code != 0) {
std::fprintf(stderr, "[rm_api] RM API destroy failed, return_code=%d\n",
destroy_return_code);
}
implementation_->initialized = false;
}
implementation_->dof = 0;
}
bool RealmanArmAdapter::isConnected() const noexcept {
return implementation_ != nullptr && implementation_->initialized &&
implementation_->handle != nullptr && implementation_->handle->id > 0;
}
rm_control::common::Result<rm_control::common::RobotInfo>
RealmanArmAdapter::getRobotInfo() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<rm_control::common::RobotInfo>::failure(
connected.error_message);
}
rm_robot_info_t info{};
const int return_code = rm_get_robot_info(implementation_->handle, &info);
if (return_code != 0) {
return rm_control::common::Result<rm_control::common::RobotInfo>::failure(
sdkResult("rm_get_robot_info", return_code).error_message);
}
rm_control::common::RobotInfo output;
output.arm_dof = static_cast<int>(info.arm_dof);
output.arm_model = static_cast<int>(info.arm_model);
output.force_type = static_cast<int>(info.force_type);
output.controller_version = static_cast<int>(info.robot_controller_version);
output.is_rm75 = info.arm_model == RM_MODEL_RM_75_E;
output.six_axis_force_sensor_available =
info.force_type == RM_MODEL_RM_SF_E || info.force_type == RM_MODEL_RM_ISF_E;
return rm_control::common::Result<rm_control::common::RobotInfo>::success(output);
}
rm_control::common::Result<rm_control::common::RobotSoftwareInfo>
RealmanArmAdapter::getRobotSoftwareInfo() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<rm_control::common::RobotSoftwareInfo>::failure(
connected.error_message);
}
rm_arm_software_version_t info{};
const int return_code = rm_get_arm_software_info(implementation_->handle, &info);
if (return_code != 0) {
return rm_control::common::Result<rm_control::common::RobotSoftwareInfo>::failure(
sdkResult("rm_get_arm_software_info", return_code).error_message);
}
rm_control::common::RobotSoftwareInfo output;
const char *api_version = rm_api_version();
output.sdk_version = api_version == nullptr ? "unknown" : api_version;
output.product_version = boundedString(info.product_version);
output.controller_generation = boundedString(info.robot_controller_version);
output.firmware_version = boundedString(info.ctrl_info.version);
output.firmware_build_time = boundedString(info.ctrl_info.build_time);
output.plan_version = boundedString(info.plan_info.version);
output.plan_build_time = boundedString(info.plan_info.build_time);
output.algorithm_version = boundedString(info.algorithm_info.version);
output.dynamics_version = boundedString(info.dynamic_info.model_version);
output.communication_version = boundedString(info.com_info.version);
output.communication_build_time = boundedString(info.com_info.build_time);
output.program_version = boundedString(info.program_info.version);
output.program_build_time = boundedString(info.program_info.build_time);
return rm_control::common::Result<rm_control::common::RobotSoftwareInfo>::success(output);
}
rm_control::common::Result<rm_control::common::ArmState>
RealmanArmAdapter::getCurrentArmState() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<rm_control::common::ArmState>::failure(
connected.error_message);
}
rm_current_arm_state_t state{};
const int return_code = rm_get_current_arm_state(implementation_->handle, &state);
if (return_code != 0) {
return rm_control::common::Result<rm_control::common::ArmState>::failure(
sdkResult("rm_get_current_arm_state", return_code).error_message);
}
rm_control::common::ArmState output;
output.joints_deg.assign(state.joint,
state.joint + rm_control::common::kRm75JointCount);
output.tcp_pose = convertPose(state.pose);
appendNonZeroErrors(state.err, &output.error_codes);
return rm_control::common::Result<rm_control::common::ArmState>::success(output);
}
rm_control::common::Result<rm_control::common::ForceData>
RealmanArmAdapter::getForceData() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<rm_control::common::ForceData>::failure(
connected.error_message);
}
rm_force_data_t data{};
const int return_code = rm_get_force_data(implementation_->handle, &data);
if (return_code != 0) {
return rm_control::common::Result<rm_control::common::ForceData>::failure(
sdkResult("rm_get_force_data", return_code).error_message);
}
rm_control::common::ForceData output;
for (std::size_t index = 0; index < output.raw.size(); ++index) {
output.raw[index] = data.force_data[index];
output.zero[index] = data.zero_force_data[index];
output.work_zero[index] = data.work_zero_force_data[index];
output.tool_zero[index] = data.tool_zero_force_data[index];
}
return rm_control::common::Result<rm_control::common::ForceData>::success(output);
}
rm_control::common::Result<rm_control::common::RobotSafetyStatus>
RealmanArmAdapter::getRobotSafetyStatus() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::failure(
connected.error_message);
}
rm_control::common::RobotSafetyStatus status;
const auto mode_and_power = readRunModeAndPower(implementation_->handle, &status);
if (!mode_and_power.ok) {
return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::failure(
mode_and_power.error_message);
}
const auto arm_state = readJointAndArmSafety(implementation_->handle, &status);
if (!arm_state.ok) {
return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::failure(
arm_state.error_message);
}
const auto controller = readControllerSafety(implementation_->handle, &status);
if (!controller.ok) {
return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::failure(
controller.error_message);
}
const auto configured_features = readConfiguredSafetyFeatures(implementation_->handle, &status);
if (!configured_features.ok) {
return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::failure(
configured_features.error_message);
}
return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::success(status);
}
rm_control::common::Result<rm_control::common::RobotJointLimits>
RealmanArmAdapter::getJointLimits() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<rm_control::common::RobotJointLimits>::failure(
connected.error_message);
}
rm_control::common::RobotJointLimits limits;
const int minimum_return_code = rm_get_joint_min_pos(
implementation_->handle, limits.minimum_position_deg.data());
if (minimum_return_code != 0) {
return rm_control::common::Result<rm_control::common::RobotJointLimits>::failure(
sdkResult("rm_get_joint_min_pos", minimum_return_code).error_message);
}
const int maximum_return_code = rm_get_joint_max_pos(
implementation_->handle, limits.maximum_position_deg.data());
if (maximum_return_code != 0) {
return rm_control::common::Result<rm_control::common::RobotJointLimits>::failure(
sdkResult("rm_get_joint_max_pos", maximum_return_code).error_message);
}
const int speed_return_code = rm_get_joint_max_speed(
implementation_->handle, limits.maximum_speed_deg_per_second.data());
if (speed_return_code != 0) {
return rm_control::common::Result<rm_control::common::RobotJointLimits>::failure(
sdkResult("rm_get_joint_max_speed", speed_return_code).error_message);
}
const int acceleration_return_code = rm_get_joint_max_acc(
implementation_->handle,
limits.maximum_acceleration_deg_per_second_squared.data());
if (acceleration_return_code != 0) {
return rm_control::common::Result<rm_control::common::RobotJointLimits>::failure(
sdkResult("rm_get_joint_max_acc", acceleration_return_code).error_message);
}
return rm_control::common::Result<rm_control::common::RobotJointLimits>::success(limits);
}
rm_control::common::Result<void> RealmanArmAdapter::moveJ(
const std::vector<float> &joints_deg,
int speed_percent,
int blend_radius,
int trajectory_connect,
bool blocking) {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
if (joints_deg.size() != rm_control::common::kRm75JointCount ||
implementation_->dof != static_cast<int>(rm_control::common::kRm75JointCount)) {
return rm_control::common::Result<void>::failure(
"RealmanArmAdapter::moveJ requires exactly seven RM75 joint angles");
}
std::array<float, rm_control::common::kRm75JointCount> target{};
std::copy(joints_deg.begin(), joints_deg.end(), target.begin());
const int return_code = rm_movej(implementation_->handle,
target.data(),
speed_percent,
blend_radius,
trajectory_connect,
blocking ? 1 : 0);
return sdkResult("rm_movej", return_code);
}
rm_control::common::Result<void> RealmanArmAdapter::moveL(
const rm_control::common::Pose &pose,
int speed_percent,
int blend_radius,
int trajectory_connect,
bool blocking) {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
const rm_pose_t target = convertPose(pose);
const int return_code = rm_movel(implementation_->handle,
target,
speed_percent,
blend_radius,
trajectory_connect,
blocking ? 1 : 0);
return sdkResult("rm_movel", return_code);
}
rm_control::common::Result<void> RealmanArmAdapter::startDragTeach(
const rm_control::common::DragTeachStartOptions &options) {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
if (options.mode == rm_control::common::DragTeachMode::OrdinaryRecorded) {
const int record_flag = options.record_vendor_trajectory
? kOrdinaryDragTrajectoryRecordEnabled
: 0;
return sdkResult("rm_start_drag_teach",
rm_start_drag_teach(implementation_->handle, record_flag));
}
const int singular_wall = options.enable_singular_wall ? 1 : 0;
return sdkResult("rm_start_multi_drag_teach(mode=3)",
rm_start_multi_drag_teach(implementation_->handle,
kSixDofPositionAndOrientationDragMode,
singular_wall));
}
rm_control::common::Result<void> RealmanArmAdapter::stopDragTeach() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
return sdkResult("rm_stop_drag_teach", rm_stop_drag_teach(implementation_->handle));
}
rm_control::common::VendorTrajectorySaveAttempt
RealmanArmAdapter::saveDragTeachTrajectory(const std::filesystem::path &path) {
rm_control::common::VendorTrajectorySaveAttempt attempt;
attempt.path = path;
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
attempt.error_message = connected.error_message;
return attempt;
}
std::string native_path;
try {
native_path = path.string();
} catch (const std::exception &exception) {
attempt.error_message =
"rm_save_trajectory path conversion failed: " + std::string(exception.what());
return attempt;
}
std::error_code filesystem_error;
if (std::filesystem::exists(path, filesystem_error)) {
attempt.error_message =
"refusing to overwrite existing RM75 raw trajectory: " + native_path;
return attempt;
}
if (filesystem_error) {
attempt.error_message = "failed to inspect RM75 raw trajectory target " +
native_path + ": " + filesystem_error.message();
return attempt;
}
if (path.parent_path().empty() ||
!std::filesystem::is_directory(path.parent_path(), filesystem_error) ||
filesystem_error) {
attempt.error_message =
"RM75 raw trajectory parent must be an existing directory for " + native_path;
return attempt;
}
if (native_path.empty() || native_path.size() > kMaximumVendorTrajectoryPathLength) {
attempt.error_message =
"rm_save_trajectory path must contain 1..299 bytes, actual=" +
std::to_string(native_path.size());
return attempt;
}
attempt.api_called = true;
attempt.return_code = rm_save_trajectory(implementation_->handle,
native_path.c_str(),
&attempt.point_count);
if (attempt.return_code != 0) {
attempt.error_message = "rm_save_trajectory failed, return_code=" +
std::to_string(attempt.return_code);
}
return attempt;
}
rm_control::common::Result<void>
RealmanArmAdapter::moveToDragTrajectoryOrigin(bool blocking) {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
return sdkResult("rm_drag_trajectory_origin",
rm_drag_trajectory_origin(implementation_->handle, blocking ? 1 : 0));
}
rm_control::common::Result<void>
RealmanArmAdapter::replayLastDragTrajectory(bool blocking) {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
return sdkResult("rm_run_drag_trajectory",
rm_run_drag_trajectory(implementation_->handle, blocking ? 1 : 0));
}
rm_control::common::Result<void> RealmanArmAdapter::pauseDragTrajectoryReplay() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
return sdkResult("rm_pause_drag_trajectory",
rm_pause_drag_trajectory(implementation_->handle));
}
rm_control::common::Result<void> RealmanArmAdapter::continueDragTrajectoryReplay() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
return sdkResult("rm_continue_drag_trajectory",
rm_continue_drag_trajectory(implementation_->handle));
}
rm_control::common::Result<void> RealmanArmAdapter::stopDragTrajectoryReplay() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
return sdkResult("rm_stop_drag_trajectory",
rm_stop_drag_trajectory(implementation_->handle));
}
rm_control::common::Result<void> RealmanArmAdapter::saveVendorDragTrajectoryProgram(
const rm_control::common::VendorDragTrajectoryExecutionRequest &request) {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
const auto validated_path = validateVendorDragTrajectoryRequest(request);
if (!validated_path.ok) {
return rm_control::common::Result<void>::failure(validated_path.error_message);
}
const auto available = requireControllerProgramSlotAvailable(
implementation_->handle, request.controller_program_id);
if (!available.ok) {
return available;
}
const auto saved = sendVendorDragTrajectoryProject(
implementation_->handle, request, validated_path.value, kOnlySaveControllerProject);
if (!saved.ok) {
return saved;
}
return requireControllerProgramSlotOccupied(
implementation_->handle, request.controller_program_id);
}
rm_control::common::Result<void> RealmanArmAdapter::runControllerProgram(
const rm_control::common::ControllerProgramRunRequest &request) {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
const auto valid_request = validateControllerProgramRunRequest(request);
if (!valid_request.ok) {
return valid_request;
}
const int blocking_mode = request.blocking ? RM_MOVE_MULTI_BLOCK : RM_MOVE_NBLOCK;
return sdkResult(
"rm_set_program_id_run",
rm_set_program_id_run(implementation_->handle,
request.controller_program_id,
request.speed_percent,
blocking_mode));
}
rm_control::common::Result<void> RealmanArmAdapter::executeVendorDragTrajectoryFile(
const rm_control::common::VendorDragTrajectoryExecutionRequest &request) {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
const auto validated_path = validateVendorDragTrajectoryRequest(request);
if (!validated_path.ok) {
return rm_control::common::Result<void>::failure(validated_path.error_message);
}
const auto unused_program_slot = requireControllerProgramSlotAvailable(
implementation_->handle, request.controller_program_id);
if (!unused_program_slot.ok) {
return unused_program_slot;
}
return sendVendorDragTrajectoryProject(
implementation_->handle, request, validated_path.value, kSaveAndRunControllerProject);
}
rm_control::common::Result<rm_control::common::ControllerProgramRunState>
RealmanArmAdapter::getControllerProgramRunState() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<rm_control::common::ControllerProgramRunState>::failure(
connected.error_message);
}
rm_program_run_state_t state{};
const int return_code = rm_get_program_run_state(implementation_->handle, &state);
if (return_code != 0) {
return rm_control::common::Result<rm_control::common::ControllerProgramRunState>::failure(
sdkResult("rm_get_program_run_state", return_code).error_message);
}
rm_control::common::ControllerProgramRunState output;
output.raw_state = state.run_state;
output.program_id = state.id;
output.plan_number = state.plan_num;
output.plan_speed_percent = state.plan_speed;
return rm_control::common::Result<rm_control::common::ControllerProgramRunState>::success(
output);
}
rm_control::common::Result<void> RealmanArmAdapter::slowStopMotion() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
return sdkResult("rm_set_arm_slow_stop",
rm_set_arm_slow_stop(implementation_->handle));
}
rm_control::common::Result<bool>
RealmanArmAdapter::isControllerProgramSlotAvailable(int controller_program_id) {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<bool>::failure(connected.error_message);
}
if (controller_program_id < kMinimumControllerProgramId ||
controller_program_id > kMaximumControllerProgramId) {
return rm_control::common::Result<bool>::failure(
"controller program id must be in the range 1..100");
}
return queryControllerProgramSlotAvailable(
implementation_->handle, controller_program_id);
}
rm_control::common::Result<rm_control::interfaces::ToolModbusBusStatus>
RealmanArmAdapter::Configure(
const rm_control::interfaces::ToolModbusBusConfiguration &configuration) {
const auto connected = ensureConnected(
implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::failure(
connected.error_message);
}
const auto valid = ValidateToolModbusConfiguration(configuration);
const auto generation = VerifyThirdGenerationController(implementation_->handle);
const auto current_status = GetStatus();
if (!valid.ok || !generation.ok || !current_status.ok) {
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::failure(
!valid.ok ? valid.error_message
: !generation.ok ? generation.error_message
: current_status.error_message);
}
if (configuration.power_cycle_tool) {
const auto powered_off = SetToolVoltageAndVerify(implementation_->handle, 0);
if (!powered_off.ok) {
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::failure(
powered_off.error_message);
}
std::this_thread::sleep_for(
std::chrono::milliseconds(configuration.power_off_delay_ms));
}
const auto reset = ResetToolModbusModeIfNeeded(
implementation_->handle, configuration.port, current_status.value.mode,
configuration.reset_existing_mode);
const auto modbus = reset.ok ? sdkResult(
"rm_set_modbus_mode",
rm_set_modbus_mode(implementation_->handle, configuration.port,
configuration.baudrate,
configuration.timeout_units)) : reset;
if (!modbus.ok) {
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::failure(
modbus.error_message);
}
std::this_thread::sleep_for(kToolModbusModeSettlingDelay);
const auto powered_on = SetToolVoltageAndVerify(
implementation_->handle, configuration.voltage_type);
if (!powered_on.ok) {
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::failure(
powered_on.error_message);
}
std::this_thread::sleep_for(
std::chrono::milliseconds(configuration.startup_delay_ms));
const auto actual_status = GetStatus();
if (!actual_status.ok) {
return actual_status;
}
const bool matches = actual_status.value.port == configuration.port &&
actual_status.value.mode == kToolModbusMasterMode &&
actual_status.value.baudrate == configuration.baudrate &&
actual_status.value.voltage_type == configuration.voltage_type &&
actual_status.value.timeout_units == configuration.timeout_units;
if (!matches) {
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::failure(
"tool Modbus status readback does not match the requested configuration");
}
return actual_status;
}
rm_control::common::Result<rm_control::interfaces::ToolModbusBusStatus>
RealmanArmAdapter::GetStatus() {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::failure(connected.error_message);
}
rm_control::interfaces::ToolModbusBusStatus status;
status.connected = true;
status.port = kToolRs485Port;
const int voltage_return_code =
rm_get_tool_voltage(implementation_->handle, &status.voltage_type);
if (voltage_return_code != 0) {
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::failure(
sdkResult("rm_get_tool_voltage", voltage_return_code).error_message);
}
const int rs485_return_code = rm_get_tool_RS485_mode(implementation_->handle,
&status.mode,
&status.baudrate,
&status.timeout_units);
if (rs485_return_code != 0) {
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::failure(
sdkResult("rm_get_tool_RS485_mode", rs485_return_code).error_message);
}
return rm_control::common::Result<
rm_control::interfaces::ToolModbusBusStatus>::success(status);
}
rm_control::common::Result<std::uint16_t>
RealmanArmAdapter::ReadInputRegister(std::uint8_t device_id,
std::uint16_t address) {
const auto connected = ensureConnected(
implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return rm_control::common::Result<std::uint16_t>::failure(
connected.error_message);
}
if (device_id == 0U || device_id > 247U) {
return rm_control::common::Result<std::uint16_t>::failure(
"RM_API2 single input-register read requires device 1..247");
}
rm_peripheral_read_write_params_t parameters{};
parameters.port = kToolRs485Port;
parameters.address = static_cast<int>(address);
parameters.device = static_cast<int>(device_id);
parameters.num = 1;
int raw_value = 0;
const int return_code = rm_read_input_registers(
implementation_->handle, parameters, &raw_value);
if (return_code != 0) {
return rm_control::common::Result<std::uint16_t>::failure(
"rm_read_input_registers failed, return_code=" +
std::to_string(return_code) + ", device=" +
std::to_string(static_cast<int>(device_id)) + ", address=" +
std::to_string(address));
}
const auto value = static_cast<std::uint16_t>(
static_cast<unsigned int>(raw_value) & 0xFFFFU);
return rm_control::common::Result<std::uint16_t>::success(value);
}
rm_control::common::Result<std::vector<std::uint16_t>>
RealmanArmAdapter::ReadInputRegisters(std::uint8_t device_id,
std::uint16_t start_address,
std::size_t register_count) {
return ReadMultipleRegistersFromRmApi(
implementation_->initialized, implementation_->handle,
MultipleRegisterReadRequest{device_id, start_address, register_count,
"rm_read_multiple_input_registers",
&rm_read_multiple_input_registers});
}
rm_control::common::Result<std::vector<std::uint16_t>>
RealmanArmAdapter::ReadHoldingRegisters(std::uint8_t device_id,
std::uint16_t start_address,
std::size_t register_count) {
return ReadMultipleRegistersFromRmApi(
implementation_->initialized, implementation_->handle,
MultipleRegisterReadRequest{device_id, start_address, register_count,
"rm_read_multiple_holding_registers",
&rm_read_multiple_holding_registers});
}
rm_control::common::Result<void> RealmanArmAdapter::WriteHoldingRegisters(
std::uint8_t device_id,
std::uint16_t start_address,
const std::vector<std::uint16_t> &values) {
const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
if (!connected.ok) {
return connected;
}
if (device_id == 0 || device_id > 247 || values.empty() ||
values.size() > kMaximumMultipleRegisterWriteCount) {
return rm_control::common::Result<void>::failure(
"RM_API2 holding-register write requires device 1..247 and count 1..10");
}
rm_peripheral_read_write_params_t parameters{};
parameters.port = kToolRs485Port;
parameters.address = static_cast<int>(start_address);
parameters.device = static_cast<int>(device_id);
parameters.num = static_cast<int>(values.size());
auto encoded = EncodeModbusRegistersAsRmApiHighLowBytes(values);
if (!encoded.ok) {
return rm_control::common::Result<void>::failure(
"failed to encode RM_API2 holding-register write: " +
encoded.error_message);
}
const int return_code = rm_write_registers(
implementation_->handle, parameters, encoded.value.data());
if (return_code != 0) {
return rm_control::common::Result<void>::failure(
"rm_write_registers failed, return_code=" + std::to_string(return_code) +
", device=" + std::to_string(static_cast<int>(device_id)) +
", address=" + std::to_string(start_address) +
", count=" + std::to_string(values.size()));
}
return rm_control::common::Result<void>::success();
}
} // namespace flight_robot::realman
tests/realman_hardware/README.md
View file @ 8275ee2e
...@@ -29,7 +29,7 @@ ...@@ -29,7 +29,7 @@
Debug： Debug：
```bash ```bash
cd /home/mashiro/project/linkerhand-cpp-sdk cd linkerhand-cpp-sdk
cmake --preset clion-debug cmake --preset clion-debug
cmake --build --preset realman-hw-tests-debug cmake --build --preset realman-hw-tests-debug
``` ```
...@@ -37,86 +37,24 @@ cmake --build --preset realman-hw-tests-debug ...@@ -37,86 +37,24 @@ cmake --build --preset realman-hw-tests-debug
Release： Release：
```bash ```bash
cd /home/mashiro/project/linkerhand-cpp-sdk cd linkerhand-cpp-sdk
cmake --preset clion-release cmake --preset clion-release
cmake --build --preset realman-hw-tests-release cmake --build --preset realman-hw-tests-release
``` ```
## 运行 ## 运行
只运行只读测试，不会移动机械臂： 这些既有硬件测试程序尚未全部具备项目规则要求的
`ROBOT_HARDWARE_TEST=1 + --live + 人工二次确认` 三重门控，因此默认不注册到 CTest，
```bash 也不再提供自动运行 preset。当前只允许编译目标，不应直接运行；完成三重门控改造后，
ctest --test-dir cmake-build-debug --output-on-failure --label-regex readonly 由操作者按审批后的硬件验收方案人工执行。
```
运行单个模块：
```bash
ctest --test-dir cmake-build-debug --output-on-failure -R realman_hw_test_force_sensor
```
运行运动跟踪测试：
```bash
ctest --test-dir cmake-build-debug --output-on-failure -R realman_hw_test_motion_tracking
```
运行手动力退让测试：
```bash
ctest --test-dir cmake-build-debug --output-on-failure -R realman_hw_test_force_retreat
```
如果 12 秒内没有检测到超过阈值的外力，CTest 会把该测试标记为 `Skipped`，不算失败。
持续保持外力退让状态：
```bash
./cmake-build-debug/realman_hw_test_force_retreat_hold
```
停止：
```bash
Ctrl+C
```
后台启动：
```bash
setsid ./cmake-build-debug/realman_hw_test_force_retreat_hold \
> reports/realman_hardware_tests/force_retreat_hold_runtime.log 2>&1 < /dev/null &
echo $! > reports/realman_hardware_tests/force_retreat_hold.pid
```
后台停止：
```bash
kill "$(cat reports/realman_hardware_tests/force_retreat_hold.pid)"
```
运行所有 RealMan 硬件测试：
```bash
ctest --test-dir cmake-build-debug --output-on-failure --label-regex realman
```
上面的“全部测试”不包含 `realman_hw_test_force_retreat`，因为它需要人工施加外力，属于手动测试。
也可以通过构建 preset 运行：
```bash
cmake --build --preset realman-hw-run-readonly-tests-debug
cmake --build --preset realman-hw-run-tests-debug
```
## CLion 使用 ## CLion 使用
1. 打开项目： 1. 打开项目：
```text ```text
/home/mashiro/project/linkerhand-cpp-sdk linkerhand-cpp-sdk
``` ```
2. 点击 `Reload CMake Project`。 2. 点击 `Reload CMake Project`。
......
tests/rm75_acceptance/README.md
View file @ 8275ee2e
...@@ -2,9 +2,7 @@ ...@@ -2,9 +2,7 @@
本目录根据下面的测试文档整理： 本目录根据下面的测试文档整理：
```text 原始验收文档位于项目外部，未把本机绝对路径写入仓库。
/home/mashiro/文档/xwechat_files/wxid_n3k7s34f0rdo22_4994/msg/file/2026-06/RM75-6F 机械臂测试 Agent 操作文档 v1.0.docx
```
目标是把验收测试拆成独立模块，像单元测试一样分别构建、分别运行、分别出报告。自动测试只覆盖不需要移动机械臂的项目；载荷、急停、重复定位、真实接触力控等项目保留为手动验收模块，避免无人确认工作空间时触发危险动作。 目标是把验收测试拆成独立模块，像单元测试一样分别构建、分别运行、分别出报告。自动测试只覆盖不需要移动机械臂的项目；载荷、急停、重复定位、真实接触力控等项目保留为手动验收模块，避免无人确认工作空间时触发危险动作。
...@@ -30,7 +28,7 @@ ...@@ -30,7 +28,7 @@
Debug： Debug：
```bash ```bash
cd /home/mashiro/project/linkerhand-cpp-sdk cd linkerhand-cpp-sdk
cmake --preset clion-debug cmake --preset clion-debug
cmake --build --preset rm75-acceptance-readonly-debug cmake --build --preset rm75-acceptance-readonly-debug
``` ```
...@@ -38,39 +36,24 @@ cmake --build --preset rm75-acceptance-readonly-debug ...@@ -38,39 +36,24 @@ cmake --build --preset rm75-acceptance-readonly-debug
Release： Release：
```bash ```bash
cd /home/mashiro/project/linkerhand-cpp-sdk cd linkerhand-cpp-sdk
cmake --preset clion-release cmake --preset clion-release
cmake --build --preset rm75-acceptance-readonly-release cmake --build --preset rm75-acceptance-readonly-release
``` ```
## 运行 ## 运行
运行 RM75 验收只读测试： 这些既有验收程序尚未全部具备项目规则要求的
`ROBOT_HARDWARE_TEST=1 + --live + 人工二次确认` 三重门控，因此默认不注册到 CTest，
```bash 也不提供自动运行 preset。当前只允许编译目标；实际硬件验收必须由操作者在门控改造完成后
cd /home/mashiro/project/linkerhand-cpp-sdk 按审批流程人工执行。
ctest --test-dir cmake-build-debug --output-on-failure --label-regex rm75
```
也可以通过 preset 一键构建并运行：
```bash
cmake --build --preset rm75-acceptance-run-readonly-debug
```
运行单个模块：
```bash
./cmake-build-debug/rm75_acceptance_preflight
./cmake-build-debug/rm75_acceptance_communication
```
## CLion 使用 ## CLion 使用
1. 用 CLion 打开： 1. 用 CLion 打开：
```text ```text
/home/mashiro/project/linkerhand-cpp-sdk linkerhand-cpp-sdk
``` ```
2. 选择 CMake Profile： 2. 选择 CMake Profile：
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment