[FEAT] Implement bidirectional transfer of motor positions #39
Description
Context
The RISE exoskeleton requires a lock-free communication mechanism between the ROS Control system and the safety-critical EtherCAT loop (1 kHz). The EtherCAT loop cannot block or wait - any timing violation causes system collapse. Traditional locks/mutexes are unsuitable for this safety-critical environment.
Without this feature: We cannot safely exchange motor position data between ROS and EtherCAT without risking data corruption or timing violations that could lead to exoskeleton failure.
By implementing this feature we will enables the core control loop: Control → ROS Wrapper → EtherCAT → Motors (send path) and Motors → EtherCAT → ROS Wrapper → Control (receive path).
Requirements
Implement the Atomic Pointer Swap Algorithm with the following pseudocode:
Initialization
// Allocate three data buffers
c_mem = allocate_buffer()
a_mem = allocate_buffer()
p_mem = allocate_buffer()
// Initialize pointers
c_p = null
a_p = null
p_p = null
Send Path (ROS → EtherCAT)
Communication Thread (ROS Subscriber Callback):
function on_motor_command_received(new_positions):
// Write new positions to communication buffer
write_to_buffer(c_p, new_positions)
// Atomically swap communication and atomic pointers
atomic_swap(&c_p, &a_p)
Performance Thread (1 kHz EtherCAT Loop):
function ethercat_cycle_send():
// Check if new data is available
if a_p != null:
// Atomically swap performance and atomic pointers
atomic_swap(&p_p, &a_p)
// Copy new positions to IO buffer
memcpy(p_io, p_p, data_size)
// Signal completion
p_p = null
// EtherCAT transmission happens automatically
ethercat_send_frame()
Receive Path (EtherCAT → ROS)
Performance Thread (1 kHz EtherCAT Loop):
function ethercat_cycle_receive():
// EtherCAT reception happens automatically
ethercat_receive_frame()
// Copy actual motor positions from IO buffer
memcpy(p_p, p_io, data_size)
// Make data available to communication thread
atomic_swap(&p_p, &a_p)
Communication Thread (ROS Publisher):
function publish_motor_feedback():
// Check if new data is available
if a_p != null:
// Atomically swap communication and atomic pointers
atomic_swap(&c_p, &a_p)
// Copy to local variable for publishing
local_positions = read_from_buffer(c_p)
// Signal completion
c_p = null
// Publish to ROS
ros_topic.publish(local_positions)
Screenshots, Links, etc.
Detailed documentation: APSA.pdf
Acceptance Criteria
- With APSA we successfully send and receive motor positions
- EtherCAT loop is running stable, maintaining 1kHz
- Control Loop can stop sending motor positions without breaking functionality
- After internal lock the Control Loop starts in sync with EtherCat Loop
- Rise-Motion publishes new motor positions instantaneously