#include <predicate_manager.h>
Public Types | |
typedef std::vector < boost::shared_ptr< Event > > | EvRefVector |
maps local pred nrs to pred objects | |
typedef boost::unordered_map < NameID, ObserverPtr > | NameIDObserver |
A map from NameIDs to local dependants. The access time is O(1) on average (see boost::unordered_map). | |
typedef boost::unordered_map < NrID, ObserverPtr, cantor_pair_hash > | NrIDObserver |
A map from NrIDs to local dependants. The Cantor pairing function ensures O(1) access (no collisions). | |
typedef boost::shared_ptr < std::set< uint32_t > > | ObserverPtr |
An "Observer" is a set of local identifiers (predicate / event numbers). This type is used to represent local dependants. | |
typedef std::vector < boost::shared_ptr< Predicate > > | PredRefVector |
A vector of predicate references (used to represent the locally registered predicate objects). | |
typedef boost::unordered_map < NrID, bool, cantor_pair_hash > | ValueMap |
A map from NrIDs to the values of the respective predicates. | |
Public Member Functions | |
void | addEvent (Event &e) |
void | addPredicate (Predicate &p) |
void | eventTrigger (const uint32_t ev_nr) |
PredicateManager () | |
maps local pred nrs to pred objects | |
void | predicateUpdateTrigger (const NrID pred_nr_id, bool value) |
void | spin () |
Private Member Functions | |
void | consumePredicateMap (const PredicateInfoMapConstPtr &msg) |
void | consumePredicateUpdate (const PredicateUpdateConstPtr &msg) |
void | PMUpdate () |
void | prepareLocalDependencies () |
void | publishEventMap () |
void | publishEventUpdate () |
void | publishPredicateMap () |
void | publishPredicateUpdate () |
void | setDependencyReferences (const NrID pred_nr_id, const NameID pred_name_id) |
void | setNrOfPredicates (const int pm_id, const std::size_t size) |
void | updateDependants (const NrID pred_nr_id) |
Private Attributes | |
bool | any_changed_ |
Wether this PM has started spinning. | |
NameIDObserver | ev_name_observer_ |
The NameIDs of all registered local events. | |
NrIDObserver | ev_nr_observer_ |
The Event observer set for each Predicate, accessible through its NameID. | |
uint32_t | ev_update_counter_ |
ros::Publisher | event_maps_pub_ |
The Predicate observer set for each Predicate, accessible through its NrID. | |
ros::Publisher | event_updates_pub_ |
std::vector< uint32_t > | events_to_publish_ |
The Event observer set for each Predicate, accessible through its NrID. | |
std::vector< bool > | initialized_pms_ |
Vector containing the number of predicates for each known PM. | |
ValueMap | last_value_map_ |
Map containing the up-to-date value of all known Predicates. | |
EvRefVector | local_ev_refs_ |
A vector of integer tokens that act as a "synchronization key" for event updates. | |
PredRefVector | local_pred_refs_ |
A vector of integer tokens that act as a "synchronization key" for predicate updates. | |
ros::NodeHandle | nh_ |
std::vector< std::size_t > | nr_of_predicates_ |
The ID of this PM. | |
uint32_t | pm_id_ |
Whether there has been at least one Predicate that changed its value. | |
NameIDObserver | pred_name_observer_ |
The NameIDs of all registered local predicates. | |
NrIDObserver | pred_nr_observer_ |
The Predicate observer set for each Predicate, accessible through its NameID. | |
std::vector< uint32_t > | pred_update_counters_ |
Subscriber to the predicate updates of other PMs. | |
ros::Publisher | predicate_maps_pub_ |
The update period of this Predicate Manager. Default is 0.1s (10 Hz). | |
ros::Subscriber | predicate_maps_sub_ |
Publisher for this PM's predicate updates. | |
ros::Publisher | predicate_updates_pub_ |
Publisher for this PM's predicate map. | |
ros::Subscriber | predicate_updates_sub_ |
Subscriber to the predicate maps of other PMs. | |
NameIDSet | registered_events_ |
Vector of references to locally registered Event Objects. | |
NameIDSet | registered_predicates_ |
The value map according to the last published update. | |
bool | started_ |
double | update_period_ |
Vector containing the IDs of all known initialized PMs. | |
ValueMap | value_map_ |
Vector of references to locally registered Predicate Objects. |
The PredicateManager (PM) class handles Predicate interdependencies, and the communication of Predicate value updates. In a multiagent system, multiple PredicateManager instances may be used concurrently, each of them representing the knowledge that is locally available to each agent. Each active PM instance is identified by an integer ID, which you should specify as a "pm_id" integer parameter in the containing node's local namespace.
Predicate updates are communicated as arrays of unsigned integers (check PredicateUpdate.msg), and not by their name, in order to minimize the ammount of information that must be sent at each update. Predicates updates are published by default to the "/predicate_updates" topic (note the absolute path). A Predicate "map", which is translation table from predicate numbers to their names, is sent to "/predicate_maps" when the PM is initialized. Using these maps, a Predicate's number and the respective PM ID uniquely identifies a given predicate in the network.
Predicates in a PM can have external dependencies (i.e. Predicates of another PM). All active PMs will attempt to maintain synchrony even if the communication medium is not fully reliable.
Registered Predicates are woken up (by calling their update() functions) whenever one of their respective dependencies changes its value.
Definition at line 67 of file predicate_manager.h.
typedef std::vector<boost::shared_ptr<Event> > predicate_manager::PredicateManager::EvRefVector |
maps local pred nrs to pred objects
Definition at line 80 of file predicate_manager.h.
typedef boost::unordered_map<NameID, ObserverPtr> predicate_manager::PredicateManager::NameIDObserver |
A map from NameIDs to local dependants. The access time is O(1) on average (see boost::unordered_map).
Definition at line 73 of file predicate_manager.h.
typedef boost::unordered_map<NrID, ObserverPtr, cantor_pair_hash> predicate_manager::PredicateManager::NrIDObserver |
A map from NrIDs to local dependants. The Cantor pairing function ensures O(1) access (no collisions).
Definition at line 75 of file predicate_manager.h.
typedef boost::shared_ptr<std::set<uint32_t> > predicate_manager::PredicateManager::ObserverPtr |
An "Observer" is a set of local identifiers (predicate / event numbers). This type is used to represent local dependants.
Definition at line 71 of file predicate_manager.h.
typedef std::vector<boost::shared_ptr<Predicate> > predicate_manager::PredicateManager::PredRefVector |
A vector of predicate references (used to represent the locally registered predicate objects).
Definition at line 79 of file predicate_manager.h.
typedef boost::unordered_map<NrID, bool, cantor_pair_hash> predicate_manager::PredicateManager::ValueMap |
A map from NrIDs to the values of the respective predicates.
Definition at line 77 of file predicate_manager.h.
maps local pred nrs to pred objects
Standard constructor.
TODO: RequestPredicateMap e RequestPredicateUpdate service
Definition at line 40 of file predicate_manager.cpp.
void PredicateManager::addEvent | ( | Event & | e | ) |
Adds an Event to this PredicateManager. Events must be added before the PM starts spinning.
p | The predicate to be registered. |
Definition at line 217 of file predicate_manager.cpp.
void PredicateManager::addPredicate | ( | Predicate & | p | ) |
Adds a Predicate to this PredicateManager. Predicates must be added before the PM starts spinning.
p | The predicate to be registered. |
pred_name_observer_[pred_name_id] may have already been created by a dependant predicate / event
Definition at line 170 of file predicate_manager.cpp.
void PredicateManager::consumePredicateMap | ( | const PredicateInfoMapConstPtr & | msg | ) | [private] |
Consumes Predicate Maps from other Predicate Managers. Predicate dependencies are specified by name, but predicates are communicated as integers. This function establishes a correspondency between the identifiers of future predicate updates from the sender PM, and the local dependencies specified in this receiver PM.
Definition at line 292 of file predicate_manager.cpp.
void PredicateManager::consumePredicateUpdate | ( | const PredicateUpdateConstPtr & | msg | ) | [private] |
Consumes Predicate updates from other Predicate Managers. This updates the local value map and calls local dependent Predicates. If all messages are being correctly transmitted, this PM only uses the "rising" and "falling" predicate sets in the input message to update its values. Otherwise, all known Predicates of the sender PM are forcefully updated.
full update -- only done at initialization or if out of synch
reduced update
Definition at line 393 of file predicate_manager.cpp.
void PredicateManager::eventTrigger | ( | const uint32_t | ev_nr | ) |
A trigger function that is called by each Event when it is ready to be published.
ev_nr_id | The number of the calling event (bound during event registration). Calling events are always local. |
Definition at line 163 of file predicate_manager.cpp.
void PredicateManager::PMUpdate | ( | ) | [private] |
An update cycle of this PM. Checks for changed values and publishes predicate updates accordingly.
Definition at line 135 of file predicate_manager.cpp.
void PredicateManager::predicateUpdateTrigger | ( | const NrID | pred_nr_id, |
bool | value | ||
) |
A trigger function that is called by each Predicate as it updates its value. This prompts the PM to update the dependants of that Predicate.
pred_nr_id | The NrID of the calling predicate (bound during predicate registration); |
value | The new value of the calling predicate. |
Definition at line 152 of file predicate_manager.cpp.
void PredicateManager::prepareLocalDependencies | ( | ) | [private] |
Establishes the correct cross-references between local predicates. This must be done after adding all Predicates, and before spinning the PM.
Definition at line 489 of file predicate_manager.cpp.
void PredicateManager::publishEventMap | ( | ) | [private] |
Publishes this PM's Event Map to the "/event_maps" topic.
Definition at line 273 of file predicate_manager.cpp.
void PredicateManager::publishEventUpdate | ( | ) | [private] |
Publishes an update of this PM's events.
Definition at line 367 of file predicate_manager.cpp.
void PredicateManager::publishPredicateMap | ( | ) | [private] |
Publishes this PM's Predicate Map to the "/predicate_maps" topic.
Definition at line 254 of file predicate_manager.cpp.
void PredicateManager::publishPredicateUpdate | ( | ) | [private] |
Publishes an update of this PM's Predicate values.
Definition at line 319 of file predicate_manager.cpp.
void PredicateManager::setDependencyReferences | ( | const NrID | pred_nr_id, |
const NameID | pred_name_id | ||
) | [private] |
Binds a Predicate to its dependents. This allows the dependent Predicates/Events to get the values of its dependency from this PM.
pred_nr_id | The NrID of the target Predicate; |
pred_name_id | The NameID of the target Predicate. |
Definition at line 510 of file predicate_manager.cpp.
void PredicateManager::setNrOfPredicates | ( | const int | pm_id, |
const std::size_t | size | ||
) | [private] |
Records the number of predicates for a given PM. This is necessary to perform full updates.
pm_id | The ID of the target PM. |
size | The number of Predicates registered to that PM. This is inferred from that PM's predicate map. |
Definition at line 538 of file predicate_manager.cpp.
void PredicateManager::spin | ( | ) |
PredicateManager implements its own spin function to ensure correct initialization, and that updates are only published when needed. You *must* call this spin function in the parent node, instead of the default ros::spin().
Definition at line 80 of file predicate_manager.cpp.
void PredicateManager::updateDependants | ( | const NrID | pred_nr_id | ) | [private] |
Updates the dependants of a Predicate (the Predicates that depend on it).
pred_nr_id | The NrID of the Predicate whose dependants we want to update. |
Dependent predicates
Dependent events
Definition at line 455 of file predicate_manager.cpp.
bool predicate_manager::PredicateManager::any_changed_ [private] |
Wether this PM has started spinning.
Definition at line 186 of file predicate_manager.h.
The NameIDs of all registered local events.
Definition at line 216 of file predicate_manager.h.
The Event observer set for each Predicate, accessible through its NameID.
Definition at line 217 of file predicate_manager.h.
uint32_t predicate_manager::PredicateManager::ev_update_counter_ [private] |
Definition at line 213 of file predicate_manager.h.
The Predicate observer set for each Predicate, accessible through its NrID.
Event-related members:
Definition at line 210 of file predicate_manager.h.
Definition at line 211 of file predicate_manager.h.
std::vector<uint32_t> predicate_manager::PredicateManager::events_to_publish_ [private] |
The Event observer set for each Predicate, accessible through its NrID.
Definition at line 218 of file predicate_manager.h.
std::vector<bool> predicate_manager::PredicateManager::initialized_pms_ [private] |
Vector containing the number of predicates for each known PM.
Definition at line 190 of file predicate_manager.h.
Map containing the up-to-date value of all known Predicates.
Definition at line 203 of file predicate_manager.h.
A vector of integer tokens that act as a "synchronization key" for event updates.
Definition at line 214 of file predicate_manager.h.
A vector of integer tokens that act as a "synchronization key" for predicate updates.
Definition at line 201 of file predicate_manager.h.
Definition at line 183 of file predicate_manager.h.
std::vector<std::size_t> predicate_manager::PredicateManager::nr_of_predicates_ [private] |
The ID of this PM.
Definition at line 189 of file predicate_manager.h.
uint32_t predicate_manager::PredicateManager::pm_id_ [private] |
Whether there has been at least one Predicate that changed its value.
Definition at line 188 of file predicate_manager.h.
The NameIDs of all registered local predicates.
Definition at line 205 of file predicate_manager.h.
The Predicate observer set for each Predicate, accessible through its NameID.
Definition at line 206 of file predicate_manager.h.
std::vector<uint32_t> predicate_manager::PredicateManager::pred_update_counters_ [private] |
Subscriber to the predicate updates of other PMs.
Definition at line 200 of file predicate_manager.h.
The update period of this Predicate Manager. Default is 0.1s (10 Hz).
Predicate-related members:
Definition at line 195 of file predicate_manager.h.
Publisher for this PM's predicate updates.
Definition at line 197 of file predicate_manager.h.
Publisher for this PM's predicate map.
Definition at line 196 of file predicate_manager.h.
Subscriber to the predicate maps of other PMs.
Definition at line 198 of file predicate_manager.h.
Vector of references to locally registered Event Objects.
Definition at line 215 of file predicate_manager.h.
The value map according to the last published update.
Definition at line 204 of file predicate_manager.h.
bool predicate_manager::PredicateManager::started_ [private] |
Definition at line 185 of file predicate_manager.h.
double predicate_manager::PredicateManager::update_period_ [private] |
Vector containing the IDs of all known initialized PMs.
Definition at line 191 of file predicate_manager.h.
Vector of references to locally registered Predicate Objects.
Definition at line 202 of file predicate_manager.h.