controllers_ros2

ROS 2 controller interfaces and helpers.

This module defines ROS 2-backed controller wrappers used by RobotBlockSet robot interfaces. It provides controller-manager helpers, joint-trajectory interfaces, and Cartesian impedance command interfaces through ROS 2 topics, actions, and services.

class robotblockset.ros2.controllers_ros2.controller_manager_helper(node: rclpy.node.Node, namespace: str = '', controller_manager_name: str | None = 'controller_manager')[source]

Bases: object

Helper for ROS 2 controller-manager lifecycle operations.

High-level lifecycle transitions:

load(name) -> ensure controller plugin instance exists (UNCONFIGURED) configure(name) -> UNCONFIGURED -> INACTIVE activate(name) -> INACTIVE -> ACTIVE (via switch_controllers) deactivate(name) -> ACTIVE -> INACTIVE unload(name) -> remove controller instance (must not be ACTIVE)

Legacy wrappers:

load_controller(name) -> load + configure start_controller(name) -> activate stop_controller(name) -> deactivate switch_controllers(start, stop) -> activate/deactivate

Utilities:

list_controllers(), get_state(name), is_active(name)

Attributes:

  • cm_name (str) – Name of the controller-manager namespace.

  • list_client (Any) – Service client for listing controllers.

  • switch_client (Any) – Service client for switching controllers.

  • load_client (Any) – Service client for loading controllers.

  • unload_client (Any) – Service client for unloading controllers.

  • configure_client (Any) – Service client for configuring controllers.

Initialize the controller-manager helper.

Parameters:

  • node (Node) – The ROS 2 node used to create service clients. The caller is responsible for spinning this node.

  • namespace (str, optional) – Namespace for the controller manager.

  • controller_manager_name (str, optional) – Name of the ROS 2 controller manager.

__init__(node: rclpy.node.Node, namespace: str = '', controller_manager_name: str | None = 'controller_manager') → None[source]

Initialize the controller-manager helper.

Parameters:

  • node (Node) – The ROS 2 node used to create service clients. The caller is responsible for spinning this node.

  • namespace (str, optional) – Namespace for the controller manager.

  • controller_manager_name (str, optional) – Name of the ROS 2 controller manager.

list_controllers() → List[controller_manager_msgs.msg.ControllerState][source]

Query the controller_manager for all available controllers.

Returns:

A list of controller state objects. If the service call fails, an empty list is returned and an error is logged.

Return type:

list of controller_manager_msgs.msg.ControllerState

get_state(name: str) → str | None[source]

Get the lifecycle state of a specific controller.

Parameters:

name (str) – Name of the controller.

Returns:

The controller state string (e.g. "active", "inactive"), or None if the controller is not found.

Return type:

str or None

is_active(name: str) → bool[source]

Check whether a controller is in the "active" state.

Parameters:

name (str) – Name of the controller.

Returns:

True if the controller exists and is active, otherwise False.

Return type:

bool

load(name: str) → bool[source]

Load a controller by name.

This requests that the controller_manager loads the specified controller plugin so that it can be configured and activated later.

Parameters:

name (str) – Name of the controller to load.

Returns:

True if the controller is already loaded or was loaded successfully, False if the load request failed.

Return type:

bool

configure(name: str) → bool[source]

Configure a controller, loading it first if necessary.

Parameters:

name (str) – Name of the controller to configure.

Returns:

True if the controller is already configured ("inactive" or "active") or was configured successfully, otherwise False.

Return type:

bool

activate(name: str) → bool[source]

Activate a controller.

Ensures the controller is loaded and configured, then requests the controller_manager to start (activate) it.

Parameters:

name (str) – Name of the controller to activate.

Returns:

True if the controller is already active or was activated successfully, False otherwise.

Return type:

bool

deactivate(name: str) → bool[source]

Deactivate (stop) a controller.

Parameters:

name (str) – Name of the controller to deactivate.

Returns:

True if the controller is already not active or was deactivated successfully, False otherwise.

Return type:

bool

unload(name: str) → bool[source]

Unload a controller.

The controller must not be active when unloading. If it is active, this function first attempts to deactivate it.

Parameters:

name (str) – Name of the controller to unload.

Returns:

True if the controller is already absent or was unloaded successfully, False if deactivation or unload failed.

Return type:

bool

start_controller(name: str) → bool[source]

Backwards-compatible wrapper to start (activate) a controller.

Parameters:

name (str) – Name of the controller to start.

Returns:

True if activation succeeded or the controller is already active, False otherwise.

Return type:

bool

load_controller(name: str) → bool[source]

Backwards-compatible wrapper to load and configure a controller.

Parameters:

name (str) – Name of the controller to load and configure.

Returns:

True if the controller is loaded and configured, False otherwise.

Return type:

bool

stop_controller(name: str) → bool[source]

Backwards-compatible wrapper to stop (deactivate) a controller.

Parameters:

name (str) – Name of the controller to stop.

Returns:

True if the controller is not active or was deactivated successfully, False otherwise.

Return type:

bool

switch_controllers(start_list: Iterable[str], stop_list: Iterable[str]) → bool[source]

Switch sets of controllers in a single operation.

Parameters:

  • start_list (Iterable[str]) – Names of controllers to activate.

  • stop_list (Iterable[str]) – Names of controllers to deactivate.

Returns:

True if the switch request succeeded, False otherwise.

Return type:

bool

get_active_controller(controller_names: Iterable[str] | None = None) → str | None[source]

Get the name of the first active controller from a candidate list.

Parameters:

controller_names (Iterable[str], optional) – Iterable of controller names to check. If None, all known controllers are considered.

Returns:

Name of the first active controller from the list, or None if none of them are active.

Return type:

str or None

class robotblockset.ros2.controllers_ros2.RosControllerInterface[source]

Bases: rbs_object

Generic interface for a ROS2-based controller.

This class defines a skeleton for ROS2 controller interfaces used within Robotics Behavior Specification (RBS) systems. Derived classes must implement the _init_ros_interfaces method to create all ROS-specific communication elements such as publishers, subscribers, actions, or services.

Attributes:

  • motion_result (int | None) – Last motion result code reported by the controller interface.

  • motion_done (bool | None) – Completion flag for the active motion command.

  • _robot (robot) – RobotBlockSet robot instance bound during activation.

  • _node (Node) – ROS 2 node used to create communication entities.

Initialize a RosControllerInterface instance.

Notes

  • This method only initializes the rbs_object base class.

  • ROS interfaces are initialized later via Activate().

__init__() → None[source]

Initialize a RosControllerInterface instance.

Notes

  • This method only initializes the rbs_object base class.

  • ROS interfaces are initialized later via Activate().

Activate(robot: robot, node: rclpy.node.Node) → None[source]

Activate the controller using the specified robot and ROS node.

This method initializes the controller with data from the robot, registers ROS interfaces, and marks the controller as active.

Parameters:

  • robot (robot) – The robot instance providing joint and state data.

  • node (Node) – The ROS2 node used to create publishers, subscribers, or action clients.

Return type:

None

Notes

  • This method should be called before issuing any control commands.

  • Derived classes typically call _init_ros_interfaces() to build ROS2 entities.

class robotblockset.ros2.controllers_ros2.CsfCartesianImpedanceControllerInterface(robot: robot | None = None, ros_plugin_name: str = 'cartesian_impedance_controller', namespace: str = '', topic: str = 'cartesian_command', Kp: ndarray = array([2000.0000, 2000.0000, 2000.0000]), Kr: ndarray = array([30.0000, 30.0000, 30.0000]), R: ndarray = array([[1.0000, 0.0000, 0.0000], [0.0000, 1.0000, 0.0000], [0.0000, 0.0000, 1.0000]]), D: float = 2.0, command_frame: str = '')[source]

Bases: RosControllerInterface

Interface for a Cartesian impedance controller.

Attributes:

  • Kp (np.ndarray) – Cartesian translational stiffness vector.

  • Kr (np.ndarray) – Cartesian rotational stiffness vector.

  • R (np.ndarray) – Rotation matrix used to orient the translational stiffness frame.

  • D (float) – Damping scaling factor.

Initialize the Cartesian impedance controller interface.

Parameters:

  • robot (robot, optional) – Reserved robot argument kept for compatibility.

  • ros_plugin_name (str, optional) – Name of the ROS 2 controller plugin.

  • namespace (str, optional) – Namespace used for the controller topics.

  • topic (str, optional) – Cartesian command topic name.

  • Kp (np.ndarray, optional) – Translational stiffness gains.

  • Kr (np.ndarray, optional) – Rotational stiffness gains.

  • R (np.ndarray, optional) – Rotation matrix of the translational stiffness frame.

  • D (float, optional) – Damping scaling factor.

__init__(robot: robot | None = None, ros_plugin_name: str = 'cartesian_impedance_controller', namespace: str = '', topic: str = 'cartesian_command', Kp: ndarray = array([2000.0000, 2000.0000, 2000.0000]), Kr: ndarray = array([30.0000, 30.0000, 30.0000]), R: ndarray = array([[1.0000, 0.0000, 0.0000], [0.0000, 1.0000, 0.0000], [0.0000, 0.0000, 1.0000]]), D: float = 2.0, command_frame: str = '') → None[source]

Initialize the Cartesian impedance controller interface.

Parameters:

  • robot (robot, optional) – Reserved robot argument kept for compatibility.

  • ros_plugin_name (str, optional) – Name of the ROS 2 controller plugin.

  • namespace (str, optional) – Namespace used for the controller topics.

  • topic (str, optional) – Cartesian command topic name.

  • Kp (np.ndarray, optional) – Translational stiffness gains.

  • Kr (np.ndarray, optional) – Rotational stiffness gains.

  • R (np.ndarray, optional) – Rotation matrix of the translational stiffness frame.

  • D (float, optional) – Damping scaling factor.

GetCartesianCompliance() → tuple[ndarray, ndarray, ndarray, float][source]

Return the active Cartesian impedance parameters.

GetCartesianStiffness() → tuple[ndarray, ndarray][source]

Return translational and rotational Cartesian stiffness.

GetCartesianDamping() → float[source]

Return the Cartesian damping scaling factor.

SetCartesianCompliance(Kp: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, Kr: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, R: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, D: float | None = None, hold_pose: bool = True) → int[source]

Update Cartesian impedance parameters for subsequent CSF commands.

SetCartesianStiffness(Kp: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, Kr: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, hold_pose: bool = True) → int[source]

Update Cartesian stiffness while preserving orientation frame and damping.

SetCartesianDamping(D: float | None = None, hold_pose: bool = True) → int[source]

Update Cartesian damping while preserving stiffness.

SetCartesianSoft(stiffness: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...], hold_pose: bool = True) → int[source]

Scale Cartesian stiffness relative to this controller’s configured defaults.

SetCartesianStiff(hold_pose: bool = True) → int[source]

Set Cartesian impedance to the configured stiff defaults.

SetCartesianCompliant(hold_pose: bool = True) → int[source]

Set Cartesian impedance to zero stiffness.

GoTo_X(x: ndarray, xdot: ndarray, trq: ndarray, wait: ndarray, Kp: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, Kr: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, R: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] | None = None, D: float | None = None, **kwargs: Any) → int[source]

Publish a Cartesian impedance command.

Parameters:

  • x (Pose3DType) – Desired task pose represented as position and quaternion.

  • xdot (Velocity3DType) – Desired task twist.

  • trq (WrenchType) – Desired feed-forward wrench.

  • wait (TimesType) – Kept for API compatibility; not used by the ROS 2 publisher.

  • Kp (ArrayLike, optional) – Translational stiffness vector.

  • Kr (ArrayLike, optional) – Rotational stiffness vector.

  • R (ArrayLike, optional) – Rotation matrix of the translational stiffness frame.

  • D (float, optional) – Damping scaling factor.

Returns:

Motion result code.

Return type:

int

Notes

The method updates the stored compliance parameters with the values used for the published command.

class robotblockset.ros2.controllers_ros2.JointPositionControllerInterface(namespace: str = '', ros_plugin_name: str = 'command', topic: str = 'joint_position')[source]

Bases: RosControllerInterface

Interface for a joint-position controller.

Attributes:

  • _namespace (str) – Namespace used for the controller topics.

  • _ros_plugin_name (str) – Name of the ROS 2 controller plugin.

  • _topic (str) – Topic used to publish joint commands.

  • _use_publisher (bool) – Indicates whether a publisher interface is active.

Initialize the joint-position controller interface.

This class provides a thin wrapper around a ROS 2 controller that accepts single-step joint commands through a trajectory_msgs/JointTrajectory topic. It is used for direct joint position setpoints without action goal management.

Parameters:

  • namespace (str, optional) – Namespace prefix for the controller (for example "arm"). If empty, no namespace prefix is used.

  • ros_plugin_name (str, optional) – Name of the controller plugin (e.g. "command"). If empty or None, the controller is assumed to be directly under the namespace (no extra name component is added).

  • topic (str, optional) – Topic name used when publishing joint-position commands.

Notes

  • Effective topic names are constructed as <namespace>/<ros_plugin_name>/<topic> or <namespace>/<topic>.

  • The actual ROS node is provided later through RosControllerInterface.Activate().

__init__(namespace: str = '', ros_plugin_name: str = 'command', topic: str = 'joint_position') → None[source]

Initialize the joint-position controller interface.

This class provides a thin wrapper around a ROS 2 controller that accepts single-step joint commands through a trajectory_msgs/JointTrajectory topic. It is used for direct joint position setpoints without action goal management.

Parameters:

  • namespace (str, optional) – Namespace prefix for the controller (for example "arm"). If empty, no namespace prefix is used.

  • ros_plugin_name (str, optional) – Name of the controller plugin (e.g. "command"). If empty or None, the controller is assumed to be directly under the namespace (no extra name component is added).

  • topic (str, optional) – Topic name used when publishing joint-position commands.

Notes

  • Effective topic names are constructed as <namespace>/<ros_plugin_name>/<topic> or <namespace>/<topic>.

  • The actual ROS node is provided later through RosControllerInterface.Activate().

GoTo_q(q: ndarray, qdot: ndarray | None = None, trq: ndarray | None = None, wait: float | None = None, **kwargs: Any) → int[source]

Publish a joint-position command.

Parameters:

  • q (JointConfigurationType) – Desired joint positions (nj,).

  • qdot (JointVelocityType, optional) – Desired joint velocities (nj,).

  • trq (JointTorqueType, optional) – Desired joint torques (nj,).

  • wait (float, optional) – Kept for API compatibility; not used by the publisher interface.

Returns:

Motion result code.

Return type:

int

Raises:

ValueError – If control is not implemented in controller.

class robotblockset.ros2.controllers_ros2.JointTrajectoryControllerInterface(namespace: str = '', ros_plugin_name: str = 'joint_trajectory_controller', topic: str = 'joint_trajectory', action: str = 'follow_joint_trajectory')[source]

Bases: RosControllerInterface

Interface for a joint-trajectory controller.

Attributes:

  • _namespace (str) – Namespace used for the controller topics and actions.

  • _ros_plugin_name (str) – Name of the ROS 2 controller plugin.

  • _topic (str) – Topic used for trajectory publishing.

  • _action (str) – Action used for trajectory execution.

  • _use_publisher (bool) – Indicates whether the publisher path is active.

  • _use_action (bool) – Indicates whether the action-client path is active.

Initialize the joint-trajectory controller interface.

This class provides a thin wrapper around a ROS 2 controller manager plugin exposing a control_msgs/FollowJointTrajectory action or a trajectory_msgs/JointTrajectory topic. It can be configured to either communicate via an action client or a plain publisher.

Parameters:

  • namespace (str, optional) – Namespace prefix for the controller (for example "arm"). If empty, no namespace prefix is used.

  • ros_plugin_name (str, optional) – Name of the controller plugin (e.g. "joint_trajectory_controller"). If empty or None, the controller is assumed to be directly under the namespace (no extra name component is added).

  • topic (str, optional) – Topic name used when publishing JointTrajectory commands.

  • action (str, optional) – Action name used when creating a FollowJointTrajectory action client. If not None, this path is preferred over publishing.

Notes

  • Effective topic/action names are constructed as:

    • <namespace>/<ros_plugin_name>/<action> or <namespace>/<action>

    • <namespace>/<ros_plugin_name>/<topic> or <namespace>/<topic>

  • The actual ROS node is provided later through RosControllerInterface.Activate().

__init__(namespace: str = '', ros_plugin_name: str = 'joint_trajectory_controller', topic: str = 'joint_trajectory', action: str = 'follow_joint_trajectory') → None[source]

Initialize the joint-trajectory controller interface.

This class provides a thin wrapper around a ROS 2 controller manager plugin exposing a control_msgs/FollowJointTrajectory action or a trajectory_msgs/JointTrajectory topic. It can be configured to either communicate via an action client or a plain publisher.

Parameters:

  • namespace (str, optional) – Namespace prefix for the controller (for example "arm"). If empty, no namespace prefix is used.

  • ros_plugin_name (str, optional) – Name of the controller plugin (e.g. "joint_trajectory_controller"). If empty or None, the controller is assumed to be directly under the namespace (no extra name component is added).

  • topic (str, optional) – Topic name used when publishing JointTrajectory commands.

  • action (str, optional) – Action name used when creating a FollowJointTrajectory action client. If not None, this path is preferred over publishing.

Notes

  • Effective topic/action names are constructed as:

    • <namespace>/<ros_plugin_name>/<action> or <namespace>/<action>

    • <namespace>/<ros_plugin_name>/<topic> or <namespace>/<topic>

  • The actual ROS node is provided later through RosControllerInterface.Activate().

GoTo_qtraj(q: ndarray, qdot: ndarray, qddot: ndarray, time: ndarray) → int[source]

Command the robot to follow a joint trajectory.

Parameters:

  • q (JointPathType) – Desired joint positions for the trajectory (n, nj), where n is the number of trajectory points.

  • qdot (JointPathType) – Desired joint velocities for the trajectory (n, nj), where n is the number of trajectory points.

  • qddot (JointPathType) – Desired joint accelerations for the trajectory. The current interfaces do not use these values.

  • time (TimesType) – Time points for the trajectory (n,).

Returns:

Motion result code.

Return type:

int

Notes

The command is routed to either the publisher-based or action-based trajectory backend depending on the active ROS 2 interface.

GoTo_qtraj_pub(q: ndarray, qdot: ndarray, qddot: ndarray, time: ndarray) → int[source]

Publish a joint trajectory through the ROS 2 topic interface.

Parameters:

  • q (JointPathType) – Desired joint positions for the trajectory (n, nj), where n is the number of trajectory points.

  • qdot (JointPathType) – Desired joint velocities for the trajectory (n, nj), where n is the number of trajectory points.

  • qddot (JointPathType) – Desired joint accelerations for the trajectory. The publisher path does not use these values.

  • time (TimesType) – Time points for the trajectory (n,).

Returns:

Motion result code.

Return type:

int

Notes

On success, the final trajectory point is copied into the robot command state and the motion is marked complete immediately after publishing.

GoTo_qtraj_act(q: ndarray, qdot: ndarray, qddot: ndarray, time: ndarray) → int[source]

Send a joint trajectory through the ROS 2 action interface.

Parameters:

  • q (JointPathType) – Desired joint positions for the trajectory (n, nj), where n is the number of trajectory points.

  • qdot (JointPathType) – Desired joint velocities for the trajectory (n, nj), where n is the number of trajectory points.

  • qddot (JointPathType) – Desired joint accelerations for the trajectory. The action path does not use these values.

  • time (TimesType) – Time points for the trajectory (n,).

Returns:

Motion result code.

Return type:

int

Notes

The method waits for the action response and, when enabled, for the final motion-completion callback.

qtraj_goal_response_callback(future: rclpy.task.Future) → None[source]

Callback executed when the action server responds to a trajectory goal request.

Parameters:

future (Future) – Future containing the ClientGoalHandle result from the send_goal_async call.

Notes

  • If the goal is rejected, motion_result is set to INVALID_GOAL.

  • If the goal is accepted, the completion callback is registered through get_result_async.

qtraj_get_result_callback(future: rclpy.task.Future) → None[source]

Callback executed when the trajectory has finished execution.

Parameters:

future (Future) – Future containing the result of the action execution.

Notes

  • Updates motion_result with the final action result error code.

  • Sets motion_done to True indicating motion completion.

  • Logs success or error based on trajectory outcome.

  • If motion was aborted, sets _abort = True on the robot instance (if available).

Return type:

None

qtraj_feedback_callback(feedback_msg: control_msgs.action.FollowJointTrajectory.Feedback) → None[source]

Callback executed when feedback is received during trajectory execution.

Parameters:

feedback_msg (FollowJointTrajectory.Feedback) – Feedback message containing intermediate trajectory updates.

Notes

  • Stores feedback data in last_feedback.

  • Feedback may include joint states and progress updates.

  • Logging is optional and typically kept disabled for performance reasons.

Return type:

None

abort_motion() → int | None[source]

Attempt to abort the current trajectory motion by canceling the active FollowJointTrajectory goal.

This method sends a cancel request to the ROS2 action server for the active trajectory goal. If no goal is currently active, a warning is logged and the method exits without performing any action.

Notes

  • This does not immediately stop the robot; it requests the controller to perform a controlled stop.

  • After the cancel request is sent, the result is processed in qtraj_cancel_done_callback().

Returns:

Motion result code of the cancel operation, or None if no active goal exists.

Return type:

int | None

qtraj_cancel_done_callback(future: rclpy.task.Future) → None[source]

Process the result of a cancel request for an active trajectory goal.

Parameters:

future (Future) – Future object returned from cancel_goal_async containing the cancel response.

Notes

  • Logs whether cancellation was successful.

  • Does not raise exceptions; any failure is logged.

  • This callback does not wait for the robot to fully stop; it only indicates whether the cancel request was accepted.

static FollowJointTrajectory_error_str(error_code: int) → str[source]

Convert a FollowJointTrajectory result code to text.

Parameters:

error_code (int) – Error code from FollowJointTrajectory action result.

Returns:

Human-readable explanation of the failure cause.

Return type:

str

static FollowJointTrajectory_goal_status_str(status_code: int) → str[source]

Convert an action goal status code to text.

Parameters:

status_code (int) – Action goal execution status.

Returns:

Human-readable status description.

Return type:

str

Classes

CsfCartesianImpedanceControllerInterface([...])

Interface for a Cartesian impedance controller.

JointPositionControllerInterface([...])

Interface for a joint-position controller.

JointTrajectoryControllerInterface([...])

Interface for a joint-trajectory controller.

RosControllerInterface()

Generic interface for a ROS2-based controller.

controller_manager_helper(node[, namespace, ...])

Helper for ROS 2 controller-manager lifecycle operations.