00001 00026 #ifndef _TOPOLOGICAL_ACTION_MANAGER_H_ 00027 #define _TOPOLOGICAL_ACTION_MANAGER_H_ 00028 00029 #include <ros/ros.h> 00030 #include <ros/callback_queue_interface.h> 00031 #include <geometry_msgs/PoseStamped.h> 00032 00033 #include <predicate_manager/PredicateInfoMap.h> 00034 #include <predicate_manager/PredicateUpdate.h> 00035 #include <topological_tools/topological_map.h> 00036 00037 00038 00039 namespace topological_tools 00040 { 00041 class TopologicalActionManager 00042 { 00043 public: 00044 TopologicalActionManager ( const std::string& map_file, ros::CallbackQueueInterface* queue = NULL ); 00045 TopologicalActionManager ( TopologicalMap& tm, ros::CallbackQueueInterface* queue = NULL ); 00046 00047 boost::shared_ptr<TopologicalNode> getCurrentNode(); 00048 boost::shared_ptr<TopologicalNode> getGoalNodeForLabel ( const std::string& connection_label ); 00049 boost::shared_ptr<TopologicalNode> getGoalNodeForLabel ( const std::string& connection_label, 00050 const std::string& origin_name ); 00051 geometry_msgs::Pose getGoalPoseForLabel ( const std::string& connection_label ); 00052 geometry_msgs::Pose getGoalPoseForLabel ( const std::string& connection_label, 00053 const std::string& origin_name ); 00054 bool isInitialized(); 00055 private: 00056 void predicateMapCallback ( const predicate_manager::PredicateInfoMapConstPtr& msg ); 00057 void predicateUpdatesCallback ( const predicate_manager::PredicateUpdateConstPtr& msg ); 00058 void predicateUpdatesInternal ( const predicate_manager::PredicateUpdate& msg ); 00059 00060 ros::NodeHandle nh_; 00061 00062 ros::Subscriber pred_updates_sub_; 00063 ros::Subscriber pred_map_sub_; 00064 00065 TopologicalMap tm_; 00066 00067 std::map<uint32_t, std::string> pred_id_node_map_; 00068 00069 int pm_id_; 00070 std::string current_node_name_; 00072 predicate_manager::PredicateUpdate requested_update_; 00073 }; 00074 } 00075 00076 #endif