Controller

Controller#

Two-dimensional#

#include "navground/core/controller.h"

struct Action#

Holds the state of a long running task and notifies observers when it terminates.

Subclassed by navground::core::FollowAction, navground::core::FollowManualCommandAction, navground::core::FollowTwistAction, navground::core::MoveAction

Public Types

enum class State#

The state of an action.

Values:

enumerator idle#: the action has not started

enumerator running#: the action is running

enumerator failure#: the action failed

enumerator success#: the action succeeded

Public Functions

inline bool done() const#

Return whenever the action is done or not.

Returns:: True if the action failed or succeeded

inline bool running() const#

Return whenever the action is running.

Returns:: True if the action is running

void abort()#: Abort the action, calling the done_cb if set.

Public Members

State state#: The current state

std::optional<std::function<void(ng_float_t)>> running_cb#

A callback called when the action is running

The callback argument is the minimal expected time to terminate the action.

std::optional<std::function<void(State state)>> done_cb#

A callback called when the action terminates

The callback argument is the final state of the action.

class Controller#

This class exposes a higher level, stateful interface to a behavior, to which it delegates 2D collision avoidance.

The controller provides actions (event-based interfaces) to

go to a point/pose, stopping once the target has been reached,
follow a point/pose, which does not terminates once the target has been reached
follow a velocity/twist

Typical usage of a controller

Pick and configure a Behavior
Initialize and configure the controller
At regular intervals, update the state, using the Behavior API, and call update.
When needed, trigger one of the actions.
Either manually check the action’s get_state or set callbacks to be notified when the action terminates or updates.
Actuate the target command by using the return value of update or by setting a callback set_cmd_cb

Subclassed by navground::core::Controller3

Public Functions

inline explicit Controller(std::shared_ptr<Behavior> behavior = nullptr)#

Constructs a new instance.

Parameters:: behavior – [in] The navigation behavior

inline Action::State get_state() const#

Gets the state of the control action.

Returns:: The state.

inline std::shared_ptr<Behavior> get_behavior() const#

Gets the navigation behavior used by the controller.

Returns:: The navigation behavior.

inline void set_behavior(std::shared_ptr<Behavior> value)#

Sets the navigation behavior used by the controller.

Parameters:: value – [in] The navigation behavior

inline ng_float_t get_speed_tolerance() const#

Gets the minimal speed to consider the agent as stopped.

The default is 1 cm/s.

Returns:: The speed tolerance.

inline void set_speed_tolerance(ng_float_t value)#

Sets the minimal speed to consider the agent as stopped.

Parameters:: value – [in] The speed tolerance

inline std::optional<Frame> get_cmd_frame() const#

Gets the frame of reference for the command.

Returns:: The frame of reference.

inline void set_cmd_frame(const std::optional<Frame> &frame)#

Sets the frame of reference for the command.

Parameters:: frame – [in] The desired value

std::shared_ptr<Action> go_to_position(const Vector2 &point, ng_float_t tolerance, std::optional<Path> along_path = std::nullopt)#

Starts an action to go to a point.

The action succeed once the agent arrives within a tolerance from the target point and comes to a stop.

If an action is already running, the controller aborts it.

Parameters:

point – [in] The target point
tolerance – [in] The spatial tolerance
along_path – [in] An optional path to follow

Returns:

The new action.

std::shared_ptr<Action> go_to_pose(const Pose2 &pose, ng_float_t position_tolerance, ng_float_t orientation_tolerance, std::optional<Path> along_path = std::nullopt)#

Starts an action to go to a pose.

The action succeed once the agent arrives within a tolerance from the target pose and comes to a stop.

If an action is already running, the controller aborts it.

Parameters:

pose – [in] The target pose
position_tolerance – [in] The spatial tolerance
orientation_tolerance – [in] The spatial tolerance
along_path – [in] An optional path to follow

Returns:

The new action.

std::shared_ptr<Action> follow_path(const Path &path, ng_float_t tolerance)#

Start following a path.

Internally calls go_to_position to move towards the last point of the path .

Parameters:

path – [in] The path
tolerance – [in] The spatial tolerance

Returns:

The new action.

std::shared_ptr<Action> follow_point(const Vector2 &point)#

Starts an action to follow a point.

The action keeps running even after the agent arrive at the target.

If an action is already running, the controller aborts it, unless it was following a point/pose, in which case it just updates the target.

Parameters:: point – [in] The target point
Returns:: The new action.

std::shared_ptr<Action> follow_pose(const Pose2 &pose)#

Starts an action to follow a pose.

The action keeps running even after the agent arrive at the target.

If an action is already running, the controller aborts it, unless it was following a point/pose, in which case it just updates the target.

Parameters:: pose – [in] The target pose
Returns:: The new action.

std::shared_ptr<Action> follow_direction(const Vector2 &direction)#

Starts an action to follow a target direction.

If an action is already running, the controller aborts it, unless it was following a velocity/twist, in which case it just updates the target.

Parameters:: direction – [in] The target direction. Must have positive norm.
Returns:: The new action.

std::shared_ptr<Action> follow_velocity(const Vector2 &velocity)#

Starts an action to follow a target velocity.

If an action is already running, the controller aborts it, unless it was following a velocity/twist, in which case it just updates the target.

Parameters:: velocity – [in] The target velocity
Returns:: The new action.

std::shared_ptr<Action> follow_twist(const Twist2 &twist)#

Starts an action to follow a target twist.

If an action is already running, the controller aborts it, unless it was following a velocity/twist, in which case it just updates the target.

Parameters:: twist – [in] The target twist
Returns:: The new action.

inline bool idle() const#

Returns whether the control action is idle.

Returns:: True only if the control action is not running.

void stop()#: Abort the running action.

Twist2 update(ng_float_t time_step)#

Updates the control for time step.

Internally calls Behavior::compute_cmd for collision avoidance.

Parameters:: time_step – [in] The time step
Returns:: The command twist to execute the current action

inline void set_cmd_cb(std::function<void(const Twist2&)> value)#

Sets the callback called each time a command is computed for an active action.

Parameters:: value – [in] A callback that takes the command twist as argument.

Three-dimensional#

#include "navground/core/controller_3d.h"

struct Cylinder#

Three dimensional obstacles with a vertical cylindrical shape.

Subclassed by navground::core::Neighbor3

Public Functions

inline Cylinder(const Vector3 position, ng_float_t radius, ng_float_t height = -1.0)#

Constructs a new instance.

Parameters:

position – [in] Center of the lower face
radius – [in] Radius of the cylinder
height – [in] Height of the cylinder

inline Disc disc() const#

Project the cylinder to the two dimensional plane.

Returns:: The projected disc.

Public Members

Vector3 position#: Center of the lower face.

ng_float_t radius#: Radius of the cylinder

ng_float_t height#: Height of the cylinder

struct Neighbor3 : public navground::core::Cylinder #

Three dimensional neighbor of a vertical cylindrical shape.

Public Functions

inline Neighbor3(const Vector3 position, ng_float_t radius, ng_float_t height = -1.0, Vector2 velocity = Vector2::Zero(), unsigned id = 0.0)#

Constructs a new instance.

Parameters:

position – [in] Center of the lower face
radius – [in] Radius of the cylinder
height – [in] Height of the cylinder
velocity – [in] Planar velocity
id – [in] Neighbor identifier

inline Neighbor neighbor() const#

Project to the two dimensional plane.

Returns:: The projected neighbor.

Public Members

Vector2 velocity#: The horizontal velocity

unsigned id#: See Neighbor::id

class Controller3 : public navground::core::Controller #

Simplistic extension of Controller to 3D.

Assumes that the agent has planar attitude and motion in 3D space.

Relies on the 2D obstacle avoidance Behavior, when provided with all relevant state (i.e., vertical position, speed, and target position), it 1) considers only obstacles that may intersect in three dimensions 2) controls vertical speed with a proportional controller.

Note that this is at best a 2.5D controller, making no attempt at 3D collision avoidance. It is best suited for planar motion at arbitrary altitudes, like when a drone flies above ground robots, effectively ignoring them.

Typical usage

Pick, configure a Behavior, set it to behavior
Initialize and configure the controller
At regular intervals update the state, using the Controller3 API, and call update_3d.
Trigger one of the actions.
Either manually check the action’s get_state or set callbacks to be notified when the action terminates or updates.
Actuate the target command by using the return value of update_3d or by setting a callback set_cmd_cb

Public Functions

inline Controller3(std::shared_ptr<Behavior> behavior = nullptr, bool limit_to_2d = false)#

Constructs a new instance.

Parameters:

behavior – [in] The navigation behavior
limit_to_2d – [in] Whether to limit the control to 2D

void set_neighbors(const std::vector<Neighbor3> &neighbors)#

Sets the neighbors.

Filter relevant neighbors, project them to 2D and pass them to GeometricState::set_neighbors

Parameters:: neighbors – [in] The neighbors

void set_static_obstacles(const std::vector<Cylinder> &obstacles)#

Sets the static obstacles.

Filter relevant obstacles, project them to 2D and pass them to GeometricState::set_static_obstacles

Parameters:: obstacles – [in] The obstacles

inline Pose3 get_pose() const#

Gets the pose.

Returns:: The pose.

inline void set_pose(const Pose3 &pose)#

Sets the pose.

Automatically also set the behavior 2D pose.

Parameters:: pose – [in] The pose

inline Twist3 get_twist() const#

Gets the twist.

Returns:: The twist.

inline void set_twist(const Twist3 &twist)#

Sets the twist.

Automatically set the behavior 2D twist.

Parameters:: twist – [in] The twist

inline void actuate(const Twist3 &twist, ng_float_t time_step)#

Actuate a twist command, integrating using Pose3::integrate.

Parameters:

twist – [in] The twist
time_step – [in] the time step

inline ng_float_t get_altitude_optimal_speed() const#

Gets the altitude optimal speed.

Returns:: The altitude optimal speed.

inline void set_altitude_optimal_speed(ng_float_t value)#

Sets the altitude optimal speed.

Parameters:: value – [in] The desired value

inline ng_float_t get_altitude_tau() const#

Gets the altitude relaxation time.

Returns:: The altitude relaxation time.

inline void set_altitude_tau(ng_float_t value)#

Sets the altitude relaxation time.

Parameters:: value – [in] The desired value

std::shared_ptr<Action> go_to_position(const Vector3 &point, ng_float_t tolerance)#

Starts an action to go to a 3D point.

The action succeed once the agent arrives within a tolerance from the target point and comes to a stop.

If an action is already running, the controller aborts it.

Parameters:

point – [in] The target point
tolerance – [in] The spatial tolerance

Returns:

The new action.

std::shared_ptr<Action> go_to_pose(const Pose3 &pose, ng_float_t position_tolerance, ng_float_t orientation_tolerance)#

Starts an action to go to a 3D pose.

The action succeed once the agent arrives within a tolerances from the target pose and comes to a stop.

If an action is already running, the controller aborts it.

Parameters:

pose – [in] The target pose
position_tolerance – [in] The spatial tolerance
orientation_tolerance – [in] The spatial tolerance

Returns:

The new action.

std::shared_ptr<Action> follow_point(const Vector3 &point)#

Starts an action to follow a 3D point.

The action keeps running even after the agent arrive at the target.

If an action is already running, the controller aborts it, unless it was following a point/pose, in which case it just updates the target.

Parameters:: point – [in] The target point
Returns:: The new action.

std::shared_ptr<Action> follow_pose(const Pose3 &pose)#

Starts an action to follow a 3D pose.

The action keeps running even after the agent arrive at the target.

If an action is already running, the controller aborts it, unless it was following a point/pose, in which case it just updates the target.

Parameters:: pose – [in] The target pose
Returns:: The new action.

std::shared_ptr<Action> follow_velocity(const Vector3 &velocity)#

Starts an action to follow a 3D velocity.

If an action is already running, the controller aborts it, unless it was following a velocity/twist, in which case it just updates the target.

Parameters:: velocity – [in] The target velocity
Returns:: The new action.

std::shared_ptr<Action> follow_twist(const Twist3 &twist)#

Starts an action to follow a 3D twist.

If an action is already running, the controller aborts it, unless it was following a velocity/twist, in which case it just updates the target.

Parameters:: twist – [in] The target twist
Returns:: The new action.

Twist3 update_3d(ng_float_t time_step)#

Updates the control for time step, computing a 3D command.

Internally calls Behavior::compute_cmd for collision avoidance.

Parameters:: time_step – [in] The time step
Returns:: The command twist to execute the current action

inline void set_cmd_cb(std::function<void(const Twist3&)> value)#

Sets the callback called each time a command is computed for an active action.

Parameters:: value – [in] The callback that takes the 3D command twist as argument.

inline bool is_limited_to_2d() const#

Determines if currently limited to 2D control.

Returns:: True if limited to 2D, False otherwise.

inline void should_be_limited_to_2d(bool value)#

Set if the control should ignore 3D information.

Parameters:: value – [in] The desired value

Controller

Contents

Controller#

Two-dimensional#

Three-dimensional#