pyrcf.control_loop ================== .. py:module:: pyrcf.control_loop .. autoapi-nested-parse:: Ctrl Loop runners for running the pyrcf components in a control loop. Submodules ---------- .. toctree:: :maxdepth: 1 /autoapi/pyrcf/control_loop/minimal_ctrl_loop/index /autoapi/pyrcf/control_loop/simple_managed_ctrl_loop/index Classes ------- .. autoapisummary:: pyrcf.control_loop.SimpleManagedCtrlLoop pyrcf.control_loop.MinimalCtrlLoop Package Contents ---------------- .. py:class:: SimpleManagedCtrlLoop(robot_interface: pyrcf.components.robot_interfaces.robot_interface_base.RobotInterface, state_estimator: pyrcf.components.state_estimators.state_estimator_base.StateEstimatorBase, controller_manager: pyrcf.components.controller_manager.controller_manager_base.ControllerManagerBase, global_planner: pyrcf.components.global_planners.global_planner_base.GlobalMotionPlannerBase, verbose: bool = True) A minimal control loop that takes in a controller manager instead of a single controller and local planner. The run method runs the following in a loop of specified frequency. 1. read robot state from robot interface 2. update robot state with state estimates from state estimator 3. get global plan message from global planner / user interface 4. for agent in controller_manager.agents: a. compute control command to track the global plan (agent may be a Planner+Controller agent) 5. accumulate commands from accumulation policy used by controller manager 6. write control command to robot .. py:attribute:: robot .. py:attribute:: state_estimator .. py:attribute:: controller_manager .. py:attribute:: global_planner .. py:attribute:: _true_dt_getter .. py:attribute:: _true_dt :value: 0.0 .. py:attribute:: _loop_count :value: 0 .. py:method:: run(loop_rate: float, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock(), debuggers: List[pyrcf.components.ctrl_loop_debuggers.ctrl_loop_debugger_base.CtrlLoopDebuggerBase] = None, prestep_callbacks: List[pyrcf.components.callback_handlers.base_callbacks.CustomCallbackBase] = None, poststep_callbacks: List[pyrcf.components.callback_handlers.base_callbacks.CustomCallbackBase] = None) The main control loop. :param loop_rate: loop rate in hz. This will always use the system clock to try and maintain the desired control loop rate. :type loop_rate: float :param clock: Defines the clock to use for querying time for the components in the control loop. Time will be queried once per loop and the same `t` and `dt` will be passed to all components in that iteration. The option to select the clock is so that sim time can be used if required. :type clock: ClockBase, optional :param debuggers: List of `CtrlLoopDebuggerBase` objects that are to be run in the control loop. :type debuggers: CtrlLoopDebuggerBase, optional :param prestep_callbacks: List of `CustomCallbackBase` objects to be run in the control loop. Not recommended to do this except for debugging. Defaults to None. :type prestep_callbacks: List[CustomCallbackBase], optional :param poststep_callbacks: List of additional `CustomCallbackBase` objects to be run in the control loop. Not recommended to do this except for debugging. Defaults to None. :type poststep_callbacks: List[CustomCallbackBase]. optional .. py:method:: get_actual_loop_rate() -> Tuple[float, float] Get the true loop rate and dt (using system clock). :returns: True dt (s), true loop rate (hz) :rtype: Tuple[float, float] .. py:method:: get_loop_count() -> int Get the current number of iterations of the control loop. :returns: current number of iterations of the control loop :rtype: int .. py:method:: shutdown() Cleanly shutdown all components in the control loop. Sequence of shutdown: - robot interface - controller manager - global planner .. py:method:: send_shutdown_signal() Request clean shutdown of control loop and components. .. py:class:: MinimalCtrlLoop(robot_interface: pyrcf.components.robot_interfaces.robot_interface_base.RobotInterface, state_estimator: pyrcf.components.state_estimators.state_estimator_base.StateEstimatorBase, controller: pyrcf.components.controllers.controller_base.ControllerBase, local_planner: pyrcf.components.local_planners.LocalPlannerBase, global_planner: pyrcf.components.global_planners.global_planner_base.GlobalMotionPlannerBase, verbose: bool = True) Bases: :py:obj:`pyrcf.control_loop.simple_managed_ctrl_loop.SimpleManagedCtrlLoop` A minimal control loop with only 1 controller and 1 local planner. The run method runs the following in a loop of specified frequency. 1. read robot state from robot interface 2. update robot state with state estimates from state estimator 3. get global plan message from global planner / user interface 4. generate local plan using the global plan message 5. compute control command to track the local plan 6. write control command to robot .. py:method:: useWithDefaults(robot_interface: pyrcf.components.robot_interfaces.robot_interface_base.RobotInterface, controller: pyrcf.components.controllers.controller_base.ControllerBase, global_planner: pyrcf.components.global_planners.global_planner_base.GlobalMotionPlannerBase = None, local_planner: pyrcf.components.local_planners.LocalPlannerBase = BlindForwardingPlanner(), state_estimator: pyrcf.components.state_estimators.state_estimator_base.StateEstimatorBase = DummyStateEstimator(squawk=False), verbose: bool = True) -> MinimalCtrlLoop :classmethod: Create a control loop default values for planners and state estimators. Default local planner: BlindForwardingPlanner Default global planner: Try JoystickGlobalPlannerInterface; fallback KeyboardGlobalPlannerInterface Default state estimator: DummyStateEstimator :param robot_interface: The robot interface to use in the loop. :type robot_interface: RobotInterface :param controller: The controller to be used. :type controller: ControllerBase :param global_planner: The global planner to use. If None provided, will try to first find a joystick, and then creates a keyboard interface if joystick is not found. :type global_planner: GlobalMotionPlannerBase, optional :param local_planner: Local planner to use. Defaults to BlindForwardingPlanner. :type local_planner: LocalPlannerBase, optional :param state_estimator: State estimator to use. Defaults to DummyStateEstimator(squawk=False). :type state_estimator: StateEstimatorBase, optional :returns: returns a valid control loop object with planners and state estimators automatically defined. :rtype: MinimalCtrlLoop