00001 00025 #ifndef _STATE_LAYER_H_ 00026 #define _STATE_LAYER_H_ 00027 00028 #include <string> 00029 00030 #include <ros/ros.h> 00031 00032 #include <predicate_manager/predicate_manager.h> 00033 #include <predicate_manager/PredicateInfoMap.h> 00034 #include <predicate_manager/PredicateUpdate.h> 00035 00036 #include <mdm_library/state_dep.h> 00037 00038 00039 00040 namespace mdm_library 00041 { 00042 class StateLayer 00043 { 00044 public: 00045 typedef boost::shared_ptr<std::set<uint32_t> > SFIDSetPtr; 00046 00047 StateLayer(); 00048 00049 void predicateUpdatesCallback ( const predicate_manager::PredicateUpdateConstPtr& msg ); 00050 00051 void predicateMapCallback ( const predicate_manager::PredicateInfoMapConstPtr& msg ); 00052 00053 void addStateFactor ( const StateDep::SFDeps& deps ); 00054 void addStateFactor ( const StateDep& deps ); 00055 private: 00056 void publishJointState(); 00057 std::string factorString ( unsigned k ); 00058 void updateStateInfo ( const uint32_t& factor ); 00059 00060 ros::NodeHandle nh_; 00061 ros::Subscriber pred_map_sub_; 00062 ros::Subscriber pred_update_sub_; 00063 ros::Subscriber state_metadata_sub_; 00064 ros::Publisher state_pub_; 00065 00066 boost::unordered_map<predicate_manager::NrID, 00067 boost::shared_ptr<bool>, 00068 predicate_manager::cantor_pair_hash> nr_value_map_; 00069 boost::unordered_map<predicate_manager::NameID, boost::shared_ptr<bool> > named_value_map_; 00070 00071 //Each pair <Agent ID, Predicate ID> may be associated to a set of dependent factors 00072 boost::unordered_map<predicate_manager::NrID, 00073 SFIDSetPtr, 00074 predicate_manager::cantor_pair_hash> nr_pred_observer_; 00075 boost::unordered_map<predicate_manager::NameID, SFIDSetPtr > named_pred_observer_; 00076 00077 std::vector<StateDep::SFDeps> factored_state_deps_; 00078 00079 std::vector<uint32_t> factored_state_; //in state factors 00080 }; 00081 } 00082 00083 #endif