00001 /* 00002 * AbstractGraphNode.hpp 00003 * 00004 * Created on: Nov 12, 2011 00005 * Author: mriedel 00006 */ 00007 00008 #ifndef ABSTRACTGRAPHNODE_HPP_ 00009 #define ABSTRACTGRAPHNODE_HPP_ 00010 00011 #include <telekyb_defines/telekyb_defines.hpp> 00012 #include <tk_behavior/Behavior.hpp> 00013 00014 // plugin stuff 00015 #include <pluginlib/class_list_macros.h> 00016 00017 #include "Neighbor.hpp" 00018 00019 using namespace TELEKYB_NAMESPACE; 00020 00021 namespace telekyb_behavior { 00022 00023 class AbstractGraphNode : public Behavior { 00024 protected: 00025 int nodeID; // id of node 00026 std::map<int, Neighbor*> neighbors; // "ID of neighboring Behaviors" 00027 00028 ros::NodeHandle nodeHandle; 00029 00030 ros::Publisher vpPublisher; 00031 ros::Publisher initDonePublisher; 00032 ros::Publisher formationConditionDonePublisher; 00033 00034 // Helper Functions 00035 void publishVP(const Eigen::Vector3d& virtualPoint) const; 00036 void publishInitDone(bool initDone_ = true) const; 00037 void publishConditionDone(bool conditionDone_ = true) const; 00038 00039 public: 00040 AbstractGraphNode(const std::string& name_, const BehaviorType& type_); 00041 virtual ~AbstractGraphNode(); 00042 00043 //virtual void initialize(); 00044 //virtual void destroy(); 00045 //virtual bool willBecomeActive(const TKState& currentState); 00046 //virtual void willBecomeInActive(); 00047 //virtual bool trajectoryStep(const TKState& currentState, TKTrajInput& generatedTrajInput); 00048 00049 void addNeighbor(int robotID); 00050 void removeNeighbor(int robotID); 00051 00052 bool allNeighborsValid() const; 00053 00054 00055 00056 }; 00057 00058 } 00059 00060 #endif /* ABSTRACTGRAPHNODE_HPP_ */