rvosimulator.h

多机器人路径规划算法

/*! \file RVOSimulator.h Contains the RVOSimulator class; the main class of the RVO library. */ 

#ifndef __RVO_SIMULATOR_H__
#define __RVO_SIMULATOR_H__

#include <vector>
#include "vector2.h"
  
/*! Infinity. A sufficiently large float. */
#define RVO_INFTY 9e9f

// Error messages

/*! The function was successful. */
#define RVO_SUCCESS 0

/*! The simulation is already initialized when an attempt is made to add a goal. */
#define RVO_SIMULATION_ALREADY_INITIALIZED_WHEN_ADDING_GOAL -10

/*! The simulation is already initialized when an attempt is made to add an agent. */
#define RVO_SIMULATION_ALREADY_INITIALIZED_WHEN_ADDING_AGENT -11

/*! The simulation is already initialized when an attempt is made to add an obstacle. */
#define RVO_SIMULATION_ALREADY_INITIALIZED_WHEN_ADDING_OBSTACLE -12

/*! The simulation is already initialized when an attempt is made to add a roadmap vertex. */
#define RVO_SIMULATION_ALREADY_INITIALIZED_WHEN_ADDING_ROADMAP_VERTEX -13

/*! The simulation is already initialized when an attempt is made to add a roadmap edge. */
#define RVO_SIMULATION_ALREADY_INITIALIZED_WHEN_ADDING_ROADMAP_EDGE -14

/*! The agent defaults have not been set when an attempt is made to add an agent. */
#define RVO_AGENT_DEFAULTS_HAVE_NOT_BEEN_SET_WHEN_ADDING_AGENT -15

/*! The time step has not been set when an attempt is made to do a simulation step. */
#define RVO_TIME_STEP_NOT_SET_WHEN_DOING_STEP -16

/*! The simulation has not been initialized when an attempt is made to do a simulation step. */
#define RVO_SIMULATION_NOT_INITIALIZED_WHEN_DOING_STEP -17

/* \namespace RVO The RVO namespace. */
namespace RVO {
  class Roadmap;
  class KDTree;
  class Agent;
  class Obstacle;
  class Goal;
  
  /*! The main class of the RVO library. */
  class RVOSimulator {
  public:
    // NOTE: NO PUBLIC CONSTRUCTOR -- singleton model

    /*! Instantiates an RVOSimulator. 
        \returns The function returns a pointer to the singleton instance.*/
    static RVOSimulator* Instance();

    /*! Deconstructor of the RVOSimulator instance. */
    ~RVOSimulator();

    // Adders
    /*! Adds a goal position to the simulation.
     \param position The position of the goal
     \returns The function returns the ID of the goal that has been added. It returns an errorcode when the function is called after the simulation has been initialized.
     */
    int addGoal(const Vector2& position);

    /*! Sets the default parameters for agents that are subsequently added. 
     \param velSampleCountDefault The default number of candidate velocities sampled for the agent in each step of the simulation. The
        running time of the simulation increases linearly with this number.
     \param neighborDistDefault The default distance within which the agent take other agents and obstacles into account in the navigation. The larger this number, the larger the running
        time of the simulation. If the number is too low, the simulation will not be safe.
     \param maxNeighborsDefault The default maximum number of other agents and obstacles the agent takes into account in the navigation. The larger this number, the larger the running time of
            the simulation. If the number is too low, the simulation will not be safe.
     \param radiusDefault The default radius of the agents
     \param goalRadiusDefault The default goal radius of the agents
     \param prefSpeedDefault The default preferred speed of the agents
     \param maxSpeedDefault The default maximum speed of the agents
     \param safetyFactorDefault The default safety factor of the agents. I.e. the weight that is given to the 'time to collision' when penalizing a candidate
        velocity for the agent (vs. the distance to the preferred velocity). The higher this value, the 'safer'
        or the 'shyer' the agent is. The lower this value, the more 'aggressive' and 'reckless' the agent is. Its unit is distance.
     \param maxAccelDefault The default maximum acceleration of the agents
     \param velocityDefault The default initial velocity of the agents
     \param orientationDefault The default initial orientation of the agents
     \param classDefault The default class of the agent
     */
    void setAgentDefaults( int velSampleCountDefault, float neighborDistDefault, int maxNeighborsDefault, float radiusDefault, float goalRadiusDefault, float prefSpeedDefault, float maxSpeedDefault, float safetyFactorDefault, float maxAccelDefault = RVO_INFTY, const Vector2& velocityDefault = Vector2(0, 0), float orientationDefault = 0, int classDefault = 0 );
    
    /*! Adds an agent with default parameters to the simulation. 
     \param startPosition The start position of the agent
     \param goalID The ID of the goal of the agent
     \returns The function returns the ID of the agent that has been added, and an errorcode if the agent defaults have not been set, or when the function is called after the simulation has been initialized.
     */
    int addAgent(const Vector2& startPosition, int goalID);
    
    /*! Adds an agent with specified parameters to the simulation. 
     \param startPosition The start position of the agent
     \param goalID The ID of the goal of the agent
     \param velSampleCount The number of candidate velocities sampled for the agent in each step of the simulation. The
        running time of the simulation increases linearly with this number.
     \param neighborDist The distance within which the agent take other agents and obstacles into account in the navigation. The larger this number, the larger the running
        time of the simulation. If the number is too low, the simulation will not be safe.
     \param maxNeighbors The maximum number of other agents and obstacles the agent takes into account in the navigation. The larger this number, the larger the running time of
            the simulation. If the number is too low, the simulation will not be safe.
     \param radius The radius of the agent
     \param goalRadius The goal radius of the agent; an agent is said to have reached its goal when it is within a distance goalRadius from its goal position.
     \param prefSpeed The preferred speed of the agent
     \param maxSpeed The maximum speed of the agent
     \param safetyFactor The safety factor of the agent. I.e. the weight that is given to the 'time to collision' when penalizing a candidate
        velocity for the agent (vs. the distance to the preferred velocity). The higher this value, the 'safer'
        or the 'shyer' the agent is. The lower this value, the more 'aggressive' and 'reckless' the agent is. Its unit is distance.
     \param maxAccel The maximum acceleration of the agent
     \param velocity The initial velocity of the agent
     \param orientation The initial orientation of the agent
     \param classID The class of the agent; the class of an agent does not affect the simulation, but can be used in external applications to distinguish among agents 
     \returns The function returns the ID of the agent that has been added. It returns an errorcode when the function is called after the simulation has been initialized.
     */
    int addAgent(const Vector2& startPosition, int goalID, int velSampleCount, float neighborDist, int maxNeighbors, float radius, float goalRadius, float prefSpeed, float maxSpeed, float safetyFactor, float maxAccel = RVO_INFTY, const Vector2& velocity = Vector2(0, 0), float orientation = 0, int classID = 0 );
    
    /*! Adds a line segment obstacle to the simulation. The line segment may not intersect previously added line segments in their interior.
     \param point1 The position of the first endpoint of the line segment obstacle
     \param point2 The position of the second endpoint of the line segment obstacle  
     \returns The function returns the ID of the obstacle that has been added. It returns an errorcode when the function is called after the simulation has been initialized.
     */
    int addObstacle(const Vector2& point1, const Vector2& point2);

    /*! Sets whether the mutually visible vertices of the roadmap should automatically be connected by edges when the simulation is initialized. Default is false.
     \param automatic If true, the mutually visible vertices will be connected in addition to manually specified edges. If false, no edges will created in addition to the manually specified ones.
    */
    void setRoadmapAutomatic(bool automatic = true);

    /*! Adds a vertex to the roadmap of environment. The roadmap is used for global planning for the agents around obstacles.
     \param position The position of the roadmap vertex
     \returns The function returns the ID of the roadmap vertex that has been added. It returns an errorcode when the function is called after the simulation has been initialized.
    */
    int addRoadmapVertex(const Vector2& position);

    /*! Adds an edge between two vertices of the roadmap. The roadmap is undirected.
     \param vertexID1 The ID of the first vertex of the edge
     \param vertexID2 The ID of the second vertex of the edge
     \returns The function returns #RVO_SUCCESS when successful, and an errorcode when the function is called after the simulation has been initialized.
    */
    int addRoadmapEdge(int vertexID1, int vertexID2);

    /*! Processes the input (goals, agents, obstacles and roadmap) of the simulation. After initialization, no content can be added anymore to the environment.
     */
    void initSimulation();

    /*! Causes the simulator to take another simulation step. Modifies position, velocity and orientation of agents. Updates the global time of the simulation, and sets a 'reached goal' flag when all of the agents have reached their goal. 
     \returns The function returns #RVO_SUCCESS on success, and an errorcode when the simulation has not been initialized, or when the global parameters have not been set.
     */
    int doStep();

    /*! \returns The function returns true when all agents have reached their goal. Returns false otherwise. 
     */
    bool getReachedGoal() const {return _allAtGoals;}

    /*! \returns The function returns the current global time of the simulation. Is initially 0.  
    */
    float getGlobalTime() const { return _globalTime; }

    // Global Getter/Setter 's 

    /*! \returns The function returns the currently set time step of the simulation. */
    float getTimeStep() const {return _timeStep;}

    /*! Sets the time step of the simulation. 
        \param stepSize The new time step of the simulation.
        */
    void setTimeStep( float stepSize ) { _timeStep = stepSize; }

    
    // Agent Getter/Setter 's

    /*! \returns The function returns the number of agents in the simulation. */
    int getNumAgents() const;

    /*! Retrieving whether an agent has reached its goal.
        \param agentID The agent's ID
        \returns The function returns true when the specified agent has reached its goal. Returns false otherwise. */
    bool getAgentReachedGoal(int agentID) const;

    /*! Retrieving the current position of an agent.
        \param agentID The agent's ID
        \returns The function returns the current position of the specified agent. */
    const Vector2& getAgentPosition(int agentID) const;

    /*! Sets the current position of an agent.
        \param agentID The agent's ID
        \param position The new position of the agent
        */
    void setAgentPosition(int agentID, const Vector2& position);
    

    /*! Retrieving the current velocity of an agent.
        \param agentID The agent's ID
        \returns The function returns the current velocity of the specified agent. */
    const Vector2& getAgentVelocity(int agentID) const; 
    
    /*! Sets the current velocity of an agent.
        \param agentID The agent's ID
        \param velocity The new velocity of the agent
       */
    void setAgentVelocity(int agentID, const Vector2& velocity);

    
    /*! Retrieving the radius of an agent.
        \param agentID The agent's ID
        \returns The function returns the radius of the specified agent. */
    float getAgentRadius(int agentID) const;
    
    /*! Sets the radius of an agent.
        \param agentID The agent's ID
        \param radius The new radius of the agent
        */
    void setAgentRadius(int agentID, float radius);


    /*! Retrieving the sample number of an agent
        \param agentID The agent's ID
        \returns The function returns the currently set number of candidate velocities that is sampled for the specified agent in each step of the simulation. */
    int getAgentVelSampleCount(int agentID) const;
    
    /*! Sets the number of candidate velocities that is sampled for the specified agent in each step of the simulation.
        \param agentID The agent's ID
        \param samples The new number of samples.
        */
    void setAgentVelSampleCount(int agentID, int samples);


    /*! Retrieving the neighbor distance of an agent
        \param agentID The agent's ID
        \returns The function returns the currently set neighbor distance of the specified agent. */
    float getAgentNeighborDist(int agentID) const;
    
    /*! Sets the neighbor distance of the specified agent.
        \param agentID The agent's ID
        \param distance The new neighbor distance.
        */
    void setAgentNeighborDist(int agentID, float distance);


    /*! Retrieving the maximum number of neighbors of an agent.
        \param agentID The agent's ID
        \returns The function returns the currently set maximum number of neighbors of the specified agent. */
    int getAgentMaxNeighbors(int agentID) const;
    
    /*! Sets the maximum number of neighbors of the specified agent. 
        \param agentID The agent's ID
        \param maximum The new maximum number of neighbors.
        */
    void setAgentMaxNeighbors(int agentID, int maximum);
    
    
    /*! Retrieving the class of an agent.
        \param agentID The agent's ID
        \returns The function returns the class of the specified agent. */
    int getAgentClass(int agentID) const;
    
    /*! Sets the class of an agent.
        \param agentID The agent's ID
        \param classID The new class of the agent
        */
    void setAgentClass(int agentID, int classID);
    
    
    /*! Retrieving the current orientation of an agent.
        \param agentID The agent's ID
        \returns The function returns the current orientation of the specified agent.*/
    float getAgentOrientation(int agentID) const;
    
    /*! Sets the current orientation of an agent.