ROS2 Actions#

The WMX R2 application exposes one action server for trajectory execution, compatible with MoveIt2 and any FollowJointTrajectory action client. No custom .action files are defined – the system uses the standard control_msgs action type.

Action Summary#
Action Name	Type	Server Node	Status
`/movensys_manipulator_arm_controller/follow_joint_trajectory`	`control_msgs/action/FollowJointTrajectory`	`joint_trajectory_controller`	Active

FollowJointTrajectory#

Action Name	`/movensys_manipulator_arm_controller/follow_joint_trajectory`
Action Type	`control_msgs/action/FollowJointTrajectory`
Server Node	`joint_trajectory_controller`
Configurable	Name set via `joint_trajectory_action` parameter
Source File	`joint_trajectory_controller.cpp`

This action server receives a joint-space trajectory (a sequence of waypoints with timestamps) and executes it on the physical robot using the WMX AdvancedMotion::StartCSplinePos() cubic spline interpolation engine.

Goal#

The goal uses the standard trajectory_msgs/msg/JointTrajectory message:

Field	Type	Description
`trajectory.joint_names`	`string[]`	Joint names (`joint1` through `joint6`)
`trajectory.points[]`	`JointTrajectoryPoint[]`	Ordered list of waypoints (maximum 1000 points)
`trajectory.points[].positions`	`float64[]`	Target joint positions in radians for each waypoint
`trajectory.points[].time_from_start`	`duration`	Timestamp relative to trajectory start

Note

Only the positions and time_from_start fields of each trajectory point are used. The velocities, accelerations, and effort fields are logged for diagnostics but not passed to the WMX engine – the cubic spline interpolation is computed internally by WMX.

Result#

Field	Type	Description
`error_code`	`int32`	`0` on success; WMX error code on failure
`error_string`	`string`	Not set by the server (default empty)

Feedback#

No intermediate feedback is published during execution. After starting the spline motion, the server runs a polling loop (10 ms interval) that checks each axis’s inPos status via CoreMotion::GetStatus() until all joints have reached position, or until the goal is canceled.

Execution Details#

The server processes the trajectory as follows:

Goal acceptance – All incoming goals are accepted unconditionally (ACCEPT_AND_EXECUTE). No goal validation beyond point count is performed.
Thread dispatch – The execute() callback runs in a detached thread spawned from handle_accepted(), allowing the action server to remain responsive.
Validation – Rejects goals with more than 1000 waypoints (aborts immediately with a warning). If the adjusted point count is 0, the server succeeds immediately without commanding motion.
Timing adjustment – The first point’s time_from_start is forced to zero. If the last point has a time interval less than 1 ms from the previous point, it is dropped to prevent interpolation errors.
Spline construction – For each trajectory point, positions are packed into a CSplinePosData structure and timestamps are converted to milliseconds. The dimensionCount and axis[] array are set from the joint_axes parameter (dimensionCount = jointAxes_.size(), typically 6 joints).
Execution – AdvancedMotion::StartCSplinePos(0, ...) begins the interpolated motion on buffer index 0 across all joints simultaneously.
Poll until done – The server loops every 10 ms, reading CoreMotion::GetStatus() and checking the inPos flag of each axis in joint_axes. The loop exits once all joints are in position.
Result – On success, error_code = 0 and the goal succeeds. On failure, the WMX error code is returned and the goal is aborted.

Note

Cancellation is supported. The handle_cancel() callback accepts the request, and the polling loop checks goal_handle->is_canceling() each iteration – on cancel it calls CoreMotion::Stop() then Wait(), reports error_code = 0, and marks the goal canceled.

Example Usage#

Send a simple two-point trajectory via the command line:

ros2 action send_goal \
  /movensys_manipulator_arm_controller/follow_joint_trajectory \
  control_msgs/action/FollowJointTrajectory \
  "{trajectory: {
    joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'],
    points: [
      {positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
       time_from_start: {sec: 0, nanosec: 0}},
      {positions: [0.5, -0.3, 0.2, 0.0, 0.1, 0.0],
       time_from_start: {sec: 3, nanosec: 0}}
    ]
  }}"

Check that the action server is available:

ros2 action list

Expected:

/movensys_manipulator_arm_controller/follow_joint_trajectory

Inspect the action interface:

ros2 action info /movensys_manipulator_arm_controller/follow_joint_trajectory

Warning

This action moves real motors. Before sending trajectory goals, confirm that the robot parameters have been verified (Robot Parameter Configuration and Validation), that the robot has passed the first-motion procedure (Commissioning and First Motion), that the workspace is clear, and that the separate safety measures in Safety Functions and Responsibility are in place. Cancelling a goal is a controlled stop, not an emergency stop.

MoveIt2 Integration#

MoveIt2 connects to this action server as a trajectory execution controller. The typical workflow is:

MoveIt2 reads the current robot state from /joint_states
The planner computes a collision-free trajectory
MoveIt2 sends the trajectory as a FollowJointTrajectory goal
The joint_trajectory_controller executes it via WMX cubic spline
The joint_state_broadcaster node publishes real-time encoder feedback back to /joint_states at 100 Hz

The action server name must match the controller configuration in MoveIt2. The default name /movensys_manipulator_arm_controller/follow_joint_trajectory is set via the joint_trajectory_action parameter in the config YAML.