82 lines
2.0 KiB
C++
82 lines
2.0 KiB
C++
/**
|
|
* @file ControlConfig.h
|
|
* @brief Class to describe a configuration of 2D agents that use PV models
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <map>
|
|
#include <vector>
|
|
#include "Testable.h"
|
|
#include "ControlPoint.h"
|
|
#include "VectorConfig.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Class for configs of 2D agent motion models that make up trajectories
|
|
* Provides a map of labeled robot poses, and means to access groups, such
|
|
* as the trajectory of a particular robot or obstacle.
|
|
*/
|
|
class ControlConfig : public Testable<ControlConfig> {
|
|
public:
|
|
/** an individual path object for an agent */
|
|
typedef std::vector<ControlPoint> path;
|
|
typedef std::map<std::string, path>::const_iterator const_iterator;
|
|
|
|
private:
|
|
/** main storage for points */
|
|
std::map<std::string, path> paths_;
|
|
|
|
public:
|
|
/** Basic Default Constructors */
|
|
ControlConfig() {}
|
|
ControlConfig(const ControlConfig& cfg) : paths_(cfg.paths_) {}
|
|
|
|
/** Default destructor */
|
|
virtual ~ControlConfig() {}
|
|
|
|
/** Standard print function with optional label */
|
|
virtual void print(const std::string& name="") const;
|
|
|
|
/** Equality up to a tolerance */
|
|
virtual bool equals(const ControlConfig& expected, double tol=1e-9) const;
|
|
|
|
/** Add a delta configuration to the config */
|
|
ControlConfig exmap(const VectorConfig & delta) const;
|
|
|
|
/** number of agents */
|
|
size_t size() const { return paths_.size(); }
|
|
|
|
/**
|
|
* Adds an agent to the config
|
|
* @param name is the name of the agent used for lookup
|
|
*/
|
|
void addAgent(const std::string& name);
|
|
|
|
/**
|
|
* Adds a point to a robot's trajectory,
|
|
* note that ordering is handled internally
|
|
* @param name is the name of the robot
|
|
* @param state is the ControlPoint to add
|
|
*/
|
|
void addPoint(const std::string& name, const ControlPoint& state);
|
|
|
|
/**
|
|
* returns the path of a particular robot
|
|
*/
|
|
path getPath(const std::string& agentID) const;
|
|
|
|
/**
|
|
* Returns true if agent is in the config
|
|
*/
|
|
bool involvesAgent(const std::string& agentID) const;
|
|
|
|
// clearing
|
|
void clear() { paths_.clear(); }
|
|
void clearAgent(const std::string& agentID);
|
|
};
|
|
} // \namespace gtsam
|
|
|