pyrcf.components.ctrl_loop_debuggers.ctrl_loop_data_publisher_base

Classes

CtrlLoopDataStreamConfig	Configurations for data streamer in the simple managed control loop.
CtrlLoopDataPublisherBase	Base class for debuggers that stream the data from all the components in the control loop

Module Contents

class pyrcf.components.ctrl_loop_debuggers.ctrl_loop_data_publisher_base.CtrlLoopDataStreamConfig

Configurations for data streamer in the simple managed control loop.

Get the configuration from the SimpleManagedCtrlLoop object using control_loop.data_streamer_config. Modify the parameters (see below) in this config as required before calling the control_loop.run() method.

NOTE: Try to keep the rate of the publisher (stream_freq) a factor of the rate of the control loop. Otherwise, the RateTrigger will not be able to keep the required rate (because RateLimiter enforces a sleep).

NOTE: Because the publisher uses a custom encoder for encoding PyRCF data types to serialisable data, this debugger can affect the speed of the control loop.

Parameters:

• publish_robot_states (bool) – Defaults to True.

• publish_agent_outputs (bool) – Publish output from each agent in the control loop. (Local plan (if available) and RobotCmd from each agent). Defaults to True.

• publish_global_plan (bool) – Defaults to True.

• publish_robot_cmd (bool) – Defaults to True.

publish_robot_states: bool = True

publish_global_plan: bool = True

publish_agent_outputs: bool = True

Publish output from each agent in the control loop. (Local plan (if available) and RobotCmd from each agent). Defaults to True.

publish_robot_cmd: bool = True

publish_joint_states_comparison: bool = True

If set to True, will publish joint states from different sources (robot state, agent plan outputs, agent control command output, final robot command) in a JointStatesCompareType object, to make it easier to compare values. (see examples/utils_demo/demo_plotjuggler_loop_debugger.py file)

publish_ee_pose_comparison: bool = False

If set to True, will publish End-effector pose data from different sources (robot state (state estimator), agent plan outputs (if ee_reference is present)) using Pose3DCompareType object, to make it easier to compare values. Only publishes poses for end-effectors available in robot_state.state_estimates.ee_states.ee_names. (See examples/utils_demo/demo_plotjuggler_loop_debugger.py file for details on how Pose3DCompareType can be used for other pose objects.)

publish_debug_msg: bool = True

If set to True, will publish custom debug data from provided function handles.

prefix_name: str = 'ctrl_loop'

class pyrcf.components.ctrl_loop_debuggers.ctrl_loop_data_publisher_base.CtrlLoopDataPublisherBase(publisher: pyrcf.utils.data_io_utils.pyrcf_publisher.PyRCFPublisherBase = None, rate: float = None, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock(), debug_publish_callables: Callable[[], Any] | List[Callable[[], Any]] = None)

Bases: pyrcf.components.ctrl_loop_debuggers.ctrl_loop_debugger_base.CtrlLoopDebuggerBase

Base class for debuggers that stream the data from all the components in the control loop using some publisher that can stream strings/bytes. At the moment, supports only zmq and ros publishing.

_publish_data: bool = False

_publisher

_additional_debug_handles = []

_data_streamer_config

property data_streamer_config: CtrlLoopDataStreamConfig

Config for the data stream published using this CtrlLoopDataPublisher debugger.

Change its values before calling the run() method, if you want to modify them.

Returns:

Modifyable config for the data stream published

using this CtrlLoopDataPublisher debugger.

Return type:

CtrlLoopDataStreamConfig

add_debug_handle_to_publish(debug_publish_callables: Callable[[], Any] | List[Callable[[], Any]])

Add additional function handles to publish extra debug data.

See example 03.

Parameters:

debug_publish_callables (Callable[[], Any] | List[Callable[[], Any]]) – Function handle (or list of) that return publishable data (json encodable) for additional streaming to plotjuggler. Use this for debugging components. These method(s)/functions(s) will be called in the control loop and their return value added to the data being published (if publishing is enabled in self.data_streamer_config).

Raises:

ValueError – If invalid value provided as argument.

_run_once_impl(t: float, dt: float, robot_state: pyrcf.core.types.RobotState, global_plan: pyrcf.core.types.GlobalMotionPlan, agent_outputs: List[Tuple[pyrcf.core.types.LocalMotionPlan, pyrcf.core.types.RobotCmd]], robot_cmd: pyrcf.core.types.RobotCmd)

The main method that will be run in every control loop. Should be overridden in the implemented child of CtrlLoopDebuggerBase.

This method will be called automatically if the timer is triggered, as long as the main run_once() method is called continuously in the control loop.

Parameters:

• t (float) – Current time in the control loop.

• dt (float) – The current dt in the control loop.

• robot_state (RobotState) – robot state after update from the state estimator in the control loop.

• global_plan (GlobalMotionPlan) – The global plan output produced by the global planner.

• agent_outputs (List[Tuple[LocalMotionPlan, RobotCmd]]) – The list of outputs from all agents in the control loop.

• robot_cmd (RobotCmd) – The final command written to the robot.

shutdown()

Cleanly shutdown the debugger. Override in child class if required. The base class implements an empty function.