World

World#

#include "navground/sim/world.h"
using navground::sim::BoundingBox = geos::geom::Envelope#

A rectangulare region

struct Entity#

Super-class that adds a unique ID to world entities.

This unique ID should not be fed to navigation behaviors, but only used internally by the simulation, for instance, to identify entities in a UI.

Subclassed by navground::sim::Agent, navground::sim::Obstacle, navground::sim::Wall

Public Functions

inline Entity()#

Constructs a new instance.

inline explicit Entity(unsigned id)#

Constructs a new instance.

Parameters:

id – [in] The identifier

inline bool has_been_in_collision_since(ng_float_t time) const#

Determines if the entity has been in collision at least once since the given time.

Parameters:

time – [in] The time

Returns:

True if been in collision since, False otherwise.

inline ng_float_t get_last_collision_time() const#

Returns the last collision time.

Returns:

The time

inline void reset()#

Resets the entity.

Public Members

unsigned uid#

Unique identifier

Public Static Functions

static inline void reset_uid()#

Reset the UID counter to zero.

struct Obstacle : public navground::sim::Entity#

A static obstacle with circular shape.

Public Functions

inline Obstacle(const Vector2 &position, ng_float_t radius)#

Constructs a new instance.

Parameters:

  • position – [in] The position of the circle

  • radius – [in] The radius of the circle

inline Obstacle()#

Constructs a new instance.

inline Obstacle(const Disc &disc)#

Constructs a new instance.

Parameters:

disc – [in] A disc

inline operator Disc() const#

Disc conversion operator.

Public Members

Disc disc#

The disc.

struct Wall : public navground::sim::Entity#

A static wall.

Currently, only line segment are valid shapes of walls.

Public Functions

inline Wall(const Vector2 &p1, const Vector2 &p2)#

Constructs a new instance.

Parameters:

  • p1 – [in] The line segment start vertex

  • p2 – [in] The line segment end vertex

inline Wall()#

Constructs a new instance.

inline explicit Wall(const LineSegment &ls)#

Constructs a new instance.

Parameters:

ls – [in] A line segment

inline operator LineSegment() const#

LineSegment conversion operator.

Public Members

LineSegment line#

The line segment.

class World#

Ghost agents used in worlds that have a lattice.

This class describes a world.

A world implements the core part of the simulation. It holds a collection of entities as walls, obstacles, and agents.

After setting up the world entities, users call update or run to perform one or more simulation steps, where

  1. each agent updates its control

  2. each agent perform actuate its control command

  3. collisions are checked and resolved

  4. time is advanced

World simulation uses a simple collision model that does not attempt to be realistic but should be computationally efficient.

  1. first runs a broad-phase using libgeos STR Trees <>_ where potential collisions are found using rectangular bounding boxes.

  2. then it runs a narrow-phase using the simple exact geometric shape of the obstacles and record pairs of entities that are in collision.

  3. finally it resolves collisions by colliding moving minimally entities away from each other, setting to zero the component of their velocities that would attract them together.

Public Types

using Lattice = std::optional<std::tuple<ng_float_t, ng_float_t>>#

An (optional) one dimensional lattice defined by the initial point and the size of the cell.

using Callback = std::function<void()>#

A callback to be executed at each simulation step

using TerminationCondition = std::function<bool(const World *world)>#

A condition for the simulation to terminate

Public Functions

inline explicit World()#

Constructs a new instance.

void update(ng_float_t time_step)#

Updates world for a single time step.

Parameters:

time_step – [in] The time step

void update_dry(ng_float_t time_step, bool advance_time = true)#

Updates world for a single time step without actuation and collisions resolution.

Parameters:

  • time_step – [in] The time step

  • advance_time – [in] Whenever to advance time too.

void actuate(ng_float_t time_step)#

Actuate then controllers and perform collisions resolutions.

Parameters:

time_step – [in] The duration of each time step

void run(unsigned steps, ng_float_t time_step)#

Updates the world for one or more time steps.

Parameters:

  • steps – [in] The number of steps

  • time_step – [in] The duration of each time step

void run_until(std::function<bool()> condition, ng_float_t time_step)#

Updates the world until a condition is satisfied.

Parameters:

  • condition – [in] The condition

  • time_step – [in] The duration of each time step

inline ng_float_t get_time() const#

Gets the simulation time.

Returns:

The simulation time.

inline unsigned get_step() const#

Gets the simulation step.

Returns:

The simulation step.

void add_agent(const std::shared_ptr<Agent> &agent)#

Adds an agent to the world.

Parameters:

agent – [in] The agent

void remove_agent(Agent *agent)#

Remove an agent from the world.

Parameters:

agent – [in] The agent

void remove_agent_with_uid(unsigned uid)#

Remove an agent from the world.

Parameters:

uid – [in] The uid of the agent

void add_wall(const LineSegment &line)#

Adds a line to the world as a wall.

Parameters:

line – [in] The line

void add_wall(const Wall &wall)#

Adds a wall to the world.

Parameters:

wall – [in] The wall

void add_obstacle(const Disc &disc)#

Adds a disc the world as a static obstacle.

Parameters:

disc – [in] The disc

void add_obstacle(const Obstacle &obstacle)#

Adds a static obstacle the world.

Parameters:

obstacle – [in] The obstacle

void add_random_obstacles(unsigned number, ng_float_t min_radius, ng_float_t max_radius, ng_float_t margin = 0.0, unsigned max_tries = 1000)#

Adds a random obstacles in the world bounding box.

It iteratively try to sample a disc that is far enough of any other item. If the sampling fails, it counts as a try and the process stop after either enough obstacles have been added or enough tries performed.

Parameters:

  • number – [in] The number of obstacles

  • min_radius – [in] The minimum radius of obstacles

  • max_radius – [in] The maximum radius of obstacles

  • margin – [in] The minimal distance to other obstacles or agents. For agents, it’s additional to their safety margin.

  • max_tries – [in] The maximum tries before terminating.

const std::vector<std::shared_ptr<Agent>> &get_agents() const#

Gets all agents in this world.

Returns:

All agents.

std::vector<Neighbor> get_neighbors(const Agent *agent, ng_float_t distance, bool ignore_lattice = false)#

Gets all neighbor of an agent.

Parameters:

  • agent – [in] The agent

  • distance – [in] The radius of the neighborhood

  • ignore_lattice – [in] Whether to ignore the lattice when computing neighbors

Returns:

All neighbor within a circle of radius radius centered around the agent.

std::vector<std::tuple<Vector2, BoundingBox>> subdivide_bounding_box(const BoundingBox &bounding_box, bool ignore_lattice = false) const#

Split a bounding box by the lattice-induced grid.

Parameters:

  • bounding_box – [in] The bounding box

  • ignore_lattice – [in] Indicates if the lattice is to be ignored

Returns:

A vector of pairs of translation (along the lattice) and subset of the bounding box that fit in that lattice cell.

std::vector<Agent*> get_agents_in_region(const BoundingBox &bb)#

Gets all agents in a bounding box.

Parameters:

bb – [in] The bounding box specified in world-fixed coordinates

Returns:

All agents that lie in a bounding box.

std::vector<Obstacle*> get_obstacles_in_region(const BoundingBox &bb)#

Gets all obstacles in a bounding box.

Parameters:

bb – [in] The bounding box specified in world-fixed coordinates

Returns:

All obstacles that lie in a bounding box.

const std::vector<std::shared_ptr<Obstacle>> &get_obstacles() const#

Gets all obstacles in this world.

Returns:

All obstacles.

std::vector<Disc> get_discs(bool ignore_lattice = false) const#

Gets all disc shaped static obstacles in this world.

Parameters:

ignore_lattice – [in] Whether to ignore the lattice

Returns:

All obstacles.

std::vector<Disc> get_discs_in_region(const BoundingBox &bb, bool ignore_lattice = false)#

Gets all agents in a bounding box.

Parameters:

  • bb – [in] The bounding box specified in world-fixed

  • ignore_lattice – [in] Whether to ignore the lattice when computing neighbors

Returns:

All obstacles that lie in a bounding box

const std::vector<std::shared_ptr<Wall>> &get_walls() const#

Gets all walls in this world.

Returns:

All walls.

std::vector<LineSegment> get_line_obstacles() const#

Gets all line obstacles in this world.

Returns:

All ine obstacles.

std::vector<LineSegment*> get_line_obstacles_in_region(const BoundingBox &bb) const#

Gets all walls in a bounding box.

Parameters:

bb – [in] The bounding box specified in world-fixed

Returns:

All walls that lie in a bounding box

void set_obstacles(const std::vector<Disc> &obstacles)#

Replaces all obstacles.

Parameters:

obstacles – [in] The new obstacles

void set_walls(const std::vector<LineSegment> &walls)#

Replaces all walls.

Parameters:

walls – [in] The new walls

inline const std::set<std::tuple<const Entity*, const Entity*>> &get_collisions() const#

Gets the colliding pairs computed during the last simulation step.

Returns:

The colliding pair of entities.

ng_float_t compute_safety_violation(const Agent *agent, std::optional<float> safety_margin = std::nullopt)#

Calculates the safety violation, i.e. the maximal penetration of a neighbor or obstacle in the safety margin of the agent.

Parameters:

  • agent – [in] The agent

  • safety_margin – [in] The safety margin. If not set, it will default to the agent’s behavior safety margin.

Returns:

The safety violation or 0 if no violation.

RandomGenerator &get_random_generator()#

The random generator shared by all distribution used to generate and simulate this world.

Returns:

The random generator

void set_random_generator(RandomGenerator &value)#

Sets the random generator shared by all distribution used to generate and simulate this world.

Parameters:

value – [in] The desired random generator

inline void copy_random_generator(World &world)#

Copy the random generator from another world.

Parameters:

world – [in] The world

unsigned get_seed() const#

Gets the random seed.

Returns:

The random seed.

void set_seed(unsigned seed)#

Sets the random seed.

Parameters:

seed – [in] The random seed

bool agents_are_idle() const#

Check if all agents are idle (i.e., their tasks are done and their controller are idle).

Returns:

True if all agents are idle

bool agents_are_idle_or_stuck() const#

Check if all agents are idle or stuck (i.e., they are no moving because they task is done or they are deadlocked)

Returns:

True if all agents are idle or stuck

void space_agents_apart(ng_float_t minimal_distance = 0, bool with_safety_margin = false, unsigned max_iterations = 10)#

Move agents so that they do not overlap anymore with themselves or with any obstacle.

Parameters:

  • minimal_distance – [in] The minimal distance

  • with_safety_margin – [in] Whether the safety margin should be added to the minimal distance

  • max_iterations – [in] The maximal number of iterations to perform.

inline bool has_lattice() const#

Determines if this world uses a lattice.

Returns:

True if it uses a lattice, False otherwise.

Lattice get_lattice(unsigned axis) const#

Gets the periodic lattice.

Parameters:

axis – [in] The axis (0 for x, 1 for y)

Returns:

An optional tuple of points that define a periodic lattice that wraps the selected axis.

void set_lattice(unsigned axis, const Lattice &value)#

Sets the periodic lattice.

Parameters:

  • axis – [in] The axis (0 for x, 1 for y)

  • value – [in] An optional tuple of points that define a periodic lattice that wraps the selected axis. Pass none to unset the lattice and remove wrapping.

std::vector<Vector2> get_lattice_grid(bool include_zero = true, bool c8 = true) const#

The N=0, 1, or 2 vectors that define the lattice, e.g., {delta_x, -delta_x} if only the axis=0 lattice is set.

Parameters:

  • include_zero – [in] Whether to include the zero vector

  • c8 – [in] Whether to use 8-connectivity instead of 4-connectivity

Returns:

A vector of 2D vectors

BoundingBox get_lattice_bounding_box() const#

Gets the lattice cell.

Returns:

The lattice cell.

bool in_collision(const Entity *e1, const Entity *e2) const#

Check if two entities are currently in collision.

Parameters:

  • e1 – [in] The first entity

  • e2 – [in] The second entity

Returns:

True if they are in collision.

inline Entity *get_entity(unsigned uid)#

Find an entity by identifier.

Parameters:

uid – [in] The entity uid

Returns:

The entity or nullptr if not found.

inline Agent *get_agent(unsigned uid)#

Find an agent by identifier.

Parameters:

uid – [in] The agent uid

Returns:

The agent or nullptr if not found.

inline void add_callback(const Callback &value)#

Adds a callback to be executed after each simulation step.

Parameters:

value – [in] The callback

inline void reset_callbacks()#

Clear all the callbacks.

inline void set_termination_condition(const std::optional<TerminationCondition> &value)#

Sets a condition to terminate simulations.

Parameters:

value – [in] The desired condition.

inline bool has_termination_condition() const#

Returns whether there is a termination condition set.

Returns:

True if a termination condition has been set.

inline bool should_terminate() const#

Checks whether the simulation should terminate.

Returns:

True if a termination condition is set and evaluates to true.

std::vector<Agent*> get_agents_in_collision(ng_float_t duration = 0.0) const#

Gets the agents that had a collision after now - duration.

Parameters:

duration – [in] The duration

Returns:

The agents in collision.

std::vector<Agent*> get_agents_in_deadlock(ng_float_t duration = 0.0) const#

Gets the agents that are in stuck since now - duration.

Parameters:

duration – [in] The duration

Returns:

The agents in deadlock.

void snap_twists_to_zero(ng_float_t epsilon = 1e-6) const#

Snap agents’ twists smaller than epsilon to zero.

Parameters:

epsilon – [in] The tolerance

inline std::optional<unsigned> index_of_agent(const Agent *agent) const#

Searches for the index of an agent.

Parameters:

agent – [in] The agent

Returns:

The index of this agent in the world agents list or null if not found.

BoundingBox get_minimal_bounding_box() const#

Gets the bounding box that contains all agents, obstacles and walls.

Returns:

The computed bounding box.

inline void set_bounding_box(const std::optional<BoundingBox> &value)#

Sets the bounding box.

Parameters:

value – [in] The desired value. Pass none to unset the bounding box.

inline BoundingBox get_bounding_box() const#

Gets the bounding box.

Returns:

The bounding box that has been set with set_bounding_box or get_minimal_bounding_box in case none is set.

inline bool has_bounding_box() const#

Determines if a bounding box is set.

Returns:

True if bounding box is set, False otherwise.

inline void agents_moved()#

Notify that some agents have moved, therefore the str tree need to be updated.

void record_collision(Entity *e1, Entity *e2)#

Record the collision between two entities.

Parameters:

  • e1 – The first entity

  • e2 – The second entity

inline void clear_collisions()#

Clear collisions.