CMS Servo controller

Run the CMS MoveIt Servo node. It accepts joint-jog, twist, or pose commands and publishes controller commands and status.

These functions require a live ROS/MoveIt stack and can command robot motion.

src/custom-ur-descriptions/cms_moveit_config/src/servo_node_jazzy.cpp · Source

Motion planning · Guide · Symbol index

How it is used

CMS bringup uses this local MoveIt Servo executable for continuous joint, twist, or pose commands. Clients publish commands or use the pause and command-type services; they do not call its C++ callbacks directly.

main() creates the wrapper and spins its ROS node. The constructor starts servoLoop() on a worker thread; subscription callbacks retain incoming commands for that loop. Follow the loop when investigating stale commands, pauses, or controller output.

Start here

Callable	Purpose
main	Initialize ROS, construct the Servo wrapper, spin it in a multithreaded executor, and shut ROS down after spin returns.
ServoNode::servoLoop	Run the timed control loop, select the pending command type, compute the next state, and publish accepted output and status.
ServoNode::pauseServo	Pause or resume Servo processing and report the resulting state.
ServoNode::switchCommandType	Select a valid Servo command enum and report whether the resulting mode equals the request.

Functions

`main()`

Initialize ROS, construct the Servo wrapper, spin it in a multithreaded executor, and shut ROS down after spin returns.

Entry point · Source

int main(int argc, char** argv)

Parameter	Meaning
`argc`	Number of process command-line arguments.
`argv`	Argument strings passed to rclcpp::init().

Returns: 0 after normal shutdown.

Errors: No local catch around construction or executor spinning.

Side effects: Starts ROS and the Servo worker. The process can command the robot through the configured controller output.

Used by: The CMS Servo executable entry point.

`ServoNode`

Owns the ROS node, planning-scene monitor, Servo instance, and control-loop thread. ROS callbacks update its inputs; the worker computes and publishes controller commands. Destruction requests the worker to stop and joins it.

`ServoNode::servoLoop()`

Run the timed control loop, select the pending command type, compute the next state, and publish accepted output and status.

Internal helper · Source

void ServoNode::servoLoop()

Returns: void when ROS stops or stop_servo_ becomes true.

Errors: INVALID and HALT_FOR_COLLISION states prevent command publication. Pending messages for an unexpected command type are cleared with a warning. No outer exception handler is installed.

Side effects: Waits for the initial robot state, updates smoothing and the command window, and publishes JointTrajectory or Float64MultiArray output plus ServoStatus. While paused it resets smoothing and sleeps.

Used by: The worker thread started by ServoNode().

`ServoNode::pauseServo()`

Pause or resume Servo processing and report the resulting state.

Callback · Source

void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
                           const std::shared_ptr<std_srvs::srv::SetBool::Response>& response)

Parameter	Meaning
`request`	SetBool request: data=true pauses; data=false resumes.
`response`	Response object populated with success and a human-readable state message.

Returns: void; writes response.success and response.message. An already active pause state is reported as success.

Errors: Underlying state reads and Servo operations are not caught locally. Resuming requests a blocking current-state read.

Side effects: Pausing disables collision checking. Resuming refreshes the current robot state, resets smoothing, clears the output rolling window, and restores collision checking. Pending input messages and their new-message flags are retained. Shared pause state changes under the mutex.

Used by: The ~/pause_servo service callback.

`ServoNode::switchCommandType()`

Select a valid Servo command enum and report whether the resulting mode equals the request.

Callback · Source

void ServoNode::switchCommandType(const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request>& request,
                                  const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Response>& response)

Parameter	Meaning
`request`	ServoCommandType request with command_type between CommandType::MIN and MAX.
`response`	Response object whose success field records whether the requested mode became active.

Returns: void; result is written to response.success.

Errors: Out-of-range values log a warning and do not call setCommandType(). No exception recovery is provided.

Side effects: May change which input command type servoLoop() accepts.

Used by: The ~/switch_command_type service callback.

`ServoNode::get_node_base_interface()`

Expose the underlying ROS node interface so an executor can manage this wrapper.

Method · Source

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ServoNode::get_node_base_interface()

Returns: The node’s shared NodeBaseInterface pointer.

Side effects: Does not start or stop the executor.

Used by: main() passes the returned interface to MultiThreadedExecutor::add_node().

`ServoNode::~ServoNode()`

Request that the servo loop stop, then join its worker thread if it is joinable.

Lifecycle · Source

ServoNode::~ServoNode()

Returns: Destructor; no returned value.

Errors: No local recovery around thread joining.

Side effects: Sets stop_servo_ and blocks until the worker thread exits.

Used by: Object destruction after the owning shared pointer is released.

`ServoNode::ServoNode()`

Create the ROS node, planning-scene monitor, Servo instance, command subscriptions, output publishers, control services, and worker thread.

Lifecycle · Source

ServoNode::ServoNode(const rclcpp::NodeOptions& options)

Parameter	Meaning
`options`	ROS node options passed to the underlying node named servo_node.

Returns: Constructs a ServoNode instance.

Errors: Failure to obtain FIFO scheduling or a realtime kernel logs a warning and continues. ROS, parameter, planning-scene, and thread construction errors are not caught here.

Side effects: Creates ROS entities, attempts realtime scheduling, and starts servoLoop() on a separate thread. The active Servo stack can publish motion commands.

Used by: main() constructs the node before spinning the executor.

Internal callbacks and helpers

`ServoNode::jointJogCallback()`

Retain the newest command for processing by the servo loop.

Callback · Source

void ServoNode::jointJogCallback(const control_msgs::msg::JointJog::ConstSharedPtr& msg)

Parameter	Meaning
`msg`	JointJog message containing joint names, velocities, optional displacements, and a timestamp.

Returns: void.

Errors: No validation or local exception handling in the callback.

Side effects: Copies the message into latest_joint_jog_ and sets new_joint_jog_msg_. The callback itself does not compute or publish a trajectory.

Used by: The corresponding subscription created by ServoNode().

`ServoNode::twistCallback()`

Retain the newest command for processing by the servo loop.

Callback · Source

void ServoNode::twistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg)

Parameter	Meaning
`msg`	TwistStamped message with linear/angular velocity, command frame, and timestamp.

Returns: void.

Errors: No validation or local exception handling in the callback.

Side effects: Copies the message into latest_twist_ and sets new_twist_msg_. The callback itself does not compute or publish a trajectory.

Used by: The corresponding subscription created by ServoNode().

`ServoNode::poseCallback()`

Retain the newest command for processing by the servo loop.

Callback · Source

void ServoNode::poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg)

Parameter	Meaning
`msg`	PoseStamped target with position, quaternion orientation, frame, and timestamp.

Returns: void.

Errors: No validation or local exception handling in the callback.

Side effects: Copies the message into latest_pose_ and sets new_pose_msg_. The callback itself does not compute or publish a trajectory.

Used by: The corresponding subscription created by ServoNode().

`ServoNode::processJointJogCommand()`

Convert a fresh joint-jog command into the next Servo state, or request a smooth halt for stale input.

Internal helper · Source

std::optional<KinematicState> ServoNode::processJointJogCommand(const moveit::core::RobotStatePtr& robot_state)

Parameter	Meaning
`robot_state`	MoveIt robot state used as the starting state for the next Servo calculation.

Returns: An optional KinematicState. Fresh input returns the Servo result; stale input returns a halt state only while smoothHalt() reports that stopping is still in progress.

Errors: Displacements are warned about and cleared; they are not executed. INVALID Servo status clears new_joint_jog_msg_. Staleness uses incoming_command_timeout. Exceptions from Servo or timestamp operations are not caught.

Side effects: Clears pending flags for the other command types and updates the active command flag. It computes a state; publication happens in servoLoop().

Used by: servoLoop(), after it checks the selected command type.

`ServoNode::processTwistCommand()`

Convert a fresh twist command into the next Servo state, or request a smooth halt for stale input.

Internal helper · Source

std::optional<KinematicState> ServoNode::processTwistCommand(const moveit::core::RobotStatePtr& robot_state)

Parameter	Meaning
`robot_state`	MoveIt robot state used as the starting state for the next Servo calculation.

Returns: An optional KinematicState. Fresh input returns the Servo result; stale input returns a halt state only while smoothHalt() reports that stopping is still in progress.

Errors: INVALID Servo status clears new_twist_msg_. Staleness uses incoming_command_timeout. Exceptions from Servo or timestamp operations are not caught.

Side effects: Clears pending flags for the other command types and updates the active command flag. It computes a state; publication happens in servoLoop().

Used by: servoLoop(), after it checks the selected command type.

`ServoNode::processPoseCommand()`

Convert a fresh pose command into the next Servo state, or request a smooth halt for stale input.

Internal helper · Source

std::optional<KinematicState> ServoNode::processPoseCommand(const moveit::core::RobotStatePtr& robot_state)

Parameter	Meaning
`robot_state`	MoveIt robot state used as the starting state for the next Servo calculation.

Returns: An optional KinematicState. Fresh input returns the Servo result; stale input returns a halt state only while smoothHalt() reports that stopping is still in progress.

Errors: INVALID Servo status clears new_pose_msg_. Staleness uses incoming_command_timeout. Exceptions from Servo or timestamp operations are not caught.

Side effects: Clears pending flags for the other command types and updates the active command flag. It computes a state; publication happens in servoLoop().

Used by: servoLoop(), after it checks the selected command type.

CMS Servo controller

How it is used

Start here

Functions

main()

ServoNode

ServoNode::servoLoop()

ServoNode::pauseServo()

ServoNode::switchCommandType()

ServoNode::get_node_base_interface()

ServoNode::~ServoNode()

ServoNode::ServoNode()

Internal callbacks and helpers

ServoNode::jointJogCallback()

ServoNode::twistCallback()

ServoNode::poseCallback()

ServoNode::processJointJogCommand()

ServoNode::processTwistCommand()

ServoNode::processPoseCommand()

`main()`

`ServoNode`

`ServoNode::servoLoop()`

`ServoNode::pauseServo()`

`ServoNode::switchCommandType()`

`ServoNode::get_node_base_interface()`

`ServoNode::~ServoNode()`

`ServoNode::ServoNode()`

`ServoNode::jointJogCallback()`

`ServoNode::twistCallback()`

`ServoNode::poseCallback()`

`ServoNode::processJointJogCommand()`

`ServoNode::processTwistCommand()`

`ServoNode::processPoseCommand()`