Class FailureDetector

Defined in File recovery_behaviors.h

Nested Relationships

Nested Types

Struct FailureDetector::VelMeasurement

Class Documentation

class FailureDetector

This class implements methods in order to detect if the robot got stucked or is oscillating.

The StuckDetector analyzes the last N commanded velocities in order to detect whether the robot might got stucked or oscillates between left/right/forward/backwards motions.

Public Functions

FailureDetector() = default: Default constructor.

~FailureDetector() = default: destructor.

inline void setBufferLength(int length)

Set buffer length (measurement history)

Parameters:: length – number of measurements to be kept

void update(const geometry_msgs::Twist &twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps)

Add a new twist measurement to the internal buffer and compute a new decision.

Parameters:

twist – geometry_msgs::Twist velocity information
v_max – maximum forward translational velocity
v_backwards_max – maximum backward translational velocity
omega_max – maximum angular velocity
v_eps – Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1)
omega_eps – Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1)

bool isOscillating() const

Check if the robot got stucked.

This call does not compute the actual decision, since it is computed within each update() invocation.

Returns:: true if the robot got stucked, false otherwise.

void clear()

Clear the current internal state.

This call also resets the internal buffer

Protected Functions

bool detect(double v_eps, double omega_eps)

Detect if the robot got stucked based on the current buffer content.

Afterwards the status might be checked using gotStucked();

Parameters:

v_eps – Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1)
omega_eps – Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1)

Returns:

true if the robot got stucked, false otherwise

Private Members

boost::circular_buffer<VelMeasurement> buffer_

Circular buffer to store the last measurements.