refactor: remove PLC motor control backend
This commit is contained in:
parent
b4ec07c851
commit
60098ee01e
@ -145,10 +145,7 @@ sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
|
|||||||
|
|
||||||
- [MotorService gRPC 接口](cmvr-es/service/README.md#motorservice)
|
- [MotorService gRPC 接口](cmvr-es/service/README.md#motorservice)
|
||||||
- [电机设备模块](cmvr-es/devices/motor/README.md)
|
- [电机设备模块](cmvr-es/devices/motor/README.md)
|
||||||
- [Modbus TCP PLC runtime](cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md)
|
|
||||||
- [MotorService 与 CMVR PLC v1 完整协议](docs/motor_service_modbus_tcp.md)
|
|
||||||
- [AUBO 控制柜 Standard 数字 IO](cmvr-es/devices/arm/aubo_arm/README.md)
|
- [AUBO 控制柜 Standard 数字 IO](cmvr-es/devices/arm/aubo_arm/README.md)
|
||||||
- [配置与部署规则](cmvr-es/config/README.md)
|
- [配置与部署规则](cmvr-es/config/README.md)
|
||||||
|
|
||||||
Modbus Quick Stop 只是功能性停止，不能替代硬接线急停或驱动器 STO。AUBO
|
AUBO JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。
|
||||||
JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。
|
|
||||||
|
|||||||
@ -1,56 +0,0 @@
|
|||||||
motor {
|
|
||||||
id: "plc_motors"
|
|
||||||
|
|
||||||
motor_groups {
|
|
||||||
id: "plc_axis_group"
|
|
||||||
bus_type: MOTOR_BUS_MODBUS_TCP
|
|
||||||
vendor: MOTOR_VENDOR_PLC_GENERIC
|
|
||||||
protocol: MOTOR_PROTOCOL_CMVR_PLC_V1
|
|
||||||
|
|
||||||
modbus_tcp {
|
|
||||||
host: "192.168.0.10"
|
|
||||||
port: 502
|
|
||||||
unit_id: 1
|
|
||||||
|
|
||||||
connect_timeout_ms: 500
|
|
||||||
io_timeout_ms: 100
|
|
||||||
heartbeat_period_ms: 100
|
|
||||||
communication_watchdog_ms: 1000
|
|
||||||
cyclic_watchdog_ms: 500
|
|
||||||
status_poll_period_ms: 20
|
|
||||||
reconnect_min_ms: 100
|
|
||||||
reconnect_max_ms: 2000
|
|
||||||
command_ack_timeout_ms: 500
|
|
||||||
|
|
||||||
protocol_major: 1
|
|
||||||
protocol_minor: 0
|
|
||||||
|
|
||||||
axes { motor_id: 1 axis_index: 0 }
|
|
||||||
axes { motor_id: 2 axis_index: 1 }
|
|
||||||
}
|
|
||||||
|
|
||||||
joint_limits {
|
|
||||||
enable: true
|
|
||||||
source: JOINT_LIMIT_SOURCE_CUSTOM
|
|
||||||
joints {
|
|
||||||
joint_name: "PLC_AXIS_1"
|
|
||||||
q_lb: -3.141592653589793
|
|
||||||
q_ub: 3.141592653589793
|
|
||||||
qd: 1.0
|
|
||||||
qdd: 2.0
|
|
||||||
}
|
|
||||||
joints {
|
|
||||||
joint_name: "PLC_AXIS_2"
|
|
||||||
q_lb: -1.5707963267948966
|
|
||||||
q_ub: 1.5707963267948966
|
|
||||||
qd: 0.5
|
|
||||||
qdd: 1.0
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
motors {
|
|
||||||
motors { id: 1 joint_name: "PLC_AXIS_1" }
|
|
||||||
motors { id: 2 joint_name: "PLC_AXIS_2" }
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -82,14 +82,6 @@ device_manager {
|
|||||||
enable: false
|
enable: false
|
||||||
}
|
}
|
||||||
|
|
||||||
devices {
|
|
||||||
id: "plc_motors"
|
|
||||||
type: DEVICE_TYPE_MOTOR_SYSTEM
|
|
||||||
config_file: "devices/motor/plc_motors.pb.txt"
|
|
||||||
# Configure the PLC endpoint and complete the safety checkout before enabling.
|
|
||||||
enable: false
|
|
||||||
}
|
|
||||||
|
|
||||||
devices {
|
devices {
|
||||||
id: "right_arm"
|
id: "right_arm"
|
||||||
type: DEVICE_TYPE_ROBOT_ARM
|
type: DEVICE_TYPE_ROBOT_ARM
|
||||||
|
|||||||
@ -42,7 +42,7 @@ config/cmvr_es.pb.txt
|
|||||||
| Microphone | [`microphone/abstract_microphone.h`](microphone/abstract_microphone.h) | [`microphone/microphone_factory.h`](microphone/microphone_factory.h) | FFmpeg |
|
| Microphone | [`microphone/abstract_microphone.h`](microphone/abstract_microphone.h) | [`microphone/microphone_factory.h`](microphone/microphone_factory.h) | FFmpeg |
|
||||||
| Speaker | [`speaker/abstract_speaker.h`](speaker/abstract_speaker.h) | [`speaker/speaker_factory.h`](speaker/speaker_factory.h) | FFmpeg |
|
| Speaker | [`speaker/abstract_speaker.h`](speaker/abstract_speaker.h) | [`speaker/speaker_factory.h`](speaker/speaker_factory.h) | FFmpeg |
|
||||||
| BioHead | [`biohead/abstract_biohead.h`](biohead/abstract_biohead.h) | DeviceFactory 直接创建 | BioHeadRobot |
|
| BioHead | [`biohead/abstract_biohead.h`](biohead/abstract_biohead.h) | DeviceFactory 直接创建 | BioHeadRobot |
|
||||||
| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT、Modbus TCP PLC |
|
| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT |
|
||||||
|
|
||||||
代码目录存在不等于已经接入配置创建链：
|
代码目录存在不等于已经接入配置创建链：
|
||||||
|
|
||||||
|
|||||||
@ -17,52 +17,5 @@ add_subdirectory(drivers/ti5_canopen)
|
|||||||
add_subdirectory(drivers/mujoco)
|
add_subdirectory(drivers/mujoco)
|
||||||
add_subdirectory(bus_runtime)
|
add_subdirectory(bus_runtime)
|
||||||
add_subdirectory(drivers/ethercat_motor)
|
add_subdirectory(drivers/ethercat_motor)
|
||||||
add_subdirectory(drivers/modbus_plc_motor)
|
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
|
||||||
enable_testing()
|
|
||||||
set(CMVR_MOTOR_LIBMODBUS_ROOT
|
|
||||||
${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11)
|
|
||||||
add_executable(modbus_tcp_motor_bus_runtime_test
|
|
||||||
bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp
|
|
||||||
)
|
|
||||||
target_include_directories(modbus_tcp_motor_bus_runtime_test
|
|
||||||
PRIVATE
|
|
||||||
${CMAKE_SOURCE_DIR}/cmvr-es
|
|
||||||
${CMVR_MOTOR_LIBMODBUS_ROOT}/include
|
|
||||||
)
|
|
||||||
target_link_directories(modbus_tcp_motor_bus_runtime_test
|
|
||||||
PRIVATE
|
|
||||||
${CMVR_MOTOR_LIBMODBUS_ROOT}/lib
|
|
||||||
)
|
|
||||||
target_link_libraries(modbus_tcp_motor_bus_runtime_test
|
|
||||||
PRIVATE
|
|
||||||
cmvr_es::device::motor_bus_runtime
|
|
||||||
cmvr_es::device::modbus_plc_motor_driver
|
|
||||||
cmvr_es::proto
|
|
||||||
modbus
|
|
||||||
gtest
|
|
||||||
gtest_main
|
|
||||||
pthread
|
|
||||||
glog
|
|
||||||
)
|
|
||||||
target_compile_definitions(modbus_tcp_motor_bus_runtime_test
|
|
||||||
PRIVATE
|
|
||||||
CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH="${CMAKE_SOURCE_DIR}/cmvr-es/config/devices/motor/plc_motors.pb.txt"
|
|
||||||
)
|
|
||||||
add_test(NAME modbus_tcp_motor_bus_runtime_test
|
|
||||||
COMMAND modbus_tcp_motor_bus_runtime_test)
|
|
||||||
set(_modbus_motor_test_environment
|
|
||||||
"LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
|
|
||||||
)
|
|
||||||
if(CMVR_TEST_SYSTEM_LIBSTDCXX)
|
|
||||||
list(APPEND _modbus_motor_test_environment
|
|
||||||
"LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
|
|
||||||
endif()
|
|
||||||
set_tests_properties(modbus_tcp_motor_bus_runtime_test PROPERTIES
|
|
||||||
TIMEOUT 15
|
|
||||||
ENVIRONMENT "${_modbus_motor_test_environment}"
|
|
||||||
)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
add_subdirectory(manager)
|
add_subdirectory(manager)
|
||||||
|
|||||||
@ -2,7 +2,7 @@
|
|||||||
|
|
||||||
`devices/motor/` 提供电机管理、协议适配、总线 runtime 和厂商驱动。Service、
|
`devices/motor/` 提供电机管理、协议适配、总线 runtime 和厂商驱动。Service、
|
||||||
RobotArm 和业务 Task 只依赖 `MotorManager`/`AbstractMotor` 的稳定接口，不应
|
RobotArm 和业务 Task 只依赖 `MotorManager`/`AbstractMotor` 的稳定接口，不应
|
||||||
直接访问 libmodbus、CAN、EtherCAT 或厂商 SDK。
|
直接访问 CAN、EtherCAT、MuJoCo 或厂商 SDK。
|
||||||
|
|
||||||
返回 [Devices 模块指南](../README.md) 或 [项目总览](../../../README.md)。
|
返回 [Devices 模块指南](../README.md) 或 [项目总览](../../../README.md)。
|
||||||
|
|
||||||
@ -11,72 +11,37 @@ RobotArm 和业务 Task 只依赖 `MotorManager`/`AbstractMotor` 的稳定接口
|
|||||||
| 目录 | 职责 |
|
| 目录 | 职责 |
|
||||||
| --- | --- |
|
| --- | --- |
|
||||||
| `manager/` | 创建 MotorGroup，按 `motor_id`/`joint_name` 暴露 `AbstractMotor` |
|
| `manager/` | 创建 MotorGroup，按 `motor_id`/`joint_name` 暴露 `AbstractMotor` |
|
||||||
| `bus_runtime/` | 连接、收发、重连、watchdog 和总线生命周期 |
|
| `bus_runtime/` | 连接、收发和总线生命周期 |
|
||||||
| `drivers/` | CANopen、EtherCAT、MuJoCo、Modbus PLC 等具体后端 |
|
| `drivers/` | CANopen、EtherCAT 和 MuJoCo 等具体后端 |
|
||||||
| `drivers/modbus_plc_motor/` | 关节限位、SI 单位和 CMVR PLC v1 命令映射 |
|
|
||||||
|
|
||||||
PLC 电机链路为：
|
## 当前后端
|
||||||
|
|
||||||
```text
|
- CAN + TI5 CANopen；
|
||||||
gRPC MotorService
|
- EtherCAT + EYOU CiA 402；
|
||||||
|
|
- MuJoCo 仿真电机。
|
||||||
v
|
|
||||||
MotorManager -> AbstractMotor
|
|
||||||
|
|
|
||||||
v
|
|
||||||
CmvrPlcMotorProtocol
|
|
||||||
|
|
|
||||||
v
|
|
||||||
ModbusTcpMotorBusRuntime
|
|
||||||
|
|
|
||||||
v
|
|
||||||
libmodbus -> PLC -> 驱动器/电机
|
|
||||||
```
|
|
||||||
|
|
||||||
各层边界：
|
配置示例：
|
||||||
|
|
||||||
- `MotorService` 负责 API 校验、单电机控制权、deadline/cancellation 和
|
- [`ti5_motors.pb.txt`](../../config/devices/motor/ti5_motors.pb.txt)
|
||||||
fail-closed Quick Stop；
|
- [`ethercat_motors.pb.txt`](../../config/devices/motor/ethercat_motors.pb.txt)
|
||||||
- `MotorManager` 负责电机查找与统一抽象；
|
- [`mujoco_motors.pb.txt`](../../config/devices/motor/mujoco_motors.pb.txt)
|
||||||
- `CmvrPlcMotorProtocol` 负责关节限位、SI 单位和 CMVR PLC v1 命令映射；
|
|
||||||
- `ModbusTcpMotorBusRuntime` 负责 PLC session、mailbox、ACK、状态和重连；
|
|
||||||
- PLC/驱动器必须独立实现通信 watchdog、周期 watchdog 和硬件安全动作。
|
|
||||||
|
|
||||||
## Modbus TCP PLC
|
对外接口与控制权语义见 [MotorService 文档](../../service/README.md#motorservice)。
|
||||||
|
|
||||||
实现、配置、依赖和测试入口见：
|
|
||||||
|
|
||||||
- [Modbus TCP runtime README](bus_runtime/modbus_tcp/README.md)
|
|
||||||
- [完整 MotorService/CMVR PLC v1 协议](../../../docs/motor_service_modbus_tcp.md)
|
|
||||||
- [MotorService 文档](../../service/README.md#motorservice)
|
|
||||||
- [`plc_motors.pb.txt`](../../config/devices/motor/plc_motors.pb.txt)
|
|
||||||
|
|
||||||
当前 Modbus 后端只提供 x86-64 的 libmodbus 3.1.11。ARM 目录没有对应库，
|
|
||||||
不能把 x86 ELF 复制到 ARM 设备使用。
|
|
||||||
|
|
||||||
## 安全边界
|
## 安全边界
|
||||||
|
|
||||||
- MotorService 是单轴 API，不提供多轴同扫描周期的原子 commit；
|
- MotorService 是单轴 API，不提供多轴同扫描周期的原子 commit；
|
||||||
- PLC/Modbus 的 Profile 或 cyclic 能力不能直接当成毫秒级机械臂组伺服；
|
- 软件 `emergencyStop` 和 Quick Stop 不具备功能安全等级；
|
||||||
- 软件 `emergencyStop`、Quick Stop 和普通 PLC 输出都不是安全急停；
|
- 真实设备必须具有经风险评估确定的硬接线急停、安全继电器和驱动器安全链；
|
||||||
- 真实设备必须具有独立的硬接线急停、安全继电器或 F-CPU/F-I/O，以及驱动器
|
- 新硬件配置保持 `enable: false`，完成方向、限位和故障注入验证后才能启用。
|
||||||
STO 等经风险评估确定的安全链；
|
|
||||||
- 新硬件配置保持 `enable: false`，完成方向、限位、watchdog 和故障注入验证后
|
|
||||||
才能启用。
|
|
||||||
|
|
||||||
## 测试
|
## 测试
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
cmake --build build --target \
|
cmake --build build --target grpc_motor_service_test -j4
|
||||||
modbus_tcp_motor_bus_runtime_test \
|
|
||||||
grpc_motor_service_test \
|
|
||||||
grpc_motor_service_modbus_e2e_test \
|
|
||||||
-j4
|
|
||||||
|
|
||||||
ctest --test-dir build \
|
ctest --test-dir build \
|
||||||
-R '^(modbus_tcp_motor_bus_runtime_test|grpc_motor_service_test|grpc_motor_service_modbus_e2e_test)$' \
|
-R '^grpc_motor_service_test$' \
|
||||||
--output-on-failure
|
--output-on-failure
|
||||||
```
|
```
|
||||||
|
|
||||||
端到端 fake PLC 测试需要本地 TCP bind/listen 权限。软件测试不能代替真实
|
该测试使用 fake motor，不替代真实总线、驱动器或安全链验证。
|
||||||
S7-1215C、驱动器、STO 和断网故障台架。
|
|
||||||
|
|||||||
@ -2,29 +2,19 @@ add_library(motor_bus_runtime SHARED
|
|||||||
can/src/can_motor_bus_runtime.cpp
|
can/src/can_motor_bus_runtime.cpp
|
||||||
mujoco/src/mujoco_motor_bus_runtime.cpp
|
mujoco/src/mujoco_motor_bus_runtime.cpp
|
||||||
ethercat/src/ethercat_motor_bus_runtime.cpp
|
ethercat/src/ethercat_motor_bus_runtime.cpp
|
||||||
modbus_tcp/src/modbus_tcp_client.cpp
|
|
||||||
modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp
|
|
||||||
)
|
)
|
||||||
|
|
||||||
set(IGH_ETHERCAT_ROOT
|
set(IGH_ETHERCAT_ROOT
|
||||||
${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0
|
${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0
|
||||||
)
|
)
|
||||||
set(CMVR_LIBMODBUS_ROOT
|
|
||||||
${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11
|
|
||||||
)
|
|
||||||
|
|
||||||
target_include_directories(motor_bus_runtime
|
target_include_directories(motor_bus_runtime
|
||||||
PUBLIC
|
PUBLIC
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}
|
${CMAKE_CURRENT_SOURCE_DIR}
|
||||||
PRIVATE
|
PRIVATE
|
||||||
${IGH_ETHERCAT_ROOT}/include
|
${IGH_ETHERCAT_ROOT}/include
|
||||||
${CMVR_LIBMODBUS_ROOT}/include
|
|
||||||
)
|
)
|
||||||
|
|
||||||
target_link_directories(motor_bus_runtime PRIVATE
|
target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib)
|
||||||
${IGH_ETHERCAT_ROOT}/lib
|
|
||||||
${CMVR_LIBMODBUS_ROOT}/lib
|
|
||||||
)
|
|
||||||
|
|
||||||
target_link_libraries(motor_bus_runtime
|
target_link_libraries(motor_bus_runtime
|
||||||
PUBLIC
|
PUBLIC
|
||||||
@ -33,7 +23,6 @@ target_link_libraries(motor_bus_runtime
|
|||||||
cmvr_es::mujoco_world
|
cmvr_es::mujoco_world
|
||||||
PRIVATE
|
PRIVATE
|
||||||
ethercat
|
ethercat
|
||||||
modbus
|
|
||||||
cmvr_es::device::canbus
|
cmvr_es::device::canbus
|
||||||
glog
|
glog
|
||||||
)
|
)
|
||||||
|
|||||||
@ -1,93 +0,0 @@
|
|||||||
# Modbus TCP PLC Runtime
|
|
||||||
|
|
||||||
本目录实现 CMVR PLC v1 的 Modbus TCP 总线 runtime。它负责 PLC 连接、身份
|
|
||||||
握手、session、心跳、重连、命令 mailbox、ACK 轮询和状态快照，不负责 gRPC
|
|
||||||
请求解析，也不提供功能安全急停。
|
|
||||||
|
|
||||||
返回 [Motor 设备模块](../../README.md) 或 [Devices 模块指南](../../../README.md)。
|
|
||||||
|
|
||||||
## 代码与配置
|
|
||||||
|
|
||||||
- [`include/modbus_tcp_client.h`](include/modbus_tcp_client.h)：有界超时的
|
|
||||||
libmodbus client
|
|
||||||
- [`include/modbus_tcp_motor_bus_runtime.h`](include/modbus_tcp_motor_bus_runtime.h)：
|
|
||||||
连接 supervisor、命令和状态 runtime
|
|
||||||
- [`include/cmvr_plc_register_map.h`](include/cmvr_plc_register_map.h)：
|
|
||||||
CMVR PLC v1 寄存器常量
|
|
||||||
- [`src/modbus_tcp_client.cpp`](src/modbus_tcp_client.cpp)
|
|
||||||
- [`src/modbus_tcp_motor_bus_runtime.cpp`](src/modbus_tcp_motor_bus_runtime.cpp)
|
|
||||||
- [`tests/modbus_tcp_motor_bus_runtime_test.cpp`](tests/modbus_tcp_motor_bus_runtime_test.cpp)
|
|
||||||
- [`../../../../config/devices/motor/plc_motors.pb.txt`](../../../../config/devices/motor/plc_motors.pb.txt)
|
|
||||||
|
|
||||||
完整寄存器表、TIA Portal 数据块、gRPC 语义和台架步骤由
|
|
||||||
[MotorService 与 CMVR PLC v1 完整协议](../../../../../docs/motor_service_modbus_tcp.md)
|
|
||||||
统一维护。
|
|
||||||
|
|
||||||
## 连接与协议约束
|
|
||||||
|
|
||||||
- `host` 必须是 IPv4 字面量，避免 DNS 让建连或停止出现无界等待；
|
|
||||||
- PLC boot ID 必须非零且每次 PLC 重启变化；
|
|
||||||
- owner 决策和命令 ACK 必须回显当前 session，旧 session 的 mailbox 不得执行；
|
|
||||||
- runtime `start()` 启动连接 supervisor，PLC 可以在进程启动时离线；
|
|
||||||
- 离线期间状态不可用，运动命令必须在写 mailbox 前失败；
|
|
||||||
- 重连必须重新完成身份、版本、boot ID 和 session 握手，不得重放旧命令；
|
|
||||||
- 状态区按 odd/even seqlock 发布，CMVR 使用
|
|
||||||
sequence-before → 64-word block → sequence-after 三段读取；
|
|
||||||
- payload 必须先完整写入，commit sequence 最后写入，PLC 只原子消费新的
|
|
||||||
commit；
|
|
||||||
- Quick Stop、Disable、故障和通信 watchdog 的状态必须通过当前 session
|
|
||||||
的 ACK/状态确认，不能把本地写成功解释为驱动器已经安全停止。
|
|
||||||
|
|
||||||
## Cyclic stream
|
|
||||||
|
|
||||||
每次 `OpenCyclicPosition/Velocity` 都创建新的轴级 stream epoch。PLC 必须在
|
|
||||||
同一个原子状态事务中：
|
|
||||||
|
|
||||||
1. 清零 `last_applied_cyclic_sequence`；
|
|
||||||
2. 清理旧样本去重状态；
|
|
||||||
3. 重置 cyclic watchdog；
|
|
||||||
4. 设置正确的 CSP/CSV mode；
|
|
||||||
5. 发布 `StreamActive=1` 后再 ACK。
|
|
||||||
|
|
||||||
重开后的首个样本序列从 `1` 开始，必须重新应用。活动 cyclic 流跨
|
|
||||||
`connection_epoch` 后不会自动重开；旧流的当前和后续 setpoint 都被拒绝，
|
|
||||||
Quick Stop 后客户端必须建立新的 gRPC 流。断链前或断链期间 pending 的
|
|
||||||
setpoint 不得进入新 session。
|
|
||||||
|
|
||||||
## 依赖与平台
|
|
||||||
|
|
||||||
x86-64 的 libmodbus 3.1.11 位于：
|
|
||||||
|
|
||||||
```text
|
|
||||||
dependency/x86/third_party/modbus/3.1.11
|
|
||||||
```
|
|
||||||
|
|
||||||
`dependency/arm/third_party/` 当前没有对应 libmodbus，因此 ARM 构建不支持
|
|
||||||
该后端。支持 ARM 前必须为目标 ABI 单独编译并验证库，不能复用 x86 二进制。
|
|
||||||
|
|
||||||
## 安全边界
|
|
||||||
|
|
||||||
标准 S7-1215C DC/DC/DC 不是 failsafe PLC。Modbus Quick Stop、
|
|
||||||
MotorService `emergencyStop`、普通 OB/FB 和普通数字输出都只是功能性控制，
|
|
||||||
不能替代：
|
|
||||||
|
|
||||||
- 硬接线急停；
|
|
||||||
- 安全继电器或 F-CPU/F-I/O；
|
|
||||||
- 驱动器双通道 STO；
|
|
||||||
- 接触器、抱闸反馈与必要的 EDM。
|
|
||||||
|
|
||||||
PLC 侧通信 watchdog 和 cyclic watchdog 必须在没有 CMVR 进程参与时独立停止
|
|
||||||
危险运动。真实启用前必须在禁能或脱载轴上验证寄存器、方向、限位、断网、PLC
|
|
||||||
重启、交换机故障和 Quick Stop 失败。
|
|
||||||
|
|
||||||
## 测试
|
|
||||||
|
|
||||||
```bash
|
|
||||||
cmake --build build --target modbus_tcp_motor_bus_runtime_test -j4
|
|
||||||
ctest --test-dir build \
|
|
||||||
-R '^modbus_tcp_motor_bus_runtime_test$' \
|
|
||||||
--output-on-failure
|
|
||||||
```
|
|
||||||
|
|
||||||
fake PLC 测试需要本地 TCP bind/listen 权限。测试通过只证明软件协议和故障注入
|
|
||||||
路径，不代表真实 PLC、驱动器或硬件安全链已经验收。
|
|
||||||
@ -1,209 +0,0 @@
|
|||||||
#ifndef CMVR_ES_CMVR_PLC_REGISTER_MAP_H
|
|
||||||
#define CMVR_ES_CMVR_PLC_REGISTER_MAP_H
|
|
||||||
|
|
||||||
#include <array>
|
|
||||||
#include <cstddef>
|
|
||||||
#include <cstdint>
|
|
||||||
|
|
||||||
namespace cmvr::device::cmvr_plc {
|
|
||||||
|
|
||||||
constexpr std::uint16_t kMagicCm = 0x434d;
|
|
||||||
constexpr std::uint16_t kMagicVr = 0x5652;
|
|
||||||
constexpr std::uint16_t kProtocolMajor = 1;
|
|
||||||
constexpr std::uint16_t kProtocolMinor = 0;
|
|
||||||
constexpr double kPositionScale = 1000000.0;
|
|
||||||
constexpr double kVelocityScale = 1000000.0;
|
|
||||||
constexpr double kAccelerationScale = 1000000.0;
|
|
||||||
constexpr double kTorqueScale = 1000.0;
|
|
||||||
|
|
||||||
constexpr int kGlobalRegisterCount = 32;
|
|
||||||
constexpr int kMagicCmOffset = 0;
|
|
||||||
constexpr int kMagicVrOffset = 1;
|
|
||||||
constexpr int kProtocolMajorOffset = 2;
|
|
||||||
constexpr int kProtocolMinorOffset = 3;
|
|
||||||
constexpr int kAxisCountOffset = 4;
|
|
||||||
constexpr int kPlcGlobalStateOffset = 5;
|
|
||||||
constexpr int kPlcBootIdOffset = 6;
|
|
||||||
constexpr int kCmvrSessionIdOffset = 8;
|
|
||||||
constexpr int kCmvrHeartbeatOffset = 10;
|
|
||||||
constexpr int kPlcHeartbeatOffset = 12;
|
|
||||||
constexpr int kCommunicationWatchdogOffset = 14;
|
|
||||||
constexpr int kGlobalErrorOffset = 16;
|
|
||||||
constexpr int kOwnerStateOffset = 17;
|
|
||||||
constexpr int kOwnerSessionIdOffset = 18;
|
|
||||||
|
|
||||||
constexpr int kAxisFirstOffset = 100;
|
|
||||||
constexpr int kAxisRegisterStride = 128;
|
|
||||||
constexpr int kAxisControlRegisterCount = 64;
|
|
||||||
constexpr int kAxisStatusRelativeOffset = 64;
|
|
||||||
constexpr int kAxisStatusRegisterCount = 64;
|
|
||||||
|
|
||||||
constexpr int axisBase(const std::uint32_t axis_index)
|
|
||||||
{
|
|
||||||
return kAxisFirstOffset + static_cast<int>(axis_index) * kAxisRegisterStride;
|
|
||||||
}
|
|
||||||
|
|
||||||
enum class CommandCode : std::uint16_t {
|
|
||||||
Nop = 0,
|
|
||||||
SetZero = 1,
|
|
||||||
MoveToZero = 2,
|
|
||||||
ProfilePosition = 3,
|
|
||||||
ProfileVelocity = 4,
|
|
||||||
OpenCyclicPosition = 5,
|
|
||||||
CyclicPositionSample = 6,
|
|
||||||
OpenCyclicVelocity = 7,
|
|
||||||
CyclicVelocitySample = 8,
|
|
||||||
CloseCyclicStream = 9,
|
|
||||||
QuickStop = 10,
|
|
||||||
Enable = 11,
|
|
||||||
Disable = 12,
|
|
||||||
};
|
|
||||||
|
|
||||||
enum class CommandState : std::uint16_t {
|
|
||||||
Idle = 0,
|
|
||||||
Received = 1,
|
|
||||||
Validating = 2,
|
|
||||||
Accepted = 3,
|
|
||||||
Running = 4,
|
|
||||||
TargetReached = 5,
|
|
||||||
Completed = 6,
|
|
||||||
Rejected = 7,
|
|
||||||
Failed = 8,
|
|
||||||
TimedOut = 9,
|
|
||||||
QuickStopped = 10,
|
|
||||||
CommunicationLost = 11,
|
|
||||||
};
|
|
||||||
|
|
||||||
enum class ResultCode : std::uint16_t {
|
|
||||||
Ok = 0,
|
|
||||||
InvalidCommand = 1,
|
|
||||||
InvalidParameter = 2,
|
|
||||||
AxisNotReady = 3,
|
|
||||||
AxisBusy = 4,
|
|
||||||
NotEnabled = 5,
|
|
||||||
PositionLimit = 6,
|
|
||||||
VelocityLimit = 7,
|
|
||||||
AccelerationLimit = 8,
|
|
||||||
ZeroNotValid = 9,
|
|
||||||
DriveFault = 10,
|
|
||||||
CommandTimeout = 11,
|
|
||||||
SequenceError = 12,
|
|
||||||
SessionMismatch = 13,
|
|
||||||
CommunicationWatchdog = 14,
|
|
||||||
CyclicWatchdog = 15,
|
|
||||||
Unsupported = 16,
|
|
||||||
InternalError = 17,
|
|
||||||
};
|
|
||||||
|
|
||||||
enum StatusFlag : std::uint16_t {
|
|
||||||
Enabled = 1U << 0U,
|
|
||||||
Moving = 1U << 1U,
|
|
||||||
TargetReached = 1U << 2U,
|
|
||||||
Fault = 1U << 3U,
|
|
||||||
QuickStopActive = 1U << 4U,
|
|
||||||
CommunicationWatchdogExpired = 1U << 5U,
|
|
||||||
CyclicWatchdogExpired = 1U << 6U,
|
|
||||||
ZeroValid = 1U << 7U,
|
|
||||||
StreamActive = 1U << 8U,
|
|
||||||
CommandBusy = 1U << 9U,
|
|
||||||
};
|
|
||||||
|
|
||||||
constexpr std::size_t kCommandPayloadRegisterCount = 62;
|
|
||||||
constexpr int kCommitSequenceRelativeOffset = 62;
|
|
||||||
|
|
||||||
constexpr std::size_t kPayloadSequence = 0;
|
|
||||||
constexpr std::size_t kCommandCode = 2;
|
|
||||||
constexpr std::size_t kCommandFlags = 3;
|
|
||||||
constexpr std::size_t kTargetPosition = 4;
|
|
||||||
constexpr std::size_t kTargetVelocity = 6;
|
|
||||||
constexpr std::size_t kAcceleration = 8;
|
|
||||||
constexpr std::size_t kTargetTorque = 10;
|
|
||||||
constexpr std::size_t kPositionTolerance = 12;
|
|
||||||
constexpr std::size_t kVelocityTolerance = 14;
|
|
||||||
constexpr std::size_t kCommandTimeout = 16;
|
|
||||||
constexpr std::size_t kStreamWatchdog = 18;
|
|
||||||
constexpr std::size_t kCyclicSampleSequence = 20;
|
|
||||||
constexpr std::size_t kClientMonotonicTime = 22;
|
|
||||||
constexpr std::size_t kExpectedZeroEpoch = 24;
|
|
||||||
constexpr std::size_t kDisconnectAction = 26;
|
|
||||||
constexpr std::size_t kCommandSessionId = 27;
|
|
||||||
constexpr std::size_t kPayloadSequenceMirror = 60;
|
|
||||||
|
|
||||||
constexpr std::size_t kAckSequence = 0;
|
|
||||||
constexpr std::size_t kActiveSequence = 2;
|
|
||||||
constexpr std::size_t kCommandState = 4;
|
|
||||||
constexpr std::size_t kResultCode = 5;
|
|
||||||
constexpr std::size_t kAxisState = 6;
|
|
||||||
constexpr std::size_t kCurrentMode = 7;
|
|
||||||
constexpr std::size_t kActualPosition = 8;
|
|
||||||
constexpr std::size_t kActualVelocity = 10;
|
|
||||||
constexpr std::size_t kActualTorque = 12;
|
|
||||||
constexpr std::size_t kTargetPositionStatus = 14;
|
|
||||||
constexpr std::size_t kTargetVelocityStatus = 16;
|
|
||||||
constexpr std::size_t kStatusFlags = 18;
|
|
||||||
constexpr std::size_t kDriveStatusword = 19;
|
|
||||||
constexpr std::size_t kFaultCode = 20;
|
|
||||||
constexpr std::size_t kZeroEpoch = 22;
|
|
||||||
constexpr std::size_t kLastAppliedCyclicSequence = 24;
|
|
||||||
constexpr std::size_t kStateSequence = 26;
|
|
||||||
constexpr std::size_t kPlcMonotonicTime = 28;
|
|
||||||
constexpr std::size_t kHeartbeatAge = 30;
|
|
||||||
constexpr std::size_t kAckSessionId = 32;
|
|
||||||
constexpr std::size_t kStateSequenceMirror = 62;
|
|
||||||
|
|
||||||
enum class OwnerState : std::uint16_t {
|
|
||||||
None = 0,
|
|
||||||
Accepting = 1,
|
|
||||||
Accepted = 2,
|
|
||||||
Rejected = 3,
|
|
||||||
};
|
|
||||||
|
|
||||||
inline void encodeUint32(std::uint16_t* registers,
|
|
||||||
const std::size_t offset,
|
|
||||||
const std::uint32_t value)
|
|
||||||
{
|
|
||||||
registers[offset] = static_cast<std::uint16_t>(value >> 16U);
|
|
||||||
registers[offset + 1] = static_cast<std::uint16_t>(value & 0xffffU);
|
|
||||||
}
|
|
||||||
|
|
||||||
inline void encodeInt32(std::uint16_t* registers,
|
|
||||||
const std::size_t offset,
|
|
||||||
const std::int32_t value)
|
|
||||||
{
|
|
||||||
encodeUint32(registers, offset, static_cast<std::uint32_t>(value));
|
|
||||||
}
|
|
||||||
|
|
||||||
inline std::uint32_t decodeUint32(const std::uint16_t* registers,
|
|
||||||
const std::size_t offset)
|
|
||||||
{
|
|
||||||
return (static_cast<std::uint32_t>(registers[offset]) << 16U) |
|
|
||||||
static_cast<std::uint32_t>(registers[offset + 1]);
|
|
||||||
}
|
|
||||||
|
|
||||||
inline std::int32_t decodeInt32(const std::uint16_t* registers,
|
|
||||||
const std::size_t offset)
|
|
||||||
{
|
|
||||||
return static_cast<std::int32_t>(decodeUint32(registers, offset));
|
|
||||||
}
|
|
||||||
|
|
||||||
inline bool isTerminal(const CommandState state)
|
|
||||||
{
|
|
||||||
return state == CommandState::Completed ||
|
|
||||||
state == CommandState::Rejected ||
|
|
||||||
state == CommandState::Failed ||
|
|
||||||
state == CommandState::TimedOut ||
|
|
||||||
state == CommandState::QuickStopped ||
|
|
||||||
state == CommandState::CommunicationLost;
|
|
||||||
}
|
|
||||||
|
|
||||||
inline bool isFailure(const CommandState state)
|
|
||||||
{
|
|
||||||
return state == CommandState::Rejected ||
|
|
||||||
state == CommandState::Failed ||
|
|
||||||
state == CommandState::TimedOut ||
|
|
||||||
state == CommandState::CommunicationLost;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace cmvr::device::cmvr_plc
|
|
||||||
|
|
||||||
#endif // CMVR_ES_CMVR_PLC_REGISTER_MAP_H
|
|
||||||
@ -1,47 +0,0 @@
|
|||||||
#ifndef CMVR_ES_MODBUS_TCP_CLIENT_H
|
|
||||||
#define CMVR_ES_MODBUS_TCP_CLIENT_H
|
|
||||||
|
|
||||||
#include <cstdint>
|
|
||||||
#include <atomic>
|
|
||||||
#include <string>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
struct _modbus;
|
|
||||||
using modbus_t = struct _modbus;
|
|
||||||
|
|
||||||
namespace cmvr::device {
|
|
||||||
|
|
||||||
class ModbusTcpClient {
|
|
||||||
public:
|
|
||||||
ModbusTcpClient() = default;
|
|
||||||
~ModbusTcpClient();
|
|
||||||
|
|
||||||
ModbusTcpClient(const ModbusTcpClient&) = delete;
|
|
||||||
ModbusTcpClient& operator=(const ModbusTcpClient&) = delete;
|
|
||||||
|
|
||||||
bool open(const std::string& host,
|
|
||||||
std::uint16_t port,
|
|
||||||
std::uint8_t unit_id,
|
|
||||||
std::uint32_t connect_timeout_ms,
|
|
||||||
std::uint32_t response_timeout_ms);
|
|
||||||
void close();
|
|
||||||
void shutdown();
|
|
||||||
bool connected() const { return context_ != nullptr; }
|
|
||||||
|
|
||||||
bool readHoldingRegisters(int address, int count, std::vector<std::uint16_t>& values);
|
|
||||||
bool writeHoldingRegisters(int address, const std::uint16_t* values, int count);
|
|
||||||
bool writeHoldingRegisters(int address, const std::vector<std::uint16_t>& values);
|
|
||||||
|
|
||||||
const std::string& lastError() const { return last_error_; }
|
|
||||||
|
|
||||||
private:
|
|
||||||
void setLastErrnoError_(const char* operation);
|
|
||||||
|
|
||||||
modbus_t* context_{nullptr};
|
|
||||||
std::atomic<int> socket_fd_{-1};
|
|
||||||
std::string last_error_;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace cmvr::device
|
|
||||||
|
|
||||||
#endif // CMVR_ES_MODBUS_TCP_CLIENT_H
|
|
||||||
@ -1,173 +0,0 @@
|
|||||||
#ifndef CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H
|
|
||||||
#define CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H
|
|
||||||
|
|
||||||
#include <array>
|
|
||||||
#include <atomic>
|
|
||||||
#include <chrono>
|
|
||||||
#include <condition_variable>
|
|
||||||
#include <cstdint>
|
|
||||||
#include <memory>
|
|
||||||
#include <mutex>
|
|
||||||
#include <optional>
|
|
||||||
#include <string>
|
|
||||||
#include <thread>
|
|
||||||
#include <unordered_map>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h"
|
|
||||||
#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h"
|
|
||||||
#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h"
|
|
||||||
|
|
||||||
namespace cmvr::device {
|
|
||||||
|
|
||||||
struct ModbusTcpMotorBusRuntimeTestAccess;
|
|
||||||
|
|
||||||
struct CmvrPlcAxisCommand {
|
|
||||||
cmvr_plc::CommandCode code{cmvr_plc::CommandCode::Nop};
|
|
||||||
std::uint16_t flags{0};
|
|
||||||
std::int32_t target_position{0};
|
|
||||||
std::int32_t target_velocity{0};
|
|
||||||
std::int32_t acceleration{0};
|
|
||||||
std::int32_t target_torque{0};
|
|
||||||
std::int32_t position_tolerance{0};
|
|
||||||
std::int32_t velocity_tolerance{0};
|
|
||||||
std::uint32_t command_timeout_ms{0};
|
|
||||||
std::uint32_t stream_watchdog_ms{0};
|
|
||||||
std::uint32_t cyclic_sample_sequence{0};
|
|
||||||
std::uint32_t client_monotonic_time_ms{0};
|
|
||||||
std::uint32_t expected_zero_epoch{0};
|
|
||||||
std::uint16_t disconnect_action{0};
|
|
||||||
};
|
|
||||||
|
|
||||||
struct CmvrPlcAxisStatus {
|
|
||||||
std::uint32_t ack_sequence{0};
|
|
||||||
std::uint32_t active_sequence{0};
|
|
||||||
cmvr_plc::CommandState command_state{cmvr_plc::CommandState::Idle};
|
|
||||||
cmvr_plc::ResultCode result_code{cmvr_plc::ResultCode::Ok};
|
|
||||||
std::uint16_t axis_state{0};
|
|
||||||
std::uint16_t current_mode{0};
|
|
||||||
std::int32_t actual_position{0};
|
|
||||||
std::int32_t actual_velocity{0};
|
|
||||||
std::int32_t actual_torque{0};
|
|
||||||
std::int32_t target_position{0};
|
|
||||||
std::int32_t target_velocity{0};
|
|
||||||
std::uint16_t status_flags{0};
|
|
||||||
std::uint16_t drive_statusword{0};
|
|
||||||
std::uint32_t fault_code{0};
|
|
||||||
std::uint32_t zero_epoch{0};
|
|
||||||
std::uint32_t last_applied_cyclic_sequence{0};
|
|
||||||
std::uint32_t state_sequence{0};
|
|
||||||
std::uint32_t plc_monotonic_time_ms{0};
|
|
||||||
std::uint32_t heartbeat_age_ms{0};
|
|
||||||
std::uint32_t ack_session_id{0};
|
|
||||||
};
|
|
||||||
|
|
||||||
class ModbusTcpMotorBusRuntime final : public AbstractMotorBusRuntime {
|
|
||||||
public:
|
|
||||||
ModbusTcpMotorBusRuntime();
|
|
||||||
~ModbusTcpMotorBusRuntime() override;
|
|
||||||
|
|
||||||
bool init(const config::MotorGroupConfig& group_cfg) override;
|
|
||||||
bool start() override;
|
|
||||||
void stop() override;
|
|
||||||
config::MotorBusType busType() const override { return config::MOTOR_BUS_MODBUS_TCP; }
|
|
||||||
|
|
||||||
bool hasMotor(std::uint8_t motor_id) const;
|
|
||||||
bool axisForMotor(std::uint8_t motor_id, std::uint32_t& axis_index) const;
|
|
||||||
bool connected() const { return connected_.load(); }
|
|
||||||
std::uint32_t sessionId() const { return session_id_.load(); }
|
|
||||||
std::uint32_t plcBootId() const { return plc_boot_id_.load(); }
|
|
||||||
std::uint64_t connectionEpoch() const { return connection_epoch_.load(); }
|
|
||||||
std::uint32_t streamWatchdogMs() const { return stream_watchdog_ms_; }
|
|
||||||
std::uint32_t commandAckTimeoutMs() const { return command_ack_timeout_ms_; }
|
|
||||||
const std::string& id() const { return id_; }
|
|
||||||
std::string lastError() const;
|
|
||||||
|
|
||||||
bool readAxisStatus(std::uint8_t motor_id, CmvrPlcAxisStatus& status);
|
|
||||||
bool submitAxisCommand(std::uint8_t motor_id,
|
|
||||||
const CmvrPlcAxisCommand& command,
|
|
||||||
bool wait_for_terminal_state = false,
|
|
||||||
std::optional<std::uint64_t>
|
|
||||||
expected_connection_epoch = std::nullopt);
|
|
||||||
bool submitAxisSafetyCommand(std::uint8_t motor_id,
|
|
||||||
const CmvrPlcAxisCommand& command);
|
|
||||||
|
|
||||||
private:
|
|
||||||
friend struct ModbusTcpMotorBusRuntimeTestAccess;
|
|
||||||
|
|
||||||
bool connectAndHandshakeLocked_();
|
|
||||||
void markDisconnectedLocked_(const std::string& error);
|
|
||||||
bool readRegistersLocked_(int address, int count, std::vector<std::uint16_t>& values);
|
|
||||||
bool writeRegistersLocked_(int address, const std::uint16_t* values, int count);
|
|
||||||
bool writeHeartbeatLocked_();
|
|
||||||
bool readAxisStatusByIndex_(std::uint32_t axis_index, CmvrPlcAxisStatus& status);
|
|
||||||
bool waitForCommand_(std::uint32_t axis_index,
|
|
||||||
std::uint32_t command_sequence,
|
|
||||||
std::uint32_t command_session_id,
|
|
||||||
std::uint64_t connection_epoch,
|
|
||||||
std::uint64_t cancel_generation,
|
|
||||||
bool cancel_on_safety_preemption,
|
|
||||||
const CmvrPlcAxisCommand& command,
|
|
||||||
bool wait_for_terminal_state,
|
|
||||||
const CmvrPlcAxisStatus& initial_status);
|
|
||||||
bool submitAxisCommandImpl_(std::uint8_t motor_id,
|
|
||||||
const CmvrPlcAxisCommand& command,
|
|
||||||
bool wait_for_terminal_state,
|
|
||||||
bool safety_priority,
|
|
||||||
std::optional<std::uint64_t>
|
|
||||||
expected_connection_epoch);
|
|
||||||
bool writeClientHeartbeatLocked_();
|
|
||||||
std::shared_ptr<std::mutex> axisMutex_(std::uint8_t motor_id) const;
|
|
||||||
std::shared_ptr<std::mutex> safetyMutex_(std::uint8_t motor_id) const;
|
|
||||||
void workerLoop_();
|
|
||||||
static std::uint32_t randomNonZeroSessionId_();
|
|
||||||
static std::uint32_t monotonicMilliseconds_();
|
|
||||||
|
|
||||||
std::string id_;
|
|
||||||
config::ModbusTcpConfig config_;
|
|
||||||
std::unordered_map<std::uint8_t, std::uint32_t> motor_axes_;
|
|
||||||
mutable std::unordered_map<std::uint8_t, std::shared_ptr<std::mutex>> axis_mutexes_;
|
|
||||||
mutable std::unordered_map<std::uint8_t, std::shared_ptr<std::mutex>> safety_mutexes_;
|
|
||||||
std::unordered_map<std::uint8_t, std::uint32_t> command_sequences_;
|
|
||||||
std::unordered_map<std::uint8_t, std::uint64_t> cancel_generations_;
|
|
||||||
// Monotonic admission counter protected by state_mutex_. Besides being
|
|
||||||
// useful when diagnosing queueing, it gives concurrency tests an exact
|
|
||||||
// synchronization point after an invocation has captured its safety
|
|
||||||
// generation and connection epoch, but before it waits on a command
|
|
||||||
// serialization mutex.
|
|
||||||
std::uint64_t command_admission_count_{0};
|
|
||||||
|
|
||||||
mutable std::mutex io_mutex_;
|
|
||||||
mutable std::mutex state_mutex_;
|
|
||||||
mutable std::mutex lifecycle_mutex_;
|
|
||||||
ModbusTcpClient client_;
|
|
||||||
std::string last_error_;
|
|
||||||
std::thread worker_;
|
|
||||||
std::mutex worker_wait_mutex_;
|
|
||||||
std::condition_variable worker_wait_cv_;
|
|
||||||
std::atomic<bool> running_{false};
|
|
||||||
std::atomic<bool> connected_{false};
|
|
||||||
std::atomic<std::uint32_t> plc_boot_id_{0};
|
|
||||||
std::atomic<std::uint64_t> connection_epoch_{0};
|
|
||||||
std::atomic<std::uint32_t> session_id_{0};
|
|
||||||
std::uint32_t last_session_id_{0};
|
|
||||||
std::uint32_t heartbeat_counter_{0};
|
|
||||||
std::chrono::steady_clock::time_point last_client_heartbeat_write_at_{};
|
|
||||||
std::uint32_t plc_heartbeat_counter_{0};
|
|
||||||
std::chrono::steady_clock::time_point plc_heartbeat_changed_at_{};
|
|
||||||
std::uint32_t connect_timeout_ms_{500};
|
|
||||||
std::uint32_t io_timeout_ms_{100};
|
|
||||||
std::uint32_t heartbeat_period_ms_{100};
|
|
||||||
std::uint32_t communication_watchdog_ms_{500};
|
|
||||||
std::uint32_t status_poll_period_ms_{20};
|
|
||||||
std::uint32_t reconnect_min_ms_{100};
|
|
||||||
std::uint32_t reconnect_max_ms_{2000};
|
|
||||||
std::uint32_t command_ack_timeout_ms_{500};
|
|
||||||
std::uint32_t stream_watchdog_ms_{500};
|
|
||||||
std::uint16_t protocol_major_{cmvr_plc::kProtocolMajor};
|
|
||||||
std::uint16_t protocol_minor_{cmvr_plc::kProtocolMinor};
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace cmvr::device
|
|
||||||
|
|
||||||
#endif // CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H
|
|
||||||
@ -1,188 +0,0 @@
|
|||||||
#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h"
|
|
||||||
|
|
||||||
#include <cerrno>
|
|
||||||
#include <limits>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <netdb.h>
|
|
||||||
#include <poll.h>
|
|
||||||
#include <sys/socket.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
#include <modbus/modbus.h>
|
|
||||||
|
|
||||||
namespace cmvr::device {
|
|
||||||
|
|
||||||
ModbusTcpClient::~ModbusTcpClient()
|
|
||||||
{
|
|
||||||
close();
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ModbusTcpClient::open(const std::string& host,
|
|
||||||
const std::uint16_t port,
|
|
||||||
const std::uint8_t unit_id,
|
|
||||||
const std::uint32_t connect_timeout_ms,
|
|
||||||
const std::uint32_t response_timeout_ms)
|
|
||||||
{
|
|
||||||
close();
|
|
||||||
context_ = modbus_new_tcp(host.c_str(), static_cast<int>(port));
|
|
||||||
if (!context_) {
|
|
||||||
last_error_ = "modbus_new_tcp failed";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (modbus_set_slave(context_, static_cast<int>(unit_id)) == -1) {
|
|
||||||
setLastErrnoError_("modbus_set_slave");
|
|
||||||
close();
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
const auto timeout = response_timeout_ms == 0 ? 200U : response_timeout_ms;
|
|
||||||
if (modbus_set_response_timeout(context_, timeout / 1000U,
|
|
||||||
(timeout % 1000U) * 1000U) == -1 ||
|
|
||||||
modbus_set_byte_timeout(context_, timeout / 1000U,
|
|
||||||
(timeout % 1000U) * 1000U) == -1) {
|
|
||||||
setLastErrnoError_("modbus_set_timeout");
|
|
||||||
close();
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
addrinfo hints{};
|
|
||||||
hints.ai_family = AF_INET;
|
|
||||||
hints.ai_socktype = SOCK_STREAM;
|
|
||||||
hints.ai_flags = AI_NUMERICHOST;
|
|
||||||
addrinfo* addresses = nullptr;
|
|
||||||
const auto service = std::to_string(port);
|
|
||||||
const auto resolve_result =
|
|
||||||
getaddrinfo(host.c_str(), service.c_str(), &hints, &addresses);
|
|
||||||
if (resolve_result != 0) {
|
|
||||||
last_error_ = std::string("host must be a numeric IPv4 address: ") +
|
|
||||||
gai_strerror(resolve_result);
|
|
||||||
close();
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
int connected_fd = -1;
|
|
||||||
for (auto* address = addresses; address; address = address->ai_next) {
|
|
||||||
const int fd = ::socket(address->ai_family, address->ai_socktype, address->ai_protocol);
|
|
||||||
if (fd < 0) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
const int old_flags = fcntl(fd, F_GETFL, 0);
|
|
||||||
if (old_flags < 0 || fcntl(fd, F_SETFL, old_flags | O_NONBLOCK) < 0) {
|
|
||||||
::close(fd);
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
socket_fd_.store(fd);
|
|
||||||
const int rc = ::connect(fd, address->ai_addr, address->ai_addrlen);
|
|
||||||
if (rc == 0 || errno == EINPROGRESS) {
|
|
||||||
pollfd descriptor{fd, POLLOUT, 0};
|
|
||||||
const auto timeout = static_cast<int>(
|
|
||||||
connect_timeout_ms == 0 ? 1000U : connect_timeout_ms);
|
|
||||||
if (rc == 0 || poll(&descriptor, 1, timeout) > 0) {
|
|
||||||
int socket_error = 0;
|
|
||||||
socklen_t error_size = sizeof(socket_error);
|
|
||||||
if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &socket_error, &error_size) == 0 &&
|
|
||||||
socket_error == 0) {
|
|
||||||
if (fcntl(fd, F_SETFL, old_flags) == 0) {
|
|
||||||
connected_fd = fd;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
socket_fd_.store(-1);
|
|
||||||
::close(fd);
|
|
||||||
}
|
|
||||||
freeaddrinfo(addresses);
|
|
||||||
if (connected_fd < 0 || modbus_set_socket(context_, connected_fd) == -1) {
|
|
||||||
if (connected_fd >= 0) {
|
|
||||||
::close(connected_fd);
|
|
||||||
}
|
|
||||||
last_error_ = "Modbus TCP connect timed out or failed";
|
|
||||||
close();
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
socket_fd_.store(connected_fd);
|
|
||||||
last_error_.clear();
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void ModbusTcpClient::close()
|
|
||||||
{
|
|
||||||
socket_fd_.store(-1);
|
|
||||||
if (context_) {
|
|
||||||
modbus_close(context_);
|
|
||||||
modbus_free(context_);
|
|
||||||
context_ = nullptr;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void ModbusTcpClient::shutdown()
|
|
||||||
{
|
|
||||||
const auto fd = socket_fd_.load();
|
|
||||||
if (fd >= 0) {
|
|
||||||
::shutdown(fd, SHUT_RDWR);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ModbusTcpClient::readHoldingRegisters(
|
|
||||||
const int address,
|
|
||||||
const int count,
|
|
||||||
std::vector<std::uint16_t>& values)
|
|
||||||
{
|
|
||||||
if (!context_) {
|
|
||||||
last_error_ = "Modbus TCP connection is closed";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (address < 0 || count <= 0 || count > MODBUS_MAX_READ_REGISTERS) {
|
|
||||||
last_error_ = "invalid holding-register read range";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
values.assign(static_cast<std::size_t>(count), 0);
|
|
||||||
const auto rc = modbus_read_registers(context_, address, count, values.data());
|
|
||||||
if (rc != count) {
|
|
||||||
setLastErrnoError_("modbus_read_registers");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
last_error_.clear();
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ModbusTcpClient::writeHoldingRegisters(
|
|
||||||
const int address,
|
|
||||||
const std::uint16_t* values,
|
|
||||||
const int count)
|
|
||||||
{
|
|
||||||
if (!context_) {
|
|
||||||
last_error_ = "Modbus TCP connection is closed";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (address < 0 || !values || count <= 0 || count > MODBUS_MAX_WRITE_REGISTERS) {
|
|
||||||
last_error_ = "invalid holding-register write range";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
const auto rc = modbus_write_registers(context_, address, count, values);
|
|
||||||
if (rc != count) {
|
|
||||||
setLastErrnoError_("modbus_write_registers");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
last_error_.clear();
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ModbusTcpClient::writeHoldingRegisters(
|
|
||||||
const int address,
|
|
||||||
const std::vector<std::uint16_t>& values)
|
|
||||||
{
|
|
||||||
if (values.size() > static_cast<std::size_t>(std::numeric_limits<int>::max())) {
|
|
||||||
last_error_ = "holding-register write is too large";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
return writeHoldingRegisters(address, values.data(), static_cast<int>(values.size()));
|
|
||||||
}
|
|
||||||
|
|
||||||
void ModbusTcpClient::setLastErrnoError_(const char* operation)
|
|
||||||
{
|
|
||||||
last_error_ = std::string(operation) + ": " + modbus_strerror(errno);
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace cmvr::device
|
|
||||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -1,21 +0,0 @@
|
|||||||
add_library(modbus_plc_motor_driver SHARED
|
|
||||||
src/cmvr_plc_motor_protocol.cpp
|
|
||||||
src/modbus_plc_motor.cpp
|
|
||||||
)
|
|
||||||
|
|
||||||
target_include_directories(modbus_plc_motor_driver
|
|
||||||
PUBLIC
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
|
||||||
)
|
|
||||||
|
|
||||||
target_link_libraries(modbus_plc_motor_driver
|
|
||||||
PUBLIC
|
|
||||||
cmvr_es::device::motor_core
|
|
||||||
cmvr_es::device::motor_bus_runtime
|
|
||||||
PRIVATE
|
|
||||||
cmvr_es::proto
|
|
||||||
glog
|
|
||||||
)
|
|
||||||
|
|
||||||
add_library(cmvr_es::device::modbus_plc_motor_driver ALIAS modbus_plc_motor_driver)
|
|
||||||
install(TARGETS modbus_plc_motor_driver LIBRARY DESTINATION lib)
|
|
||||||
@ -1,89 +0,0 @@
|
|||||||
#ifndef CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H
|
|
||||||
#define CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H
|
|
||||||
|
|
||||||
#include <cstdint>
|
|
||||||
#include <memory>
|
|
||||||
#include <mutex>
|
|
||||||
#include <optional>
|
|
||||||
#include <unordered_map>
|
|
||||||
|
|
||||||
#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h"
|
|
||||||
#include "devices/motor/motor_protocol_interface.h"
|
|
||||||
|
|
||||||
namespace cmvr::device {
|
|
||||||
|
|
||||||
class CmvrPlcMotorProtocol final : public MotorProtocolInterface {
|
|
||||||
public:
|
|
||||||
explicit CmvrPlcMotorProtocol(std::shared_ptr<ModbusTcpMotorBusRuntime> bus_runtime);
|
|
||||||
~CmvrPlcMotorProtocol() override = default;
|
|
||||||
|
|
||||||
bool initNode(std::uint8_t node_id) override;
|
|
||||||
void setMode(std::uint8_t node_id, msgs::RunMode mode) override;
|
|
||||||
msgs::RunMode getMode(std::uint8_t node_id) override;
|
|
||||||
void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override;
|
|
||||||
void setLimitQd(std::uint8_t node_id, double qd) override;
|
|
||||||
void setLimitQ(std::uint8_t node_id, double ub, double lb) override;
|
|
||||||
bool calibrateZeroQ(std::uint8_t node_id) override;
|
|
||||||
bool reachedTargetQ(std::uint8_t node_id) override;
|
|
||||||
bool commandProfilePosition(std::uint8_t node_id,
|
|
||||||
double target_q,
|
|
||||||
double max_qd,
|
|
||||||
double max_qdd) override;
|
|
||||||
bool commandProfileVelocity(std::uint8_t node_id,
|
|
||||||
double target_qd,
|
|
||||||
double max_qdd) override;
|
|
||||||
bool commandCyclicPosition(std::uint8_t node_id,
|
|
||||||
double target_q,
|
|
||||||
double target_qd) override;
|
|
||||||
bool commandCyclicVelocity(std::uint8_t node_id, double target_qd) override;
|
|
||||||
bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override;
|
|
||||||
void setMotorConversion(std::uint8_t node_id,
|
|
||||||
double encoder_counts_per_rev,
|
|
||||||
double gear_ratio) override;
|
|
||||||
bool torqueOn(std::uint8_t node_id) override;
|
|
||||||
bool torqueOff(std::uint8_t node_id) override;
|
|
||||||
bool brakeRelease(std::uint8_t node_id) override;
|
|
||||||
bool quickStop(std::uint8_t node_id) override;
|
|
||||||
double getQ(std::uint8_t node_id) override;
|
|
||||||
double getQd(std::uint8_t node_id) override;
|
|
||||||
|
|
||||||
private:
|
|
||||||
struct NodeState {
|
|
||||||
msgs::RunMode requested_mode{msgs::RUN_MODE_UNSPECIFIED};
|
|
||||||
double limit_q_lb{0.0};
|
|
||||||
double limit_q_ub{0.0};
|
|
||||||
double limit_qd{0.0};
|
|
||||||
double limit_qdd{0.0};
|
|
||||||
bool cyclic_stream_open{false};
|
|
||||||
bool cyclic_reconnect_latched{false};
|
|
||||||
std::uint32_t cyclic_sequence{0};
|
|
||||||
std::uint64_t cyclic_generation{0};
|
|
||||||
std::uint64_t cyclic_connection_epoch{0};
|
|
||||||
bool profile_feedback_bound{false};
|
|
||||||
std::uint64_t profile_connection_epoch{0};
|
|
||||||
};
|
|
||||||
|
|
||||||
bool submitSimple_(std::uint8_t node_id,
|
|
||||||
cmvr_plc::CommandCode code,
|
|
||||||
bool wait_for_terminal,
|
|
||||||
std::uint32_t timeout_ms);
|
|
||||||
bool ensureCyclicOpen_(std::uint8_t node_id,
|
|
||||||
NodeState& state,
|
|
||||||
msgs::RunMode mode);
|
|
||||||
static std::optional<std::int32_t> toMicroUnits_(double value);
|
|
||||||
static double fromMicroUnits_(std::int32_t value);
|
|
||||||
static bool isSupportedMode_(msgs::RunMode mode);
|
|
||||||
bool validatePosition_(const NodeState& state, double position) const;
|
|
||||||
bool validateVelocity_(const NodeState& state, double velocity) const;
|
|
||||||
bool validateAcceleration_(const NodeState& state, double acceleration) const;
|
|
||||||
bool readMotionStatus_(std::uint8_t node_id, CmvrPlcAxisStatus& status);
|
|
||||||
NodeState& nodeStateLocked_(std::uint8_t node_id);
|
|
||||||
|
|
||||||
std::shared_ptr<ModbusTcpMotorBusRuntime> bus_runtime_;
|
|
||||||
std::mutex nodes_mutex_;
|
|
||||||
std::unordered_map<std::uint8_t, NodeState> nodes_;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace cmvr::device
|
|
||||||
|
|
||||||
#endif // CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H
|
|
||||||
@ -1,21 +0,0 @@
|
|||||||
#ifndef CMVR_ES_MODBUS_PLC_MOTOR_H
|
|
||||||
#define CMVR_ES_MODBUS_PLC_MOTOR_H
|
|
||||||
|
|
||||||
#include "cmvr/config/motor_config/motor_config.pb.h"
|
|
||||||
#include "devices/motor/abstract_motor.h"
|
|
||||||
|
|
||||||
namespace cmvr::device {
|
|
||||||
|
|
||||||
class ModbusPlcMotor final : public AbstractMotor {
|
|
||||||
public:
|
|
||||||
explicit ModbusPlcMotor(const config::MotorConfigItem& config);
|
|
||||||
|
|
||||||
std::string typeName() const override { return "ModbusPlcMotor"; }
|
|
||||||
bool init() override;
|
|
||||||
bool torqueOff() override;
|
|
||||||
bool quickStop() override;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace cmvr::device
|
|
||||||
|
|
||||||
#endif // CMVR_ES_MODBUS_PLC_MOTOR_H
|
|
||||||
@ -1,582 +0,0 @@
|
|||||||
#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h"
|
|
||||||
|
|
||||||
#include <algorithm>
|
|
||||||
#include <cmath>
|
|
||||||
#include <limits>
|
|
||||||
|
|
||||||
#include "common/base/logging/logger.h"
|
|
||||||
|
|
||||||
namespace cmvr::device {
|
|
||||||
namespace {
|
|
||||||
|
|
||||||
// MotorService accepts waits up to 10 minutes. The PLC timeout is a ceiling;
|
|
||||||
// shorter RPC timeouts still actively QuickStop from the service layer.
|
|
||||||
constexpr std::uint32_t kProfileCommandTimeoutCeilingMs = 600000;
|
|
||||||
constexpr std::uint32_t kSafetyCommandTimeoutMs = 5000;
|
|
||||||
|
|
||||||
bool statusAllowsMotionFeedback(const CmvrPlcAxisStatus& status)
|
|
||||||
{
|
|
||||||
constexpr std::uint16_t kFatalFlags =
|
|
||||||
cmvr_plc::StatusFlag::Fault |
|
|
||||||
cmvr_plc::StatusFlag::CommunicationWatchdogExpired |
|
|
||||||
cmvr_plc::StatusFlag::CyclicWatchdogExpired;
|
|
||||||
return (status.status_flags & kFatalFlags) == 0U &&
|
|
||||||
status.result_code == cmvr_plc::ResultCode::Ok &&
|
|
||||||
static_cast<std::uint16_t>(status.command_state) <=
|
|
||||||
static_cast<std::uint16_t>(
|
|
||||||
cmvr_plc::CommandState::CommunicationLost) &&
|
|
||||||
!cmvr_plc::isFailure(status.command_state);
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
CmvrPlcMotorProtocol::CmvrPlcMotorProtocol(
|
|
||||||
std::shared_ptr<ModbusTcpMotorBusRuntime> bus_runtime)
|
|
||||||
: bus_runtime_(std::move(bus_runtime))
|
|
||||||
{
|
|
||||||
comm_proto = CommProto::CUSTOM;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::initNode(const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) {
|
|
||||||
CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] missing PLC axis mapping for motor "
|
|
||||||
<< static_cast<int>(node_id);
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& state = nodeStateLocked_(node_id);
|
|
||||||
state.cyclic_connection_epoch = bus_runtime_->connectionEpoch();
|
|
||||||
state.cyclic_stream_open = false;
|
|
||||||
state.cyclic_reconnect_latched = false;
|
|
||||||
state.cyclic_sequence = 0;
|
|
||||||
state.cyclic_generation = 0;
|
|
||||||
state.profile_feedback_bound = false;
|
|
||||||
state.profile_connection_epoch = 0;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CmvrPlcMotorProtocol::setMode(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const msgs::RunMode mode)
|
|
||||||
{
|
|
||||||
if (!isSupportedMode_(mode)) {
|
|
||||||
CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] unsupported mode "
|
|
||||||
<< msgs::RunMode_Name(mode);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& state = nodeStateLocked_(node_id);
|
|
||||||
const bool cyclic_mode =
|
|
||||||
mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION ||
|
|
||||||
mode == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
|
|
||||||
if (cyclic_mode) {
|
|
||||||
// Every explicit cyclic setMode call is a new upper-layer stream
|
|
||||||
// generation, even when the mode value itself is unchanged. This is
|
|
||||||
// the only transition allowed to clear a reconnect latch.
|
|
||||||
state.requested_mode = mode;
|
|
||||||
state.cyclic_connection_epoch =
|
|
||||||
bus_runtime_ ? bus_runtime_->connectionEpoch() : 0U;
|
|
||||||
state.cyclic_stream_open = false;
|
|
||||||
state.cyclic_reconnect_latched = false;
|
|
||||||
state.cyclic_sequence = 0;
|
|
||||||
if (++state.cyclic_generation == 0U) {
|
|
||||||
++state.cyclic_generation;
|
|
||||||
}
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if (state.requested_mode != mode) {
|
|
||||||
state.cyclic_stream_open = false;
|
|
||||||
}
|
|
||||||
state.requested_mode = mode;
|
|
||||||
}
|
|
||||||
|
|
||||||
msgs::RunMode CmvrPlcMotorProtocol::getMode(const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
CmvrPlcAxisStatus status;
|
|
||||||
if (bus_runtime_ && bus_runtime_->readAxisStatus(node_id, status)) {
|
|
||||||
const auto mode = static_cast<msgs::RunMode>(status.current_mode);
|
|
||||||
if (isSupportedMode_(mode)) {
|
|
||||||
return mode;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
return nodeStateLocked_(node_id).requested_mode;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CmvrPlcMotorProtocol::setLimitQdd(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const double u_qdd,
|
|
||||||
const double l_qdd)
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
nodeStateLocked_(node_id).limit_qdd = std::max(std::abs(u_qdd), std::abs(l_qdd));
|
|
||||||
}
|
|
||||||
|
|
||||||
void CmvrPlcMotorProtocol::setLimitQd(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const double qd)
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
nodeStateLocked_(node_id).limit_qd = std::abs(qd);
|
|
||||||
}
|
|
||||||
|
|
||||||
void CmvrPlcMotorProtocol::setLimitQ(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const double ub,
|
|
||||||
const double lb)
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& state = nodeStateLocked_(node_id);
|
|
||||||
state.limit_q_ub = ub;
|
|
||||||
state.limit_q_lb = lb;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::calibrateZeroQ(const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
return submitSimple_(node_id, cmvr_plc::CommandCode::SetZero, true,
|
|
||||||
kSafetyCommandTimeoutMs);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::reachedTargetQ(const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
CmvrPlcAxisStatus status;
|
|
||||||
return readMotionStatus_(node_id, status) &&
|
|
||||||
statusAllowsMotionFeedback(status) &&
|
|
||||||
(status.status_flags & cmvr_plc::StatusFlag::TargetReached) != 0U;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::commandProfilePosition(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const double target_q,
|
|
||||||
const double max_qd,
|
|
||||||
const double max_qdd)
|
|
||||||
{
|
|
||||||
NodeState state;
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& stored = nodeStateLocked_(node_id);
|
|
||||||
stored.requested_mode = msgs::RUN_MODE_PROFILE_POSITION;
|
|
||||||
stored.cyclic_stream_open = false;
|
|
||||||
state = stored;
|
|
||||||
}
|
|
||||||
if (!validatePosition_(state, target_q) ||
|
|
||||||
!validateVelocity_(state, max_qd) ||
|
|
||||||
!validateAcceleration_(state, max_qdd)) {
|
|
||||||
CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] invalid profile-position command";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
const auto position = toMicroUnits_(target_q);
|
|
||||||
const auto velocity = toMicroUnits_(std::abs(max_qd));
|
|
||||||
const auto acceleration = toMicroUnits_(std::abs(max_qdd));
|
|
||||||
if (!position || !velocity || !acceleration) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
CmvrPlcAxisCommand command;
|
|
||||||
command.code = cmvr_plc::CommandCode::ProfilePosition;
|
|
||||||
command.target_position = *position;
|
|
||||||
command.target_velocity = *velocity;
|
|
||||||
command.acceleration = *acceleration;
|
|
||||||
command.command_timeout_ms = kProfileCommandTimeoutCeilingMs;
|
|
||||||
if (!bus_runtime_) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
const auto submission_epoch = bus_runtime_->connectionEpoch();
|
|
||||||
const auto submitted =
|
|
||||||
bus_runtime_->submitAxisCommand(
|
|
||||||
node_id, command, false, submission_epoch);
|
|
||||||
const auto completed_epoch = bus_runtime_->connectionEpoch();
|
|
||||||
if (!submitted && completed_epoch == submission_epoch) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& stored = nodeStateLocked_(node_id);
|
|
||||||
// Bind on every cross-epoch outcome, including an ambiguous failed
|
|
||||||
// submit, so feedback remains invalid until safety cleanup.
|
|
||||||
stored.profile_feedback_bound = true;
|
|
||||||
stored.profile_connection_epoch = submission_epoch;
|
|
||||||
}
|
|
||||||
return submitted && completed_epoch == submission_epoch;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::commandProfileVelocity(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const double target_qd,
|
|
||||||
const double max_qdd)
|
|
||||||
{
|
|
||||||
NodeState state;
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& stored = nodeStateLocked_(node_id);
|
|
||||||
stored.requested_mode = msgs::RUN_MODE_PROFILE_VELOCITY;
|
|
||||||
stored.cyclic_stream_open = false;
|
|
||||||
state = stored;
|
|
||||||
}
|
|
||||||
if (!validateVelocity_(state, target_qd) ||
|
|
||||||
!validateAcceleration_(state, max_qdd)) {
|
|
||||||
CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] invalid profile-velocity command";
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
const auto velocity = toMicroUnits_(target_qd);
|
|
||||||
const auto acceleration = toMicroUnits_(std::abs(max_qdd));
|
|
||||||
if (!velocity || !acceleration) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
CmvrPlcAxisCommand command;
|
|
||||||
command.code = cmvr_plc::CommandCode::ProfileVelocity;
|
|
||||||
command.target_velocity = *velocity;
|
|
||||||
command.acceleration = *acceleration;
|
|
||||||
command.command_timeout_ms = kProfileCommandTimeoutCeilingMs;
|
|
||||||
if (!bus_runtime_) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
const auto submission_epoch = bus_runtime_->connectionEpoch();
|
|
||||||
const auto submitted =
|
|
||||||
bus_runtime_->submitAxisCommand(
|
|
||||||
node_id, command, false, submission_epoch);
|
|
||||||
const auto completed_epoch = bus_runtime_->connectionEpoch();
|
|
||||||
if (!submitted && completed_epoch == submission_epoch) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& stored = nodeStateLocked_(node_id);
|
|
||||||
stored.profile_feedback_bound = true;
|
|
||||||
stored.profile_connection_epoch = submission_epoch;
|
|
||||||
}
|
|
||||||
return submitted && completed_epoch == submission_epoch;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::commandCyclicPosition(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const double target_q,
|
|
||||||
const double target_qd)
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& state = nodeStateLocked_(node_id);
|
|
||||||
if (!validatePosition_(state, target_q) ||
|
|
||||||
!validateVelocity_(state, target_qd) ||
|
|
||||||
!ensureCyclicOpen_(node_id, state, msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
const auto position = toMicroUnits_(target_q);
|
|
||||||
const auto velocity = toMicroUnits_(target_qd);
|
|
||||||
if (!position || !velocity) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
CmvrPlcAxisCommand command;
|
|
||||||
command.code = cmvr_plc::CommandCode::CyclicPositionSample;
|
|
||||||
command.target_position = *position;
|
|
||||||
command.target_velocity = *velocity;
|
|
||||||
command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs();
|
|
||||||
if (++state.cyclic_sequence == 0) {
|
|
||||||
++state.cyclic_sequence;
|
|
||||||
}
|
|
||||||
command.cyclic_sample_sequence = state.cyclic_sequence;
|
|
||||||
const auto submitted =
|
|
||||||
bus_runtime_->submitAxisCommand(
|
|
||||||
node_id, command, false, state.cyclic_connection_epoch);
|
|
||||||
if (submitted) {
|
|
||||||
state.profile_feedback_bound = false;
|
|
||||||
state.profile_connection_epoch = 0;
|
|
||||||
} else if (
|
|
||||||
bus_runtime_->connectionEpoch() != state.cyclic_connection_epoch) {
|
|
||||||
state.cyclic_reconnect_latched = true;
|
|
||||||
state.cyclic_stream_open = false;
|
|
||||||
state.cyclic_sequence = 0;
|
|
||||||
state.cyclic_connection_epoch = bus_runtime_->connectionEpoch();
|
|
||||||
}
|
|
||||||
return submitted;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::commandCyclicVelocity(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const double target_qd)
|
|
||||||
{
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& state = nodeStateLocked_(node_id);
|
|
||||||
if (!validateVelocity_(state, target_qd) ||
|
|
||||||
!ensureCyclicOpen_(node_id, state, msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
const auto velocity = toMicroUnits_(target_qd);
|
|
||||||
if (!velocity) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
CmvrPlcAxisCommand command;
|
|
||||||
command.code = cmvr_plc::CommandCode::CyclicVelocitySample;
|
|
||||||
command.target_velocity = *velocity;
|
|
||||||
command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs();
|
|
||||||
if (++state.cyclic_sequence == 0) {
|
|
||||||
++state.cyclic_sequence;
|
|
||||||
}
|
|
||||||
command.cyclic_sample_sequence = state.cyclic_sequence;
|
|
||||||
const auto submitted =
|
|
||||||
bus_runtime_->submitAxisCommand(
|
|
||||||
node_id, command, false, state.cyclic_connection_epoch);
|
|
||||||
if (submitted) {
|
|
||||||
state.profile_feedback_bound = false;
|
|
||||||
state.profile_connection_epoch = 0;
|
|
||||||
} else if (
|
|
||||||
bus_runtime_->connectionEpoch() != state.cyclic_connection_epoch) {
|
|
||||||
state.cyclic_reconnect_latched = true;
|
|
||||||
state.cyclic_stream_open = false;
|
|
||||||
state.cyclic_sequence = 0;
|
|
||||||
state.cyclic_connection_epoch = bus_runtime_->connectionEpoch();
|
|
||||||
}
|
|
||||||
return submitted;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::commandCyclicTorque(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const double target_tau)
|
|
||||||
{
|
|
||||||
(void)target_tau;
|
|
||||||
CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] cyclic torque is unsupported, motor="
|
|
||||||
<< static_cast<int>(node_id);
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CmvrPlcMotorProtocol::setMotorConversion(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const double encoder_counts_per_rev,
|
|
||||||
const double gear_ratio)
|
|
||||||
{
|
|
||||||
(void)node_id;
|
|
||||||
(void)encoder_counts_per_rev;
|
|
||||||
(void)gear_ratio;
|
|
||||||
// CMVR PLC v1 exchanges SI quantities in fixed-point micro-units.
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::torqueOn(const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
return submitSimple_(node_id, cmvr_plc::CommandCode::Enable, true,
|
|
||||||
kSafetyCommandTimeoutMs);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::torqueOff(const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
CmvrPlcAxisCommand command;
|
|
||||||
command.code = cmvr_plc::CommandCode::Disable;
|
|
||||||
command.command_timeout_ms = kSafetyCommandTimeoutMs;
|
|
||||||
const auto result =
|
|
||||||
bus_runtime_ && bus_runtime_->submitAxisSafetyCommand(node_id, command);
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& state = nodeStateLocked_(node_id);
|
|
||||||
state.cyclic_stream_open = false;
|
|
||||||
if (result) {
|
|
||||||
state.profile_feedback_bound = false;
|
|
||||||
state.profile_connection_epoch = 0;
|
|
||||||
}
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::brakeRelease(const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] brake release is unsupported, motor="
|
|
||||||
<< static_cast<int>(node_id);
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::quickStop(const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
CmvrPlcAxisCommand command;
|
|
||||||
command.code = cmvr_plc::CommandCode::QuickStop;
|
|
||||||
command.command_timeout_ms = kSafetyCommandTimeoutMs;
|
|
||||||
const auto result =
|
|
||||||
bus_runtime_ && bus_runtime_->submitAxisSafetyCommand(node_id, command);
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
auto& state = nodeStateLocked_(node_id);
|
|
||||||
state.cyclic_stream_open = false;
|
|
||||||
if (result) {
|
|
||||||
state.profile_feedback_bound = false;
|
|
||||||
state.profile_connection_epoch = 0;
|
|
||||||
}
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
|
|
||||||
double CmvrPlcMotorProtocol::getQ(const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
CmvrPlcAxisStatus status;
|
|
||||||
if (!readMotionStatus_(node_id, status) ||
|
|
||||||
!statusAllowsMotionFeedback(status)) {
|
|
||||||
return std::numeric_limits<double>::quiet_NaN();
|
|
||||||
}
|
|
||||||
return fromMicroUnits_(status.actual_position);
|
|
||||||
}
|
|
||||||
|
|
||||||
double CmvrPlcMotorProtocol::getQd(const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
CmvrPlcAxisStatus status;
|
|
||||||
if (!readMotionStatus_(node_id, status) ||
|
|
||||||
!statusAllowsMotionFeedback(status)) {
|
|
||||||
return std::numeric_limits<double>::quiet_NaN();
|
|
||||||
}
|
|
||||||
return fromMicroUnits_(status.actual_velocity);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::readMotionStatus_(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
CmvrPlcAxisStatus& status)
|
|
||||||
{
|
|
||||||
if (!bus_runtime_) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
std::lock_guard<std::mutex> lock(nodes_mutex_);
|
|
||||||
const auto& state = nodeStateLocked_(node_id);
|
|
||||||
const auto read_epoch = bus_runtime_->connectionEpoch();
|
|
||||||
if (state.profile_feedback_bound &&
|
|
||||||
state.profile_connection_epoch != read_epoch) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (!bus_runtime_->readAxisStatus(node_id, status)) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
// Reject a status transaction that straddled a disconnect/reconnect even
|
|
||||||
// when no profile binding was active at the first check.
|
|
||||||
if (bus_runtime_->connectionEpoch() != read_epoch) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
return !state.profile_feedback_bound ||
|
|
||||||
state.profile_connection_epoch == read_epoch;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::submitSimple_(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
const cmvr_plc::CommandCode code,
|
|
||||||
const bool wait_for_terminal,
|
|
||||||
const std::uint32_t timeout_ms)
|
|
||||||
{
|
|
||||||
CmvrPlcAxisCommand command;
|
|
||||||
command.code = code;
|
|
||||||
command.command_timeout_ms = timeout_ms;
|
|
||||||
return bus_runtime_ &&
|
|
||||||
bus_runtime_->submitAxisCommand(node_id, command, wait_for_terminal);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::ensureCyclicOpen_(
|
|
||||||
const std::uint8_t node_id,
|
|
||||||
NodeState& state,
|
|
||||||
const msgs::RunMode mode)
|
|
||||||
{
|
|
||||||
if (!bus_runtime_) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
const auto connection_epoch = bus_runtime_->connectionEpoch();
|
|
||||||
if (state.cyclic_connection_epoch != connection_epoch) {
|
|
||||||
// A cyclic generation is bound to the PLC ownership epoch in which it
|
|
||||||
// was created. Never reinterpret a setpoint from that generation as
|
|
||||||
// the first setpoint of a freshly reconnected PLC stream.
|
|
||||||
if (state.cyclic_generation != 0U) {
|
|
||||||
state.cyclic_reconnect_latched = true;
|
|
||||||
}
|
|
||||||
state.cyclic_connection_epoch = connection_epoch;
|
|
||||||
state.cyclic_stream_open = false;
|
|
||||||
state.cyclic_sequence = 0;
|
|
||||||
}
|
|
||||||
if (state.cyclic_reconnect_latched) {
|
|
||||||
CMVR_LOG(WARNING)
|
|
||||||
<< "[CmvrPlcMotorProtocol] cyclic stream crossed a PLC session; "
|
|
||||||
"explicit setMode from a new upper-layer stream is required, motor="
|
|
||||||
<< static_cast<int>(node_id);
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (state.cyclic_stream_open && state.requested_mode == mode) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Preserve direct protocol use that does not call setMode explicitly:
|
|
||||||
// its first successful Open still establishes a generation. Once that
|
|
||||||
// generation has observed a reconnect, only explicit setMode can recover.
|
|
||||||
if (state.cyclic_generation == 0U) {
|
|
||||||
state.cyclic_generation = 1U;
|
|
||||||
state.cyclic_connection_epoch = connection_epoch;
|
|
||||||
}
|
|
||||||
|
|
||||||
CmvrPlcAxisCommand command;
|
|
||||||
command.code = mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION
|
|
||||||
? cmvr_plc::CommandCode::OpenCyclicPosition
|
|
||||||
: cmvr_plc::CommandCode::OpenCyclicVelocity;
|
|
||||||
command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs();
|
|
||||||
command.command_timeout_ms = kSafetyCommandTimeoutMs;
|
|
||||||
if (!bus_runtime_->submitAxisCommand(
|
|
||||||
node_id, command, false, state.cyclic_connection_epoch)) {
|
|
||||||
if (bus_runtime_->connectionEpoch() !=
|
|
||||||
state.cyclic_connection_epoch) {
|
|
||||||
state.cyclic_reconnect_latched = true;
|
|
||||||
state.cyclic_stream_open = false;
|
|
||||||
state.cyclic_sequence = 0;
|
|
||||||
state.cyclic_connection_epoch =
|
|
||||||
bus_runtime_->connectionEpoch();
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
state.requested_mode = mode;
|
|
||||||
state.cyclic_stream_open = true;
|
|
||||||
state.cyclic_sequence = 0;
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::optional<std::int32_t> CmvrPlcMotorProtocol::toMicroUnits_(const double value)
|
|
||||||
{
|
|
||||||
if (!std::isfinite(value)) {
|
|
||||||
return std::nullopt;
|
|
||||||
}
|
|
||||||
const auto scaled = std::round(value * cmvr_plc::kPositionScale);
|
|
||||||
if (scaled < static_cast<double>(std::numeric_limits<std::int32_t>::min()) ||
|
|
||||||
scaled > static_cast<double>(std::numeric_limits<std::int32_t>::max())) {
|
|
||||||
return std::nullopt;
|
|
||||||
}
|
|
||||||
return static_cast<std::int32_t>(scaled);
|
|
||||||
}
|
|
||||||
|
|
||||||
double CmvrPlcMotorProtocol::fromMicroUnits_(const std::int32_t value)
|
|
||||||
{
|
|
||||||
return static_cast<double>(value) / cmvr_plc::kPositionScale;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::isSupportedMode_(const msgs::RunMode mode)
|
|
||||||
{
|
|
||||||
return mode == msgs::RUN_MODE_PROFILE_POSITION ||
|
|
||||||
mode == msgs::RUN_MODE_PROFILE_VELOCITY ||
|
|
||||||
mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION ||
|
|
||||||
mode == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::validatePosition_(
|
|
||||||
const NodeState& state,
|
|
||||||
const double position) const
|
|
||||||
{
|
|
||||||
return std::isfinite(position) &&
|
|
||||||
(state.limit_q_ub <= state.limit_q_lb ||
|
|
||||||
(position >= state.limit_q_lb && position <= state.limit_q_ub));
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::validateVelocity_(
|
|
||||||
const NodeState& state,
|
|
||||||
const double velocity) const
|
|
||||||
{
|
|
||||||
return std::isfinite(velocity) &&
|
|
||||||
(state.limit_qd <= 0.0 || std::abs(velocity) <= state.limit_qd);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CmvrPlcMotorProtocol::validateAcceleration_(
|
|
||||||
const NodeState& state,
|
|
||||||
const double acceleration) const
|
|
||||||
{
|
|
||||||
return std::isfinite(acceleration) && acceleration >= 0.0 &&
|
|
||||||
(state.limit_qdd <= 0.0 || acceleration <= state.limit_qdd);
|
|
||||||
}
|
|
||||||
|
|
||||||
CmvrPlcMotorProtocol::NodeState& CmvrPlcMotorProtocol::nodeStateLocked_(
|
|
||||||
const std::uint8_t node_id)
|
|
||||||
{
|
|
||||||
return nodes_[node_id];
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace cmvr::device
|
|
||||||
@ -1,55 +0,0 @@
|
|||||||
#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h"
|
|
||||||
|
|
||||||
#include "common/base/logging/logger.h"
|
|
||||||
#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h"
|
|
||||||
#include <cmath>
|
|
||||||
|
|
||||||
namespace cmvr::device {
|
|
||||||
|
|
||||||
ModbusPlcMotor::ModbusPlcMotor(const config::MotorConfigItem& config)
|
|
||||||
{
|
|
||||||
info_.id = config.id();
|
|
||||||
info_.joint_name = config.joint_name();
|
|
||||||
info_.limit_q_lb = config.limit_q_lb();
|
|
||||||
info_.limit_q_ub = config.limit_q_ub();
|
|
||||||
info_.limit_qd = config.limit_qd();
|
|
||||||
info_.limit_qdd = config.limit_qdd();
|
|
||||||
node_id_ = static_cast<std::uint8_t>(config.id());
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ModbusPlcMotor::init()
|
|
||||||
{
|
|
||||||
if (!protocol_ ||
|
|
||||||
!std::dynamic_pointer_cast<CmvrPlcMotorProtocol>(protocol_)) {
|
|
||||||
CMVR_LOG(ERROR) << "[ModbusPlcMotor] invalid CMVR PLC protocol for "
|
|
||||||
<< info_.joint_name;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) ||
|
|
||||||
!std::isfinite(info_.limit_qd) || !std::isfinite(info_.limit_qdd) ||
|
|
||||||
info_.limit_q_ub <= info_.limit_q_lb ||
|
|
||||||
info_.limit_qd <= 0.0 || info_.limit_qdd <= 0.0) {
|
|
||||||
CMVR_LOG(ERROR) << "[ModbusPlcMotor] finite position/velocity/acceleration "
|
|
||||||
"limits are required for "
|
|
||||||
<< info_.joint_name;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
setLimitQ(info_.limit_q_ub, info_.limit_q_lb);
|
|
||||||
setLimitQd(info_.limit_qd);
|
|
||||||
setLimitQdd(info_.limit_qdd, -info_.limit_qdd);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ModbusPlcMotor::torqueOff()
|
|
||||||
{
|
|
||||||
auto protocol = std::dynamic_pointer_cast<CmvrPlcMotorProtocol>(protocol_);
|
|
||||||
return protocol && protocol->torqueOff(node_id_);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool ModbusPlcMotor::quickStop()
|
|
||||||
{
|
|
||||||
auto protocol = std::dynamic_pointer_cast<CmvrPlcMotorProtocol>(protocol_);
|
|
||||||
return protocol && protocol->quickStop(node_id_);
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace cmvr::device
|
|
||||||
@ -13,7 +13,6 @@ target_link_libraries(motor_manager
|
|||||||
cmvr_es::device::ti5_canopen_motor_driver
|
cmvr_es::device::ti5_canopen_motor_driver
|
||||||
cmvr_es::device::mujoco_motor_driver
|
cmvr_es::device::mujoco_motor_driver
|
||||||
cmvr_es::device::ethercat_motor_driver
|
cmvr_es::device::ethercat_motor_driver
|
||||||
cmvr_es::device::modbus_plc_motor_driver
|
|
||||||
cmvr_es::ik_solver
|
cmvr_es::ik_solver
|
||||||
glog
|
glog
|
||||||
)
|
)
|
||||||
|
|||||||
@ -78,10 +78,6 @@ private:
|
|||||||
const config::MotorGroupConfig& group_cfg,
|
const config::MotorGroupConfig& group_cfg,
|
||||||
const std::vector<config::MotorConfigItem>& motor_cfgs,
|
const std::vector<config::MotorConfigItem>& motor_cfgs,
|
||||||
const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
|
const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
|
||||||
std::vector<std::shared_ptr<AbstractMotor>> createModbusTcpMotors_(
|
|
||||||
const config::MotorGroupConfig& group_cfg,
|
|
||||||
const std::vector<config::MotorConfigItem>& motor_cfgs,
|
|
||||||
const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
config::MotorConfig cfg_;
|
config::MotorConfig cfg_;
|
||||||
|
|||||||
@ -15,14 +15,11 @@
|
|||||||
#include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h"
|
#include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h"
|
||||||
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
|
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
|
||||||
#include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
|
#include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
|
||||||
#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h"
|
|
||||||
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
|
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
|
||||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
|
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
|
||||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
|
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
|
||||||
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
|
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
|
||||||
#include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
|
#include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
|
||||||
#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h"
|
|
||||||
#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h"
|
|
||||||
#include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h"
|
#include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h"
|
||||||
#include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
|
#include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
|
||||||
|
|
||||||
@ -418,16 +415,6 @@ std::shared_ptr<AbstractMotorBusRuntime> MotorManager::createBusRuntime_(
|
|||||||
return std::make_shared<CanMotorBusRuntime>();
|
return std::make_shared<CanMotorBusRuntime>();
|
||||||
case config::MOTOR_BUS_MUJOCO:
|
case config::MOTOR_BUS_MUJOCO:
|
||||||
return std::make_shared<MujocoMotorBusRuntime>();
|
return std::make_shared<MujocoMotorBusRuntime>();
|
||||||
case config::MOTOR_BUS_MODBUS_TCP:
|
|
||||||
if (group_cfg.vendor() == config::MOTOR_VENDOR_PLC_GENERIC &&
|
|
||||||
group_cfg.protocol() == config::MOTOR_PROTOCOL_CMVR_PLC_V1) {
|
|
||||||
return std::make_shared<ModbusTcpMotorBusRuntime>();
|
|
||||||
}
|
|
||||||
CMVR_LOG(ERROR) << "[MotorManager] unsupported Modbus TCP motor: vendor="
|
|
||||||
<< config::MotorVendor_Name(group_cfg.vendor())
|
|
||||||
<< ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
|
|
||||||
<< ", group=" << group_cfg.id();
|
|
||||||
return nullptr;
|
|
||||||
case config::MOTOR_BUS_ETHERCAT: {
|
case config::MOTOR_BUS_ETHERCAT: {
|
||||||
auto runtime = std::make_shared<EthercatMotorBusRuntime>();
|
auto runtime = std::make_shared<EthercatMotorBusRuntime>();
|
||||||
if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU &&
|
if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU &&
|
||||||
@ -467,8 +454,6 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createMotors_(
|
|||||||
return createMujocoMotors_(group_cfg, motor_cfgs, bus_runtime);
|
return createMujocoMotors_(group_cfg, motor_cfgs, bus_runtime);
|
||||||
case config::MOTOR_BUS_ETHERCAT:
|
case config::MOTOR_BUS_ETHERCAT:
|
||||||
return createEthercatMotors_(group_cfg, motor_cfgs, bus_runtime);
|
return createEthercatMotors_(group_cfg, motor_cfgs, bus_runtime);
|
||||||
case config::MOTOR_BUS_MODBUS_TCP:
|
|
||||||
return createModbusTcpMotors_(group_cfg, motor_cfgs, bus_runtime);
|
|
||||||
default:
|
default:
|
||||||
CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: "
|
CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: "
|
||||||
<< config::MotorBusType_Name(group_cfg.bus_type())
|
<< config::MotorBusType_Name(group_cfg.bus_type())
|
||||||
@ -611,50 +596,4 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
|
|||||||
return motors;
|
return motors;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createModbusTcpMotors_(
|
|
||||||
const config::MotorGroupConfig& group_cfg,
|
|
||||||
const std::vector<config::MotorConfigItem>& motor_cfgs,
|
|
||||||
const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const
|
|
||||||
{
|
|
||||||
auto modbus_runtime =
|
|
||||||
std::dynamic_pointer_cast<ModbusTcpMotorBusRuntime>(bus_runtime);
|
|
||||||
if (!modbus_runtime || !group_cfg.has_modbus_tcp()) {
|
|
||||||
CMVR_LOG(ERROR) << "[MotorManager] missing Modbus TCP runtime/config: "
|
|
||||||
<< group_cfg.id();
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
if (group_cfg.vendor() != config::MOTOR_VENDOR_PLC_GENERIC ||
|
|
||||||
group_cfg.protocol() != config::MOTOR_PROTOCOL_CMVR_PLC_V1) {
|
|
||||||
CMVR_LOG(ERROR) << "[MotorManager] unsupported Modbus TCP PLC motor: vendor="
|
|
||||||
<< config::MotorVendor_Name(group_cfg.vendor())
|
|
||||||
<< ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol());
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
for (const auto& motor_cfg : motor_cfgs) {
|
|
||||||
if (motor_cfg.id() < 0 || motor_cfg.id() > 255 ||
|
|
||||||
!modbus_runtime->hasMotor(static_cast<std::uint8_t>(motor_cfg.id()))) {
|
|
||||||
CMVR_LOG(ERROR) << "[MotorManager] missing PLC axis mapping for motor id "
|
|
||||||
<< motor_cfg.id() << " in group: " << group_cfg.id();
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
auto protocol = std::make_shared<CmvrPlcMotorProtocol>(modbus_runtime);
|
|
||||||
std::vector<std::shared_ptr<AbstractMotor>> motors;
|
|
||||||
motors.reserve(motor_cfgs.size());
|
|
||||||
for (const auto& cfg : motor_cfgs) {
|
|
||||||
auto motor = std::make_shared<ModbusPlcMotor>(cfg);
|
|
||||||
motor->setProtocol(protocol);
|
|
||||||
// initNode() and ModbusPlcMotor::init() only establish local mappings and
|
|
||||||
// limits. The shared TCP connection starts after all motors are created.
|
|
||||||
if (!motor->init()) {
|
|
||||||
CMVR_LOG(ERROR) << "[MotorManager] failed to init Modbus PLC motor: "
|
|
||||||
<< cfg.joint_name();
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
motors.push_back(std::move(motor));
|
|
||||||
}
|
|
||||||
return motors;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace cmvr::device
|
} // namespace cmvr::device
|
||||||
|
|||||||
@ -221,44 +221,6 @@ if(BUILD_TESTING)
|
|||||||
ENVIRONMENT "${_grpc_agv_test_environment}"
|
ENVIRONMENT "${_grpc_agv_test_environment}"
|
||||||
)
|
)
|
||||||
|
|
||||||
set(_grpc_motor_modbus_e2e_libmodbus_root
|
|
||||||
"${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11")
|
|
||||||
add_executable(grpc_motor_service_modbus_e2e_test
|
|
||||||
grpc/tests/grpc_motor_service_modbus_e2e_test.cpp
|
|
||||||
)
|
|
||||||
target_include_directories(grpc_motor_service_modbus_e2e_test
|
|
||||||
PRIVATE
|
|
||||||
${CMAKE_SOURCE_DIR}/cmvr-es
|
|
||||||
${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager
|
|
||||||
${_grpc_motor_modbus_e2e_libmodbus_root}/include
|
|
||||||
)
|
|
||||||
target_link_directories(grpc_motor_service_modbus_e2e_test
|
|
||||||
PRIVATE
|
|
||||||
${_grpc_motor_modbus_e2e_libmodbus_root}/lib
|
|
||||||
)
|
|
||||||
target_link_libraries(grpc_motor_service_modbus_e2e_test
|
|
||||||
PRIVATE
|
|
||||||
service
|
|
||||||
modbus
|
|
||||||
gtest
|
|
||||||
gtest_main
|
|
||||||
pthread
|
|
||||||
)
|
|
||||||
add_test(
|
|
||||||
NAME grpc_motor_service_modbus_e2e_test
|
|
||||||
COMMAND grpc_motor_service_modbus_e2e_test
|
|
||||||
)
|
|
||||||
set(_grpc_motor_modbus_e2e_environment
|
|
||||||
"LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}"
|
|
||||||
)
|
|
||||||
if(CMVR_TEST_SYSTEM_LIBSTDCXX)
|
|
||||||
list(APPEND _grpc_motor_modbus_e2e_environment
|
|
||||||
"LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}")
|
|
||||||
endif()
|
|
||||||
set_tests_properties(grpc_motor_service_modbus_e2e_test PROPERTIES
|
|
||||||
TIMEOUT 20
|
|
||||||
ENVIRONMENT "${_grpc_motor_modbus_e2e_environment}"
|
|
||||||
)
|
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
# --------------------------------------------------------
|
# --------------------------------------------------------
|
||||||
|
|||||||
@ -42,7 +42,7 @@ grpcurl -plaintext \
|
|||||||
## MotorService
|
## MotorService
|
||||||
|
|
||||||
`MotorService` 将 gRPC 电机命令适配到已经由 `DeviceManager` 创建的
|
`MotorService` 将 gRPC 电机命令适配到已经由 `DeviceManager` 创建的
|
||||||
`MotorManager` 和 `AbstractMotor`，不直接持有 PLC、现场总线或厂商驱动。
|
`MotorManager` 和 `AbstractMotor`，不直接持有现场总线或厂商驱动。
|
||||||
|
|
||||||
关键文件：
|
关键文件：
|
||||||
|
|
||||||
@ -52,8 +52,6 @@ grpcurl -plaintext \
|
|||||||
和 [`grpc/src/grpc_motor_service.cpp`](grpc/src/grpc_motor_service.cpp)
|
和 [`grpc/src/grpc_motor_service.cpp`](grpc/src/grpc_motor_service.cpp)
|
||||||
- 注册：[`../task/grpc_server_task/src/grpc_server_task.cpp`](../task/grpc_server_task/src/grpc_server_task.cpp)
|
- 注册：[`../task/grpc_server_task/src/grpc_server_task.cpp`](../task/grpc_server_task/src/grpc_server_task.cpp)
|
||||||
- 单元测试：[`grpc/tests/grpc_motor_service_test.cpp`](grpc/tests/grpc_motor_service_test.cpp)
|
- 单元测试：[`grpc/tests/grpc_motor_service_test.cpp`](grpc/tests/grpc_motor_service_test.cpp)
|
||||||
- gRPC–Modbus 端到端测试：
|
|
||||||
[`grpc/tests/grpc_motor_service_modbus_e2e_test.cpp`](grpc/tests/grpc_motor_service_modbus_e2e_test.cpp)
|
|
||||||
|
|
||||||
服务按单电机仲裁。同步 Profile 命令、Cyclic Position/Velocity 双向流、
|
服务按单电机仲裁。同步 Profile 命令、Cyclic Position/Velocity 双向流、
|
||||||
`setEnabled`、状态读取和软件 `emergencyStop` 共用同一控制权状态：
|
`setEnabled`、状态读取和软件 `emergencyStop` 共用同一控制权状态：
|
||||||
@ -66,11 +64,6 @@ grpcurl -plaintext \
|
|||||||
- 只有成功执行 `setEnabled(true)` 才解除服务内软件急停锁存；
|
- 只有成功执行 `setEnabled(true)` 才解除服务内软件急停锁存；
|
||||||
- 服务层 Quick Stop 和 `emergencyStop` 都不具备功能安全等级。
|
- 服务层 Quick Stop 和 `emergencyStop` 都不具备功能安全等级。
|
||||||
|
|
||||||
PLC 后端的连接 epoch、stream epoch、寄存器、ACK 和 TIA Portal 要求见：
|
|
||||||
|
|
||||||
- [Modbus TCP PLC runtime](../devices/motor/bus_runtime/modbus_tcp/README.md)
|
|
||||||
- [MotorService 与 CMVR PLC v1 完整协议](../../docs/motor_service_modbus_tcp.md)
|
|
||||||
|
|
||||||
AUBO 控制柜 IO 不经过 `MotorService`，由
|
AUBO 控制柜 IO 不经过 `MotorService`，由
|
||||||
`ArmService/ExecuteJsonCommand` 转发到目标 `RobotArm`。厂商命令和安全约束见
|
`ArmService/ExecuteJsonCommand` 转发到目标 `RobotArm`。厂商命令和安全约束见
|
||||||
[AUBO 控制柜 IO](../devices/arm/aubo_arm/README.md)。
|
[AUBO 控制柜 IO](../devices/arm/aubo_arm/README.md)。
|
||||||
|
|||||||
@ -966,7 +966,7 @@ grpc::Status gRPCMotorServiceImpl::setZeroImpl(
|
|||||||
}
|
}
|
||||||
if (!calibrated) {
|
if (!calibrated) {
|
||||||
const std::string error =
|
const std::string error =
|
||||||
"zero calibration outcome unknown; inspect PLC zero_epoch/session before retry";
|
"zero calibration outcome unknown; inspect device state before retry";
|
||||||
setLastError(resolved.control, error);
|
setLastError(resolved.control, error);
|
||||||
lease.reset();
|
lease.reset();
|
||||||
fillFeedback(response->mutable_header(), false, error);
|
fillFeedback(response->mutable_header(), false, error);
|
||||||
@ -1984,7 +1984,7 @@ grpc::Status gRPCMotorServiceImpl::setEnabledImpl(
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// A false acknowledgement may still mean the PLC committed the
|
// A false acknowledgement may still mean the backend committed the
|
||||||
// request. A successful enable also needs rollback if cancellation or
|
// request. A successful enable also needs rollback if cancellation or
|
||||||
// E-stop won while torqueOn was in flight.
|
// E-stop won while torqueOn was in flight.
|
||||||
const bool cleanup_required =
|
const bool cleanup_required =
|
||||||
|
|||||||
@ -1,907 +0,0 @@
|
|||||||
#include "service/grpc/include/grpc_motor_service.h"
|
|
||||||
|
|
||||||
#include <arpa/inet.h>
|
|
||||||
#include <array>
|
|
||||||
#include <atomic>
|
|
||||||
#include <cerrno>
|
|
||||||
#include <chrono>
|
|
||||||
#include <cstdio>
|
|
||||||
#include <cstring>
|
|
||||||
#include <fstream>
|
|
||||||
#include <memory>
|
|
||||||
#include <sstream>
|
|
||||||
#include <string>
|
|
||||||
#include <thread>
|
|
||||||
|
|
||||||
#include <grpcpp/grpcpp.h>
|
|
||||||
#include <gtest/gtest.h>
|
|
||||||
#include <modbus/modbus.h>
|
|
||||||
#include <netinet/in.h>
|
|
||||||
#include <sys/socket.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
|
|
||||||
#include "cmvr/config/motor_config/motor_config.pb.h"
|
|
||||||
#include "common/config/config_files.h"
|
|
||||||
#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h"
|
|
||||||
#include "devices/motor/manager/include/motor_manager.h"
|
|
||||||
#include "manager/device_manager/include/device_manager.h"
|
|
||||||
|
|
||||||
namespace cmvr::service {
|
|
||||||
namespace {
|
|
||||||
|
|
||||||
using namespace std::chrono_literals;
|
|
||||||
namespace plc = device::cmvr_plc;
|
|
||||||
|
|
||||||
constexpr char kManagerId[] = "e2e_plc_motors";
|
|
||||||
constexpr char kMotorGroupId[] = "e2e_plc_axis_group";
|
|
||||||
constexpr char kJointName[] = "E2E_PLC_AXIS";
|
|
||||||
|
|
||||||
std::uint16_t reserveLoopbackPort()
|
|
||||||
{
|
|
||||||
const int socket_fd = ::socket(AF_INET, SOCK_STREAM, 0);
|
|
||||||
if (socket_fd < 0) {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
sockaddr_in address{};
|
|
||||||
address.sin_family = AF_INET;
|
|
||||||
address.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
|
|
||||||
address.sin_port = 0;
|
|
||||||
if (::bind(socket_fd, reinterpret_cast<sockaddr*>(&address),
|
|
||||||
sizeof(address)) != 0) {
|
|
||||||
::close(socket_fd);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
socklen_t length = sizeof(address);
|
|
||||||
if (::getsockname(socket_fd, reinterpret_cast<sockaddr*>(&address),
|
|
||||||
&length) != 0) {
|
|
||||||
::close(socket_fd);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
const auto port = ntohs(address.sin_port);
|
|
||||||
::close(socket_fd);
|
|
||||||
return port;
|
|
||||||
}
|
|
||||||
|
|
||||||
template <typename Predicate>
|
|
||||||
bool waitUntil(Predicate&& predicate, const std::chrono::milliseconds timeout)
|
|
||||||
{
|
|
||||||
const auto deadline = std::chrono::steady_clock::now() + timeout;
|
|
||||||
do {
|
|
||||||
if (predicate()) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
std::this_thread::sleep_for(5ms);
|
|
||||||
} while (std::chrono::steady_clock::now() < deadline);
|
|
||||||
return predicate();
|
|
||||||
}
|
|
||||||
|
|
||||||
class FakeCmvrPlc final {
|
|
||||||
public:
|
|
||||||
~FakeCmvrPlc() { stop(); }
|
|
||||||
|
|
||||||
bool start()
|
|
||||||
{
|
|
||||||
port_ = reserveLoopbackPort();
|
|
||||||
if (port_ == 0) {
|
|
||||||
std::fprintf(stderr, "reserveLoopbackPort failed: %s\n",
|
|
||||||
std::strerror(errno));
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
context_ = modbus_new_tcp("127.0.0.1", port_);
|
|
||||||
mapping_ = modbus_mapping_new(
|
|
||||||
1, 1, plc::kAxisFirstOffset + plc::kAxisRegisterStride, 1);
|
|
||||||
if (!context_ || !mapping_) {
|
|
||||||
std::fprintf(stderr, "fake PLC allocation failed: %s\n",
|
|
||||||
modbus_strerror(errno));
|
|
||||||
stop();
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
auto* registers = mapping_->tab_registers;
|
|
||||||
registers[plc::kMagicCmOffset] = plc::kMagicCm;
|
|
||||||
registers[plc::kMagicVrOffset] = plc::kMagicVr;
|
|
||||||
registers[plc::kProtocolMajorOffset] = plc::kProtocolMajor;
|
|
||||||
registers[plc::kProtocolMinorOffset] = plc::kProtocolMinor;
|
|
||||||
registers[plc::kAxisCountOffset] = 1;
|
|
||||||
registers[plc::kOwnerStateOffset] =
|
|
||||||
static_cast<std::uint16_t>(plc::OwnerState::None);
|
|
||||||
plc::encodeUint32(registers, plc::kPlcBootIdOffset, 0x45453245U);
|
|
||||||
plc::encodeUint32(registers, plc::kPlcHeartbeatOffset,
|
|
||||||
plc_heartbeat_);
|
|
||||||
|
|
||||||
const auto status_base =
|
|
||||||
plc::axisBase(0) + plc::kAxisStatusRelativeOffset;
|
|
||||||
registers[status_base + plc::kCommandState] =
|
|
||||||
static_cast<std::uint16_t>(plc::CommandState::Idle);
|
|
||||||
registers[status_base + plc::kResultCode] =
|
|
||||||
static_cast<std::uint16_t>(plc::ResultCode::Ok);
|
|
||||||
publishSnapshot_(registers + status_base);
|
|
||||||
|
|
||||||
listen_socket_.store(modbus_tcp_listen(context_, 2));
|
|
||||||
if (listen_socket_.load() < 0) {
|
|
||||||
std::fprintf(stderr, "modbus_tcp_listen failed on %u: %s\n",
|
|
||||||
port_, modbus_strerror(errno));
|
|
||||||
stop();
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
running_.store(true);
|
|
||||||
worker_ = std::thread(&FakeCmvrPlc::loop_, this);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void stop()
|
|
||||||
{
|
|
||||||
running_.store(false);
|
|
||||||
const auto client = client_socket_.exchange(-1);
|
|
||||||
if (client >= 0) {
|
|
||||||
::shutdown(client, SHUT_RDWR);
|
|
||||||
::close(client);
|
|
||||||
}
|
|
||||||
const auto listener = listen_socket_.exchange(-1);
|
|
||||||
if (listener >= 0) {
|
|
||||||
::shutdown(listener, SHUT_RDWR);
|
|
||||||
::close(listener);
|
|
||||||
}
|
|
||||||
if (worker_.joinable()) {
|
|
||||||
worker_.join();
|
|
||||||
}
|
|
||||||
if (mapping_) {
|
|
||||||
modbus_mapping_free(mapping_);
|
|
||||||
mapping_ = nullptr;
|
|
||||||
}
|
|
||||||
if (context_) {
|
|
||||||
modbus_free(context_);
|
|
||||||
context_ = nullptr;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void disconnectClient()
|
|
||||||
{
|
|
||||||
const auto client = client_socket_.load();
|
|
||||||
if (client >= 0) {
|
|
||||||
::shutdown(client, SHUT_RDWR);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
std::uint16_t port() const { return port_; }
|
|
||||||
std::uint32_t commandCount() const { return command_count_.load(); }
|
|
||||||
std::uint32_t acceptedSessionCount() const
|
|
||||||
{
|
|
||||||
return accepted_session_count_.load();
|
|
||||||
}
|
|
||||||
std::uint32_t enableCount() const { return enable_count_.load(); }
|
|
||||||
std::uint32_t profilePositionCount() const
|
|
||||||
{
|
|
||||||
return profile_position_count_.load();
|
|
||||||
}
|
|
||||||
std::uint32_t profileVelocityCount() const
|
|
||||||
{
|
|
||||||
return profile_velocity_count_.load();
|
|
||||||
}
|
|
||||||
std::uint32_t cyclicOpenCount() const
|
|
||||||
{
|
|
||||||
return cyclic_open_count_.load();
|
|
||||||
}
|
|
||||||
std::uint32_t cyclicSampleCount() const
|
|
||||||
{
|
|
||||||
return cyclic_sample_count_.load();
|
|
||||||
}
|
|
||||||
std::uint32_t quickStopCount() const
|
|
||||||
{
|
|
||||||
return quick_stop_count_.load();
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
void loop_()
|
|
||||||
{
|
|
||||||
std::array<std::uint8_t, MODBUS_TCP_MAX_ADU_LENGTH> request{};
|
|
||||||
while (running_.load()) {
|
|
||||||
int listener = listen_socket_.load();
|
|
||||||
const int accepted =
|
|
||||||
listener < 0 ? -1 : modbus_tcp_accept(context_, &listener);
|
|
||||||
if (accepted < 0) {
|
|
||||||
if (running_.load()) {
|
|
||||||
std::this_thread::sleep_for(5ms);
|
|
||||||
}
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
client_socket_.store(accepted);
|
|
||||||
while (running_.load()) {
|
|
||||||
const auto request_length =
|
|
||||||
modbus_receive(context_, request.data());
|
|
||||||
if (request_length <= 0) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
if (modbus_reply(context_, request.data(), request_length,
|
|
||||||
mapping_) < 0) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// Treat the mapping as the PLC's published process image.
|
|
||||||
// Read-only FC3 requests must not themselves advance the
|
|
||||||
// seqlock; otherwise the runtime's guard/full/guard snapshot
|
|
||||||
// validation could never observe one stable scan.
|
|
||||||
if (request_length > 7 && request[7] == 0x10U) {
|
|
||||||
processMailbox_();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const auto client = client_socket_.exchange(-1);
|
|
||||||
if (client >= 0) {
|
|
||||||
::close(client);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void processMailbox_()
|
|
||||||
{
|
|
||||||
auto* registers = mapping_->tab_registers;
|
|
||||||
const auto base = plc::axisBase(0);
|
|
||||||
const auto status_base = base + plc::kAxisStatusRelativeOffset;
|
|
||||||
beginSnapshot_(registers + status_base);
|
|
||||||
|
|
||||||
plc::encodeUint32(registers, plc::kPlcHeartbeatOffset,
|
|
||||||
++plc_heartbeat_);
|
|
||||||
const auto session =
|
|
||||||
plc::decodeUint32(registers, plc::kCmvrSessionIdOffset);
|
|
||||||
if (session != observed_session_) {
|
|
||||||
// A new session is not observable as Accepted until the old
|
|
||||||
// stream/mailbox state has been made safe.
|
|
||||||
registers[plc::kOwnerStateOffset] =
|
|
||||||
static_cast<std::uint16_t>(plc::OwnerState::Accepting);
|
|
||||||
observed_session_ = session;
|
|
||||||
active_session_ = session;
|
|
||||||
last_sequence_ = 0;
|
|
||||||
registers[status_base + plc::kStatusFlags] &=
|
|
||||||
static_cast<std::uint16_t>(~plc::StatusFlag::StreamActive);
|
|
||||||
plc::encodeUint32(registers + base, plc::kPayloadSequence, 0);
|
|
||||||
plc::encodeUint32(registers + base,
|
|
||||||
plc::kPayloadSequenceMirror, 0);
|
|
||||||
plc::encodeUint32(registers + base,
|
|
||||||
plc::kCommitSequenceRelativeOffset, 0);
|
|
||||||
plc::encodeUint32(registers, plc::kOwnerSessionIdOffset,
|
|
||||||
active_session_);
|
|
||||||
registers[plc::kOwnerStateOffset] =
|
|
||||||
static_cast<std::uint16_t>(
|
|
||||||
active_session_ == 0 ? plc::OwnerState::None
|
|
||||||
: plc::OwnerState::Accepted);
|
|
||||||
if (active_session_ != 0) {
|
|
||||||
accepted_session_count_.fetch_add(1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (active_session_ == 0 || active_session_ != session) {
|
|
||||||
publishSnapshot_(registers + status_base);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
const auto sequence =
|
|
||||||
plc::decodeUint32(registers + base, plc::kPayloadSequence);
|
|
||||||
const auto sequence_mirror =
|
|
||||||
plc::decodeUint32(registers + base,
|
|
||||||
plc::kPayloadSequenceMirror);
|
|
||||||
const auto commit =
|
|
||||||
plc::decodeUint32(registers + base,
|
|
||||||
plc::kCommitSequenceRelativeOffset);
|
|
||||||
const auto command_session =
|
|
||||||
plc::decodeUint32(registers + base, plc::kCommandSessionId);
|
|
||||||
if (sequence == 0 || sequence == last_sequence_ ||
|
|
||||||
sequence != sequence_mirror || sequence != commit ||
|
|
||||||
command_session != active_session_) {
|
|
||||||
publishSnapshot_(registers + status_base);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
last_sequence_ = sequence;
|
|
||||||
command_count_.fetch_add(1);
|
|
||||||
plc::encodeUint32(registers + status_base, plc::kAckSequence,
|
|
||||||
sequence);
|
|
||||||
plc::encodeUint32(registers + status_base, plc::kActiveSequence,
|
|
||||||
sequence);
|
|
||||||
plc::encodeUint32(registers + status_base, plc::kAckSessionId,
|
|
||||||
active_session_);
|
|
||||||
registers[status_base + plc::kResultCode] =
|
|
||||||
static_cast<std::uint16_t>(plc::ResultCode::Ok);
|
|
||||||
|
|
||||||
const auto code = static_cast<plc::CommandCode>(
|
|
||||||
registers[base + plc::kCommandCode]);
|
|
||||||
const auto target_position =
|
|
||||||
plc::decodeInt32(registers + base, plc::kTargetPosition);
|
|
||||||
const auto target_velocity =
|
|
||||||
plc::decodeInt32(registers + base, plc::kTargetVelocity);
|
|
||||||
auto& flags = registers[status_base + plc::kStatusFlags];
|
|
||||||
auto state = plc::CommandState::Completed;
|
|
||||||
|
|
||||||
switch (code) {
|
|
||||||
case plc::CommandCode::ProfilePosition:
|
|
||||||
profile_position_count_.fetch_add(1);
|
|
||||||
registers[status_base + plc::kCurrentMode] =
|
|
||||||
msgs::RUN_MODE_PROFILE_POSITION;
|
|
||||||
plc::encodeInt32(registers + status_base,
|
|
||||||
plc::kActualPosition, target_position);
|
|
||||||
plc::encodeInt32(registers + status_base,
|
|
||||||
plc::kActualVelocity, 0);
|
|
||||||
plc::encodeInt32(registers + status_base,
|
|
||||||
plc::kTargetPositionStatus, target_position);
|
|
||||||
flags |= plc::StatusFlag::TargetReached;
|
|
||||||
flags &= static_cast<std::uint16_t>(
|
|
||||||
~plc::StatusFlag::QuickStopActive);
|
|
||||||
state = plc::CommandState::TargetReached;
|
|
||||||
break;
|
|
||||||
case plc::CommandCode::ProfileVelocity:
|
|
||||||
profile_velocity_count_.fetch_add(1);
|
|
||||||
registers[status_base + plc::kCurrentMode] =
|
|
||||||
msgs::RUN_MODE_PROFILE_VELOCITY;
|
|
||||||
plc::encodeInt32(registers + status_base,
|
|
||||||
plc::kActualVelocity, target_velocity);
|
|
||||||
plc::encodeInt32(registers + status_base,
|
|
||||||
plc::kTargetVelocityStatus, target_velocity);
|
|
||||||
flags &= static_cast<std::uint16_t>(
|
|
||||||
~plc::StatusFlag::QuickStopActive);
|
|
||||||
state = plc::CommandState::Accepted;
|
|
||||||
break;
|
|
||||||
case plc::CommandCode::OpenCyclicPosition:
|
|
||||||
cyclic_open_count_.fetch_add(1);
|
|
||||||
registers[status_base + plc::kCurrentMode] =
|
|
||||||
msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
|
|
||||||
plc::encodeUint32(
|
|
||||||
registers + status_base,
|
|
||||||
plc::kLastAppliedCyclicSequence, 0);
|
|
||||||
flags |= plc::StatusFlag::StreamActive;
|
|
||||||
flags &= static_cast<std::uint16_t>(
|
|
||||||
~plc::StatusFlag::QuickStopActive);
|
|
||||||
state = plc::CommandState::Accepted;
|
|
||||||
break;
|
|
||||||
case plc::CommandCode::CyclicPositionSample:
|
|
||||||
cyclic_sample_count_.fetch_add(1);
|
|
||||||
plc::encodeInt32(registers + status_base,
|
|
||||||
plc::kActualPosition, target_position);
|
|
||||||
plc::encodeInt32(registers + status_base,
|
|
||||||
plc::kActualVelocity, target_velocity);
|
|
||||||
plc::encodeUint32(
|
|
||||||
registers + status_base,
|
|
||||||
plc::kLastAppliedCyclicSequence,
|
|
||||||
plc::decodeUint32(registers + base,
|
|
||||||
plc::kCyclicSampleSequence));
|
|
||||||
state = plc::CommandState::Accepted;
|
|
||||||
break;
|
|
||||||
case plc::CommandCode::QuickStop:
|
|
||||||
quick_stop_count_.fetch_add(1);
|
|
||||||
plc::encodeInt32(registers + status_base,
|
|
||||||
plc::kActualVelocity, 0);
|
|
||||||
flags |= plc::StatusFlag::QuickStopActive;
|
|
||||||
flags &= static_cast<std::uint16_t>(
|
|
||||||
~plc::StatusFlag::StreamActive);
|
|
||||||
state = plc::CommandState::QuickStopped;
|
|
||||||
break;
|
|
||||||
case plc::CommandCode::Enable:
|
|
||||||
enable_count_.fetch_add(1);
|
|
||||||
flags |= plc::StatusFlag::Enabled;
|
|
||||||
flags &= static_cast<std::uint16_t>(
|
|
||||||
~plc::StatusFlag::QuickStopActive);
|
|
||||||
break;
|
|
||||||
case plc::CommandCode::Disable:
|
|
||||||
flags &= static_cast<std::uint16_t>(
|
|
||||||
~plc::StatusFlag::Enabled);
|
|
||||||
flags &= static_cast<std::uint16_t>(
|
|
||||||
~plc::StatusFlag::StreamActive);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
registers[status_base + plc::kResultCode] =
|
|
||||||
static_cast<std::uint16_t>(
|
|
||||||
plc::ResultCode::Unsupported);
|
|
||||||
state = plc::CommandState::Rejected;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
registers[status_base + plc::kCommandState] =
|
|
||||||
static_cast<std::uint16_t>(state);
|
|
||||||
publishSnapshot_(registers + status_base);
|
|
||||||
}
|
|
||||||
|
|
||||||
void publishSnapshot_(std::uint16_t* status)
|
|
||||||
{
|
|
||||||
if (pending_state_sequence_ == 0U) {
|
|
||||||
beginSnapshot_(status);
|
|
||||||
}
|
|
||||||
plc::encodeUint32(status, plc::kHeartbeatAge, 0);
|
|
||||||
plc::encodeUint32(status, plc::kStateSequenceMirror,
|
|
||||||
pending_state_sequence_);
|
|
||||||
plc::encodeUint32(status, plc::kStateSequence,
|
|
||||||
pending_state_sequence_);
|
|
||||||
state_sequence_ = pending_state_sequence_;
|
|
||||||
pending_state_sequence_ = 0U;
|
|
||||||
}
|
|
||||||
|
|
||||||
void beginSnapshot_(std::uint16_t* status)
|
|
||||||
{
|
|
||||||
auto next = state_sequence_ + 2U;
|
|
||||||
if (next == 0U) {
|
|
||||||
next = 2U;
|
|
||||||
}
|
|
||||||
pending_state_sequence_ = next;
|
|
||||||
plc::encodeUint32(status, plc::kStateSequence, next - 1U);
|
|
||||||
}
|
|
||||||
|
|
||||||
modbus_t* context_{nullptr};
|
|
||||||
modbus_mapping_t* mapping_{nullptr};
|
|
||||||
std::thread worker_;
|
|
||||||
std::atomic<bool> running_{false};
|
|
||||||
std::atomic<int> listen_socket_{-1};
|
|
||||||
std::atomic<int> client_socket_{-1};
|
|
||||||
std::atomic<std::uint32_t> command_count_{0};
|
|
||||||
std::atomic<std::uint32_t> accepted_session_count_{0};
|
|
||||||
std::atomic<std::uint32_t> enable_count_{0};
|
|
||||||
std::atomic<std::uint32_t> profile_position_count_{0};
|
|
||||||
std::atomic<std::uint32_t> profile_velocity_count_{0};
|
|
||||||
std::atomic<std::uint32_t> cyclic_open_count_{0};
|
|
||||||
std::atomic<std::uint32_t> cyclic_sample_count_{0};
|
|
||||||
std::atomic<std::uint32_t> quick_stop_count_{0};
|
|
||||||
std::uint32_t observed_session_{0};
|
|
||||||
std::uint32_t active_session_{0};
|
|
||||||
std::uint32_t plc_heartbeat_{1};
|
|
||||||
std::uint32_t state_sequence_{0};
|
|
||||||
std::uint32_t pending_state_sequence_{0};
|
|
||||||
std::uint32_t last_sequence_{0};
|
|
||||||
std::uint16_t port_{0};
|
|
||||||
};
|
|
||||||
|
|
||||||
std::string writeTemporaryMotorConfig(const std::uint16_t port)
|
|
||||||
{
|
|
||||||
char path[] = "/tmp/cmvr_motor_service_modbus_e2e_XXXXXX.pb.txt";
|
|
||||||
const int descriptor = ::mkstemps(path, 7);
|
|
||||||
if (descriptor < 0) {
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
::close(descriptor);
|
|
||||||
|
|
||||||
std::ofstream output(path, std::ios::out | std::ios::trunc);
|
|
||||||
if (!output.is_open()) {
|
|
||||||
std::remove(path);
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
|
|
||||||
output << "motor {\n"
|
|
||||||
<< " id: \"" << kManagerId << "\"\n"
|
|
||||||
<< " motor_groups {\n"
|
|
||||||
<< " id: \"" << kMotorGroupId << "\"\n"
|
|
||||||
<< " bus_type: MOTOR_BUS_MODBUS_TCP\n"
|
|
||||||
<< " vendor: MOTOR_VENDOR_PLC_GENERIC\n"
|
|
||||||
<< " protocol: MOTOR_PROTOCOL_CMVR_PLC_V1\n"
|
|
||||||
<< " modbus_tcp {\n"
|
|
||||||
<< " host: \"127.0.0.1\"\n"
|
|
||||||
<< " port: " << port << "\n"
|
|
||||||
<< " unit_id: 1\n"
|
|
||||||
<< " connect_timeout_ms: 200\n"
|
|
||||||
<< " io_timeout_ms: 50\n"
|
|
||||||
<< " heartbeat_period_ms: 20\n"
|
|
||||||
<< " communication_watchdog_ms: 300\n"
|
|
||||||
<< " status_poll_period_ms: 2\n"
|
|
||||||
<< " reconnect_min_ms: 10\n"
|
|
||||||
<< " reconnect_max_ms: 50\n"
|
|
||||||
<< " command_ack_timeout_ms: 300\n"
|
|
||||||
<< " cyclic_watchdog_ms: 300\n"
|
|
||||||
<< " protocol_major: 1\n"
|
|
||||||
<< " protocol_minor: 0\n"
|
|
||||||
<< " axes { motor_id: 1 axis_index: 0 }\n"
|
|
||||||
<< " }\n"
|
|
||||||
<< " joint_limits {\n"
|
|
||||||
<< " enable: true\n"
|
|
||||||
<< " source: JOINT_LIMIT_SOURCE_CUSTOM\n"
|
|
||||||
<< " joints {\n"
|
|
||||||
<< " joint_name: \"" << kJointName << "\"\n"
|
|
||||||
<< " q_lb: -2.0\n"
|
|
||||||
<< " q_ub: 2.0\n"
|
|
||||||
<< " qd: 2.0\n"
|
|
||||||
<< " qdd: 4.0\n"
|
|
||||||
<< " }\n"
|
|
||||||
<< " }\n"
|
|
||||||
<< " motors {\n"
|
|
||||||
<< " motors { id: 1 joint_name: \"" << kJointName << "\" }\n"
|
|
||||||
<< " }\n"
|
|
||||||
<< " }\n"
|
|
||||||
<< "}\n";
|
|
||||||
output.close();
|
|
||||||
if (!output) {
|
|
||||||
std::remove(path);
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
return path;
|
|
||||||
}
|
|
||||||
|
|
||||||
class MotorServiceModbusE2eTest : public ::testing::Test {
|
|
||||||
protected:
|
|
||||||
void SetUp() override
|
|
||||||
{
|
|
||||||
device::DeviceManager::destroyInstance();
|
|
||||||
ASSERT_TRUE(plc_.start());
|
|
||||||
|
|
||||||
config_path_ = writeTemporaryMotorConfig(plc_.port());
|
|
||||||
ASSERT_FALSE(config_path_.empty());
|
|
||||||
|
|
||||||
config::MotorRootConfig parsed;
|
|
||||||
ASSERT_TRUE(ConfigHelper::loadConfigFileSilent(config_path_, parsed));
|
|
||||||
ASSERT_EQ(parsed.motor().id(), kManagerId);
|
|
||||||
ASSERT_EQ(parsed.motor().motor_groups_size(), 1);
|
|
||||||
|
|
||||||
config::DeviceManagerConfig device_config;
|
|
||||||
device_config.set_init_all_motors_when_no_active_joints(true);
|
|
||||||
auto* entry = device_config.add_devices();
|
|
||||||
entry->set_id(kManagerId);
|
|
||||||
entry->set_type(
|
|
||||||
config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
|
|
||||||
entry->set_config_file(config_path_);
|
|
||||||
entry->set_enable(true);
|
|
||||||
|
|
||||||
auto& device_manager =
|
|
||||||
device::DeviceManager::getInstance(device_config);
|
|
||||||
manager_ =
|
|
||||||
device_manager.getDevice<device::MotorManager>(kManagerId);
|
|
||||||
ASSERT_NE(manager_, nullptr);
|
|
||||||
ASSERT_NE(manager_->getMotor(1), nullptr);
|
|
||||||
ASSERT_EQ(manager_->getMotor(1)->jointName(), kJointName);
|
|
||||||
|
|
||||||
service_ = std::make_unique<gRPCMotorServiceImpl>();
|
|
||||||
ASSERT_TRUE(startGrpcServer_());
|
|
||||||
}
|
|
||||||
|
|
||||||
void TearDown() override
|
|
||||||
{
|
|
||||||
if (grpc_server_) {
|
|
||||||
grpc_server_->Shutdown();
|
|
||||||
grpc_server_->Wait();
|
|
||||||
grpc_server_.reset();
|
|
||||||
}
|
|
||||||
stub_.reset();
|
|
||||||
service_.reset();
|
|
||||||
|
|
||||||
if (manager_) {
|
|
||||||
manager_->stop();
|
|
||||||
manager_.reset();
|
|
||||||
}
|
|
||||||
device::DeviceManager::destroyInstance();
|
|
||||||
plc_.stop();
|
|
||||||
|
|
||||||
if (!grpc_socket_path_.empty()) {
|
|
||||||
std::remove(grpc_socket_path_.c_str());
|
|
||||||
grpc_socket_path_.clear();
|
|
||||||
}
|
|
||||||
if (!config_path_.empty()) {
|
|
||||||
std::remove(config_path_.c_str());
|
|
||||||
config_path_.clear();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static api::MotorTarget makeTarget_()
|
|
||||||
{
|
|
||||||
api::MotorTarget target;
|
|
||||||
target.mutable_header()->set_device_id(kManagerId);
|
|
||||||
target.set_motor_id(1);
|
|
||||||
return target;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool startGrpcServer_()
|
|
||||||
{
|
|
||||||
grpc_socket_path_ =
|
|
||||||
"/tmp/cmvr_motor_service_modbus_e2e_" +
|
|
||||||
std::to_string(static_cast<long long>(::getpid())) + ".sock";
|
|
||||||
std::remove(grpc_socket_path_.c_str());
|
|
||||||
const std::string server_address = "unix:" + grpc_socket_path_;
|
|
||||||
|
|
||||||
grpc::ServerBuilder builder;
|
|
||||||
builder.AddListeningPort(
|
|
||||||
server_address, grpc::InsecureServerCredentials());
|
|
||||||
builder.RegisterService(service_.get());
|
|
||||||
grpc_server_ = builder.BuildAndStart();
|
|
||||||
if (!grpc_server_) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
stub_ = api::MotorService::NewStub(
|
|
||||||
grpc::CreateChannel(
|
|
||||||
server_address,
|
|
||||||
grpc::InsecureChannelCredentials()));
|
|
||||||
return stub_ != nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
FakeCmvrPlc plc_;
|
|
||||||
std::shared_ptr<device::MotorManager> manager_;
|
|
||||||
std::unique_ptr<gRPCMotorServiceImpl> service_;
|
|
||||||
std::unique_ptr<grpc::Server> grpc_server_;
|
|
||||||
std::unique_ptr<api::MotorService::Stub> stub_;
|
|
||||||
std::string config_path_;
|
|
||||||
std::string grpc_socket_path_;
|
|
||||||
};
|
|
||||||
|
|
||||||
TEST_F(MotorServiceModbusE2eTest,
|
|
||||||
StubTraversesManagerAndModbusRuntimeWithSafetyAndReconnect)
|
|
||||||
{
|
|
||||||
api::SetMotorEnabledRequest enable_request;
|
|
||||||
*enable_request.mutable_target() = makeTarget_();
|
|
||||||
enable_request.set_enabled(true);
|
|
||||||
grpc::ClientContext enable_context;
|
|
||||||
enable_context.set_deadline(
|
|
||||||
std::chrono::system_clock::now() + 3s);
|
|
||||||
api::MotorCommandResponse enable_response;
|
|
||||||
const auto enable_status =
|
|
||||||
stub_->setEnabled(&enable_context, enable_request, &enable_response);
|
|
||||||
ASSERT_TRUE(enable_status.ok()) << enable_status.error_message();
|
|
||||||
ASSERT_TRUE(enable_response.header().success())
|
|
||||||
<< enable_response.header().error_message();
|
|
||||||
EXPECT_EQ(plc_.enableCount(), 1U);
|
|
||||||
|
|
||||||
api::ProfilePositionRequest position_request;
|
|
||||||
*position_request.mutable_target() = makeTarget_();
|
|
||||||
position_request.set_target_position_rad(0.75);
|
|
||||||
position_request.set_max_velocity_rad_s(0.5);
|
|
||||||
position_request.set_acceleration_rad_s2(1.0);
|
|
||||||
position_request.mutable_wait()->set_timeout_ms(1000);
|
|
||||||
position_request.mutable_wait()->set_poll_period_ms(2);
|
|
||||||
position_request.mutable_wait()->set_settle_sample_count(1);
|
|
||||||
grpc::ClientContext position_context;
|
|
||||||
position_context.set_deadline(
|
|
||||||
std::chrono::system_clock::now() + 3s);
|
|
||||||
api::MotorCommandResponse position_response;
|
|
||||||
const auto position_status = stub_->profilePosition(
|
|
||||||
&position_context, position_request, &position_response);
|
|
||||||
ASSERT_TRUE(position_status.ok()) << position_status.error_message();
|
|
||||||
ASSERT_TRUE(position_response.header().success())
|
|
||||||
<< position_response.header().error_message();
|
|
||||||
EXPECT_NEAR(position_response.status().position_rad(), 0.75, 1e-6);
|
|
||||||
EXPECT_TRUE(position_response.status().target_reached());
|
|
||||||
EXPECT_EQ(plc_.profilePositionCount(), 1U);
|
|
||||||
|
|
||||||
api::GetMotorStatusRequest get_status_request;
|
|
||||||
*get_status_request.mutable_target() = makeTarget_();
|
|
||||||
grpc::ClientContext get_status_context;
|
|
||||||
get_status_context.set_deadline(
|
|
||||||
std::chrono::system_clock::now() + 3s);
|
|
||||||
api::GetMotorStatusResponse get_status_response;
|
|
||||||
const auto get_status = stub_->getStatus(
|
|
||||||
&get_status_context, get_status_request, &get_status_response);
|
|
||||||
ASSERT_TRUE(get_status.ok()) << get_status.error_message();
|
|
||||||
ASSERT_TRUE(get_status_response.header().success())
|
|
||||||
<< get_status_response.header().error_message();
|
|
||||||
EXPECT_EQ(get_status_response.status().motor_id(), 1U);
|
|
||||||
EXPECT_EQ(get_status_response.status().joint_name(), kJointName);
|
|
||||||
EXPECT_EQ(get_status_response.status().run_mode(),
|
|
||||||
msgs::RUN_MODE_PROFILE_POSITION);
|
|
||||||
EXPECT_NEAR(get_status_response.status().position_rad(), 0.75, 1e-6);
|
|
||||||
|
|
||||||
api::ProfileVelocityRequest velocity_request;
|
|
||||||
*velocity_request.mutable_target() = makeTarget_();
|
|
||||||
velocity_request.set_target_velocity_rad_s(-0.2);
|
|
||||||
velocity_request.set_acceleration_rad_s2(0.5);
|
|
||||||
velocity_request.mutable_wait()->set_timeout_ms(1000);
|
|
||||||
velocity_request.mutable_wait()->set_poll_period_ms(2);
|
|
||||||
velocity_request.mutable_wait()->set_settle_sample_count(1);
|
|
||||||
grpc::ClientContext velocity_context;
|
|
||||||
velocity_context.set_deadline(
|
|
||||||
std::chrono::system_clock::now() + 3s);
|
|
||||||
api::MotorCommandResponse velocity_response;
|
|
||||||
const auto velocity_status = stub_->profileVelocity(
|
|
||||||
&velocity_context, velocity_request, &velocity_response);
|
|
||||||
ASSERT_TRUE(velocity_status.ok()) << velocity_status.error_message();
|
|
||||||
ASSERT_TRUE(velocity_response.header().success())
|
|
||||||
<< velocity_response.header().error_message();
|
|
||||||
EXPECT_NEAR(velocity_response.status().velocity_rad_s(), -0.2, 1e-6);
|
|
||||||
EXPECT_EQ(plc_.profileVelocityCount(), 1U);
|
|
||||||
|
|
||||||
const auto quick_stops_before_stream = plc_.quickStopCount();
|
|
||||||
grpc::ClientContext stream_context;
|
|
||||||
stream_context.set_deadline(
|
|
||||||
std::chrono::system_clock::now() + 3s);
|
|
||||||
auto stream = stub_->streamCyclicPosition(&stream_context);
|
|
||||||
ASSERT_NE(stream, nullptr);
|
|
||||||
|
|
||||||
api::CyclicPositionRequest open_request;
|
|
||||||
*open_request.mutable_open()->mutable_target() = makeTarget_();
|
|
||||||
open_request.mutable_open()->set_watchdog_timeout_ms(500);
|
|
||||||
ASSERT_TRUE(stream->Write(open_request));
|
|
||||||
|
|
||||||
api::CyclicControlResponse stream_response;
|
|
||||||
ASSERT_TRUE(stream->Read(&stream_response));
|
|
||||||
ASSERT_TRUE(stream_response.header().success())
|
|
||||||
<< stream_response.header().error_message();
|
|
||||||
EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_OPENED);
|
|
||||||
|
|
||||||
api::CyclicPositionRequest setpoint_request;
|
|
||||||
auto* setpoint = setpoint_request.mutable_setpoint();
|
|
||||||
setpoint->set_sequence(1);
|
|
||||||
setpoint->set_target_position_rad(0.25);
|
|
||||||
setpoint->set_target_velocity_rad_s(0.1);
|
|
||||||
ASSERT_TRUE(stream->Write(setpoint_request));
|
|
||||||
|
|
||||||
stream_response.Clear();
|
|
||||||
ASSERT_TRUE(stream->Read(&stream_response));
|
|
||||||
ASSERT_TRUE(stream_response.header().success())
|
|
||||||
<< stream_response.header().error_message();
|
|
||||||
EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_APPLIED);
|
|
||||||
EXPECT_EQ(stream_response.sequence(), 1U);
|
|
||||||
EXPECT_FALSE(stream_response.has_status());
|
|
||||||
EXPECT_EQ(plc_.cyclicOpenCount(), 1U);
|
|
||||||
EXPECT_EQ(plc_.cyclicSampleCount(), 1U);
|
|
||||||
|
|
||||||
ASSERT_TRUE(stream->WritesDone());
|
|
||||||
stream_response.Clear();
|
|
||||||
ASSERT_TRUE(stream->Read(&stream_response));
|
|
||||||
EXPECT_TRUE(stream_response.header().success())
|
|
||||||
<< stream_response.header().error_message();
|
|
||||||
EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_STOPPED);
|
|
||||||
EXPECT_FALSE(stream->Read(&stream_response));
|
|
||||||
const auto stream_finish = stream->Finish();
|
|
||||||
ASSERT_TRUE(stream_finish.ok()) << stream_finish.error_message();
|
|
||||||
EXPECT_GT(plc_.quickStopCount(), quick_stops_before_stream);
|
|
||||||
|
|
||||||
const auto quick_stops_before_emergency = plc_.quickStopCount();
|
|
||||||
api::EmergencyStopRequest emergency_request;
|
|
||||||
*emergency_request.mutable_target() = makeTarget_();
|
|
||||||
grpc::ClientContext emergency_context;
|
|
||||||
emergency_context.set_deadline(
|
|
||||||
std::chrono::system_clock::now() + 3s);
|
|
||||||
api::MotorCommandResponse emergency_response;
|
|
||||||
const auto emergency_status = stub_->emergencyStop(
|
|
||||||
&emergency_context, emergency_request, &emergency_response);
|
|
||||||
ASSERT_TRUE(emergency_status.ok()) << emergency_status.error_message();
|
|
||||||
ASSERT_TRUE(emergency_response.header().success())
|
|
||||||
<< emergency_response.header().error_message();
|
|
||||||
EXPECT_TRUE(emergency_response.status().emergency_stopped());
|
|
||||||
EXPECT_GT(plc_.quickStopCount(), quick_stops_before_emergency);
|
|
||||||
|
|
||||||
const auto commands_before_rejected_motion = plc_.commandCount();
|
|
||||||
api::ProfilePositionRequest rejected_request;
|
|
||||||
*rejected_request.mutable_target() = makeTarget_();
|
|
||||||
rejected_request.set_target_position_rad(0.5);
|
|
||||||
rejected_request.set_max_velocity_rad_s(0.5);
|
|
||||||
rejected_request.set_acceleration_rad_s2(1.0);
|
|
||||||
grpc::ClientContext rejected_context;
|
|
||||||
rejected_context.set_deadline(
|
|
||||||
std::chrono::system_clock::now() + 3s);
|
|
||||||
api::MotorCommandResponse rejected_response;
|
|
||||||
const auto rejected_status = stub_->profilePosition(
|
|
||||||
&rejected_context, rejected_request, &rejected_response);
|
|
||||||
EXPECT_EQ(rejected_status.error_code(),
|
|
||||||
grpc::StatusCode::FAILED_PRECONDITION);
|
|
||||||
EXPECT_FALSE(rejected_response.header().success());
|
|
||||||
EXPECT_EQ(plc_.commandCount(), commands_before_rejected_motion);
|
|
||||||
|
|
||||||
const auto sessions_before_disconnect =
|
|
||||||
plc_.acceptedSessionCount();
|
|
||||||
const auto commands_before_disconnect = plc_.commandCount();
|
|
||||||
plc_.disconnectClient();
|
|
||||||
ASSERT_TRUE(waitUntil(
|
|
||||||
[&] {
|
|
||||||
return plc_.acceptedSessionCount() >
|
|
||||||
sessions_before_disconnect;
|
|
||||||
},
|
|
||||||
3s));
|
|
||||||
std::this_thread::sleep_for(100ms);
|
|
||||||
EXPECT_EQ(plc_.commandCount(), commands_before_disconnect);
|
|
||||||
|
|
||||||
api::SetMotorEnabledRequest reenable_request;
|
|
||||||
*reenable_request.mutable_target() = makeTarget_();
|
|
||||||
reenable_request.set_enabled(true);
|
|
||||||
grpc::ClientContext reenable_context;
|
|
||||||
reenable_context.set_deadline(
|
|
||||||
std::chrono::system_clock::now() + 3s);
|
|
||||||
api::MotorCommandResponse reenable_response;
|
|
||||||
const auto reenable_status = stub_->setEnabled(
|
|
||||||
&reenable_context, reenable_request, &reenable_response);
|
|
||||||
ASSERT_TRUE(reenable_status.ok()) << reenable_status.error_message();
|
|
||||||
EXPECT_TRUE(reenable_response.header().success())
|
|
||||||
<< reenable_response.header().error_message();
|
|
||||||
EXPECT_EQ(plc_.commandCount(), commands_before_disconnect + 1U);
|
|
||||||
EXPECT_FALSE(reenable_response.status().emergency_stopped());
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST_F(MotorServiceModbusE2eTest,
|
|
||||||
ActiveCyclicStreamFailsClosedAcrossReconnectAndNewStreamRecovers)
|
|
||||||
{
|
|
||||||
grpc::ClientContext old_context;
|
|
||||||
old_context.set_deadline(std::chrono::system_clock::now() + 8s);
|
|
||||||
auto old_stream = stub_->streamCyclicPosition(&old_context);
|
|
||||||
ASSERT_NE(old_stream, nullptr);
|
|
||||||
|
|
||||||
api::CyclicPositionRequest open_request;
|
|
||||||
*open_request.mutable_open()->mutable_target() = makeTarget_();
|
|
||||||
open_request.mutable_open()->set_watchdog_timeout_ms(1000);
|
|
||||||
ASSERT_TRUE(old_stream->Write(open_request));
|
|
||||||
|
|
||||||
api::CyclicControlResponse response;
|
|
||||||
ASSERT_TRUE(old_stream->Read(&response));
|
|
||||||
ASSERT_TRUE(response.header().success())
|
|
||||||
<< response.header().error_message();
|
|
||||||
ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED);
|
|
||||||
|
|
||||||
api::CyclicPositionRequest first_sample;
|
|
||||||
first_sample.mutable_setpoint()->set_sequence(1);
|
|
||||||
first_sample.mutable_setpoint()->set_target_position_rad(0.1);
|
|
||||||
first_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0);
|
|
||||||
ASSERT_TRUE(old_stream->Write(first_sample));
|
|
||||||
response.Clear();
|
|
||||||
ASSERT_TRUE(old_stream->Read(&response));
|
|
||||||
ASSERT_TRUE(response.header().success())
|
|
||||||
<< response.header().error_message();
|
|
||||||
ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED);
|
|
||||||
ASSERT_EQ(plc_.cyclicOpenCount(), 1U);
|
|
||||||
ASSERT_EQ(plc_.cyclicSampleCount(), 1U);
|
|
||||||
|
|
||||||
const auto accepted_sessions_before = plc_.acceptedSessionCount();
|
|
||||||
plc_.disconnectClient();
|
|
||||||
ASSERT_TRUE(waitUntil(
|
|
||||||
[&] {
|
|
||||||
return plc_.acceptedSessionCount() >
|
|
||||||
accepted_sessions_before;
|
|
||||||
},
|
|
||||||
3s));
|
|
||||||
|
|
||||||
const auto opens_after_reconnect = plc_.cyclicOpenCount();
|
|
||||||
const auto samples_after_reconnect = plc_.cyclicSampleCount();
|
|
||||||
const auto stops_after_reconnect = plc_.quickStopCount();
|
|
||||||
api::CyclicPositionRequest stale_sample;
|
|
||||||
stale_sample.mutable_setpoint()->set_sequence(2);
|
|
||||||
stale_sample.mutable_setpoint()->set_target_position_rad(0.2);
|
|
||||||
stale_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0);
|
|
||||||
ASSERT_TRUE(old_stream->Write(stale_sample));
|
|
||||||
|
|
||||||
response.Clear();
|
|
||||||
ASSERT_TRUE(old_stream->Read(&response));
|
|
||||||
EXPECT_FALSE(response.header().success());
|
|
||||||
EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_FAILED);
|
|
||||||
EXPECT_NE(response.header().error_message().find(
|
|
||||||
"rejected cyclic position setpoint"),
|
|
||||||
std::string::npos);
|
|
||||||
EXPECT_FALSE(old_stream->Read(&response));
|
|
||||||
const auto old_finish = old_stream->Finish();
|
|
||||||
EXPECT_TRUE(
|
|
||||||
old_finish.error_code() == grpc::StatusCode::FAILED_PRECONDITION ||
|
|
||||||
old_finish.error_code() == grpc::StatusCode::CANCELLED)
|
|
||||||
<< old_finish.error_message();
|
|
||||||
// Stream cleanup may commit a safety QuickStop, but the stale generation
|
|
||||||
// must never commit an Open or sample into the new PLC session.
|
|
||||||
EXPECT_EQ(plc_.cyclicOpenCount(), opens_after_reconnect);
|
|
||||||
EXPECT_EQ(plc_.cyclicSampleCount(), samples_after_reconnect);
|
|
||||||
EXPECT_GT(plc_.quickStopCount(), stops_after_reconnect);
|
|
||||||
|
|
||||||
grpc::ClientContext new_context;
|
|
||||||
new_context.set_deadline(std::chrono::system_clock::now() + 5s);
|
|
||||||
auto new_stream = stub_->streamCyclicPosition(&new_context);
|
|
||||||
ASSERT_NE(new_stream, nullptr);
|
|
||||||
ASSERT_TRUE(new_stream->Write(open_request));
|
|
||||||
|
|
||||||
response.Clear();
|
|
||||||
ASSERT_TRUE(new_stream->Read(&response));
|
|
||||||
ASSERT_TRUE(response.header().success())
|
|
||||||
<< response.header().error_message();
|
|
||||||
ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED);
|
|
||||||
|
|
||||||
api::CyclicPositionRequest fresh_sample;
|
|
||||||
fresh_sample.mutable_setpoint()->set_sequence(1);
|
|
||||||
fresh_sample.mutable_setpoint()->set_target_position_rad(0.3);
|
|
||||||
fresh_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0);
|
|
||||||
ASSERT_TRUE(new_stream->Write(fresh_sample));
|
|
||||||
response.Clear();
|
|
||||||
ASSERT_TRUE(new_stream->Read(&response));
|
|
||||||
ASSERT_TRUE(response.header().success())
|
|
||||||
<< response.header().error_message();
|
|
||||||
EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED);
|
|
||||||
EXPECT_EQ(plc_.cyclicOpenCount(), opens_after_reconnect + 1U);
|
|
||||||
EXPECT_EQ(plc_.cyclicSampleCount(), samples_after_reconnect + 1U);
|
|
||||||
|
|
||||||
ASSERT_TRUE(new_stream->WritesDone());
|
|
||||||
response.Clear();
|
|
||||||
ASSERT_TRUE(new_stream->Read(&response));
|
|
||||||
EXPECT_TRUE(response.header().success())
|
|
||||||
<< response.header().error_message();
|
|
||||||
EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_STOPPED);
|
|
||||||
EXPECT_FALSE(new_stream->Read(&response));
|
|
||||||
const auto new_finish = new_stream->Finish();
|
|
||||||
EXPECT_TRUE(new_finish.ok()) << new_finish.error_message();
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
} // namespace cmvr::service
|
|
||||||
@ -309,7 +309,7 @@ protected:
|
|||||||
std::string grpc_socket_path_;
|
std::string grpc_socket_path_;
|
||||||
};
|
};
|
||||||
|
|
||||||
TEST_F(MotorServiceTest, SetZeroAckLossReportsUnknownOutcome)
|
TEST_F(MotorServiceTest, SetZeroBackendFailureReportsUnknownOutcome)
|
||||||
{
|
{
|
||||||
protocol_->calibrate_success_ = false;
|
protocol_->calibrate_success_ = false;
|
||||||
|
|
||||||
@ -323,7 +323,7 @@ TEST_F(MotorServiceTest, SetZeroAckLossReportsUnknownOutcome)
|
|||||||
EXPECT_FALSE(response.header().success());
|
EXPECT_FALSE(response.header().success());
|
||||||
EXPECT_NE(response.header().error_message().find("outcome unknown"),
|
EXPECT_NE(response.header().error_message().find("outcome unknown"),
|
||||||
std::string::npos);
|
std::string::npos);
|
||||||
EXPECT_NE(response.header().error_message().find("zero_epoch"),
|
EXPECT_NE(response.header().error_message().find("device state"),
|
||||||
std::string::npos);
|
std::string::npos);
|
||||||
EXPECT_EQ(protocol_->quick_stop_count_.load(), 0);
|
EXPECT_EQ(protocol_->quick_stop_count_.load(), 0);
|
||||||
}
|
}
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@ -101,7 +101,7 @@ message SetMotorEnabledRequest {
|
|||||||
|
|
||||||
message CyclicStreamOpen {
|
message CyclicStreamOpen {
|
||||||
MotorTarget target = 1;
|
MotorTarget target = 1;
|
||||||
// The PLC/driver watchdog is authoritative. This service watchdog prevents a
|
// The device/driver watchdog is authoritative. This service watchdog prevents a
|
||||||
// stalled gRPC client from retaining control indefinitely.
|
// stalled gRPC client from retaining control indefinitely.
|
||||||
uint32 watchdog_timeout_ms = 2;
|
uint32 watchdog_timeout_ms = 2;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -76,35 +76,13 @@ message MujocoMotorGroupConfig {
|
|||||||
string world_id = 1;
|
string world_id = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
message ModbusTcpAxisConfig {
|
|
||||||
int32 motor_id = 1;
|
|
||||||
uint32 axis_index = 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
message ModbusTcpConfig {
|
|
||||||
string host = 1;
|
|
||||||
uint32 port = 2;
|
|
||||||
uint32 unit_id = 3;
|
|
||||||
uint32 connect_timeout_ms = 4;
|
|
||||||
uint32 io_timeout_ms = 5;
|
|
||||||
uint32 heartbeat_period_ms = 6;
|
|
||||||
uint32 communication_watchdog_ms = 7;
|
|
||||||
uint32 status_poll_period_ms = 8;
|
|
||||||
uint32 reconnect_min_ms = 9;
|
|
||||||
uint32 reconnect_max_ms = 10;
|
|
||||||
uint32 command_ack_timeout_ms = 11;
|
|
||||||
uint32 protocol_major = 12;
|
|
||||||
uint32 protocol_minor = 13;
|
|
||||||
uint32 cyclic_watchdog_ms = 14;
|
|
||||||
repeated ModbusTcpAxisConfig axes = 20;
|
|
||||||
}
|
|
||||||
|
|
||||||
enum MotorBusType {
|
enum MotorBusType {
|
||||||
MOTOR_BUS_UNKNOWN = 0;
|
MOTOR_BUS_UNKNOWN = 0;
|
||||||
MOTOR_BUS_CAN = 1;
|
MOTOR_BUS_CAN = 1;
|
||||||
MOTOR_BUS_ETHERCAT = 2;
|
MOTOR_BUS_ETHERCAT = 2;
|
||||||
MOTOR_BUS_MUJOCO = 3;
|
MOTOR_BUS_MUJOCO = 3;
|
||||||
MOTOR_BUS_MODBUS_TCP = 4;
|
reserved 4;
|
||||||
|
reserved "MOTOR_BUS_MODBUS_TCP";
|
||||||
}
|
}
|
||||||
|
|
||||||
enum MotorVendor {
|
enum MotorVendor {
|
||||||
@ -112,7 +90,8 @@ enum MotorVendor {
|
|||||||
MOTOR_VENDOR_TI5 = 1;
|
MOTOR_VENDOR_TI5 = 1;
|
||||||
MOTOR_VENDOR_MUJOCO = 2;
|
MOTOR_VENDOR_MUJOCO = 2;
|
||||||
MOTOR_VENDOR_EYOU = 3;
|
MOTOR_VENDOR_EYOU = 3;
|
||||||
MOTOR_VENDOR_PLC_GENERIC = 4;
|
reserved 4;
|
||||||
|
reserved "MOTOR_VENDOR_PLC_GENERIC";
|
||||||
}
|
}
|
||||||
|
|
||||||
enum MotorProtocol {
|
enum MotorProtocol {
|
||||||
@ -120,10 +99,14 @@ enum MotorProtocol {
|
|||||||
MOTOR_PROTOCOL_CANOPEN = 1;
|
MOTOR_PROTOCOL_CANOPEN = 1;
|
||||||
MOTOR_PROTOCOL_ETHERCAT_CIA402 = 2;
|
MOTOR_PROTOCOL_ETHERCAT_CIA402 = 2;
|
||||||
MOTOR_PROTOCOL_MUJOCO = 3;
|
MOTOR_PROTOCOL_MUJOCO = 3;
|
||||||
MOTOR_PROTOCOL_CMVR_PLC_V1 = 4;
|
reserved 4;
|
||||||
|
reserved "MOTOR_PROTOCOL_CMVR_PLC_V1";
|
||||||
}
|
}
|
||||||
|
|
||||||
message MotorGroupConfig {
|
message MotorGroupConfig {
|
||||||
|
reserved 13;
|
||||||
|
reserved "modbus_tcp";
|
||||||
|
|
||||||
string id = 1;
|
string id = 1;
|
||||||
MotorBusType bus_type = 2;
|
MotorBusType bus_type = 2;
|
||||||
MotorVendor vendor = 3;
|
MotorVendor vendor = 3;
|
||||||
@ -133,7 +116,6 @@ message MotorGroupConfig {
|
|||||||
SocketCanConfig can = 10;
|
SocketCanConfig can = 10;
|
||||||
EtherCATConfig ethercat = 11;
|
EtherCATConfig ethercat = 11;
|
||||||
MujocoMotorGroupConfig mujoco = 12;
|
MujocoMotorGroupConfig mujoco = 12;
|
||||||
ModbusTcpConfig modbus_tcp = 13;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
JointLimitsConfig joint_limits = 30;
|
JointLimitsConfig joint_limits = 30;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user