109 lines
3.0 KiB
C++
109 lines
3.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:
|
|
/** allow for shared pointers */
|
|
typedef boost::shared_ptr<ControlConfig> shared_config;
|
|
|
|
/** 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
|
|
* @param index is the index in the trajectory to insert the point (defaults to
|
|
* pushing to the back of the trajectory)
|
|
*/
|
|
void addPoint(const std::string& name, const ControlPoint& state, int index=-1);
|
|
|
|
/**
|
|
* returns the path of a particular robot
|
|
*/
|
|
path getPath(const std::string& agentID) const;
|
|
|
|
/** get a vector in the configuration by key */
|
|
ControlPoint get(const std::string& key) 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);
|
|
|
|
/**
|
|
* Generates a key for a key
|
|
* @param name is the name of the agent
|
|
* @param num is the sequence number of the robot
|
|
* @return a key in the form [name]_[num]
|
|
*/
|
|
static std::string nameGen(const std::string& name, size_t num);
|
|
|
|
/**
|
|
* Compares two values of a config
|
|
* Used for creating NonlinearEqualities
|
|
* @param key identifier for the constrained variable
|
|
* @param feasible defines the feasible set
|
|
* @param input is the config to compare
|
|
* @return true if the selected value in feasible equals the input config
|
|
*/
|
|
static bool compareConfigState(const std::string& key,
|
|
const ControlConfig& feasible, const ControlConfig& input);
|
|
};
|
|
} // \namespace gtsam
|
|
|