Making the system robust with Fallbacks

In the previous Point Navigation tutorial, we built a standard point navigation stack. While functional, it is “optimistic”, it assumes that all the components will keep working smoothly and no errors or failures would happen during runtime.

In reality, sensors disconnect, optimization solvers fail to converge, and dynamic obstacles block paths.

In this tutorial, we will give our Turtlebot3 a few Fallback techniques, so the robot can attempt to self-heal and reconfigure itself when these inevitable failures occur.

Handling Algorithm Failures

Algorithms used inside the navigation components can fail during runtime for many reasons. A motion control algorithm used in the Controller can fail, for example, due to:

• Mathematical Singularity: The optimization solver might fail to find a solution that satisfies all kinematic constraints (e.g., the robot is cornered).

• Local Minima: The robot might get “stuck” oscillating in a narrow corridor or doorway.

• Tracking Error: The robot has drifted too far from the path, and the controller cannot compute a valid velocity to rejoin it smoothly.

Implementation

If our primary high-performance algorithm (e.g., DWA) fails, we shouldn’t just stop. We can switch to a “Safety” algorithm (like a simple PurePursuit).

• Start by importing the Action class required to define the Fallback routines, and the ControllersID enum for selecting our main controller and our fallback controller:

from kompass.ros import Action
from kompass.control import ControllersID

• Select the primary control algorithm, and define the Action to switch the algorithm into our fallback choice:

# Select the primary control algorithm
controller.algorithm = ControllersID.DWA

# Define the action
switch_algorithm_action =  Action(method=controller.set_algorithm, args=(ControllersID.PURE_PURSUIT,))

• Set the fallbacks action sequence: Try restarting the controller -> If fails again -> Switch the control algorithm

controller.on_algorithm_fail(action=[Action(controller.restart), switch_algorithm_action], max_retries=1)

Handling System Failures

Some component, like the DriveManager talks directly to the low-level hardware (micro-controller/motor drivers). And in rel-life the robot can face different system failures during runtime:

• Cable Issues: Loose USB/Serial cables caused by vibration.

• EMI: Electromagnetic interference causing packet loss on the serial bus.

• Watchdog Trip: The micro-controller resets itself because it didn’t receive a command in time.

Implementation

To make our system more resilient, we can implement a simple ‘restart’ fallback for system failures. Since hardware glitches are often transient, this simple restart fallback can re-establishes the serial handshake and be quite effective.

• We can do this by simply adding the following line to our recipe:

# Not setting max_retires -> will attempt restarting infinity
driver.on_system_fail(Action(driver.restart))

• We can also select a general component-level fallback for all our component by doing:

launcher.on_fail(action_name="restart")

Complete Recipe

Here is the complete turtlebot3_with_fallbacks.py script combining the Point Navigation setup with our new robustness layer.

turtlebot3_with_fallbacks.py

import numpy as np
from kompass.robot import (
    AngularCtrlLimits,
    LinearCtrlLimits,
    RobotGeometry,
    RobotType,
)
from kompass.config import RobotConfig, RobotFrames
from kompass.components import (
    Controller,
    DriveManager,
    Planner,
    PlannerConfig,
)
from kompass.control import ControllersID
from kompass.ros import Topic, Launcher, Action

# Setup your robot configuration
my_robot = RobotConfig(
    model_type=RobotType.DIFFERENTIAL_DRIVE,
    geometry_type=RobotGeometry.Type.CYLINDER,
    geometry_params=np.array([0.1, 0.3]),
    ctrl_vx_limits=LinearCtrlLimits(max_vel=0.2, max_acc=1.5, max_decel=2.5),
    ctrl_omega_limits=AngularCtrlLimits(
        max_vel=0.4, max_acc=2.0, max_decel=2.0, max_steer=np.pi / 3)
)

# Set the robot frames
robot_frames = RobotFrames(
    robot_base='base_link',
    odom='odom',
    world='map',
    scan='LDS-01')

# Setup components with default config, inputs and outputs
planner_config = PlannerConfig(loop_rate=1.0)       # 1 Hz
planner = Planner(component_name="planner", config=planner_config)

# Set Planner goal input to Rviz Clicked point
goal_topic = Topic(name="/clicked_point", msg_type="PointStamped")
planner.inputs(goal_point=goal_topic)

# Get a default controller component
controller = Controller(component_name="controller")

# Configure Controller to use local map instead of direct sensor information
controller.direct_sensor = False

# Select the primary control algorithm
controller.algorithm = ControllersID.DWA

# Define the action
switch_algorithm_action =  Action(method=controller.set_algorithm, args=(ControllersID.PURE_PURSUIT,))

# Set the Controller 'algorithm-level' failure fallback
controller.on_algorithm_fail(action=[Action(controller.restart), switch_algorithm_action], max_retries=1)

# Get the default DriveManager
driver = DriveManager(component_name="drive_manager")

# Publish Twist or TwistStamped from the DriveManager based on the distribution
if "ROS_DISTRO" in os.environ and (
    os.environ["ROS_DISTRO"] in ["rolling", "jazzy", "kilted"]
):
    cmd_msg_type : str = "TwistStamped"
else:
    cmd_msg_type = "Twist"

driver.outputs(robot_command=Topic(name="/cmd_vel", msg_type=cmd_msg_type))

# Set the DriveManager System-Level failure Fallback
# Not setting max_retires -> will attempt restarting infinity
driver.on_system_fail(Action(driver.restart))

# Get a default Local Mapper component
mapper = LocalMapper(component_name="mapper")

# Init a launcher
launcher = Launcher()

# Pass kompass components to the launcher
launcher.kompass(
    components=[planner, controller, driver, mapper],
    activate_all_components_on_start=True,
    multi_processing=True)

# Set the robot
launcher.robot = robot_config

# Set the frames
launcher.frames = frames_config

# Optional Generic Fallback Policy: If any component fails -> restart it with unlimited retries
# launcher.on_fail(action_name="restart")

# After all configuration is done bringup the stack
launcher.bringup()

Next Steps

Your robot can now heal itself from internal failures! But what about reacting to the external world?

In the next tutorial, we will use Events to make the robot reactive to its environment.

Adding Reflexes (Events) →