00001 00009 /***************************************************************************** 00010 ** Ifdefs 00011 *****************************************************************************/ 00012 00013 #ifndef YUJIN_OCS_CMD_VEL_SUBSCRIBERS_HPP_ 00014 #define YUJIN_OCS_CMD_VEL_SUBSCRIBERS_HPP_ 00015 00016 /***************************************************************************** 00017 ** Includes 00018 *****************************************************************************/ 00019 00020 #include <ros/ros.h> 00021 #include <geometry_msgs/Twist.h> 00022 #include <yaml-cpp/yaml.h> 00023 00024 #ifdef HAVE_NEW_YAMLCPP 00025 // The >> operator disappeared in yaml-cpp 0.5, so this function is 00026 // added to provide support for code written under the yaml-cpp 0.3 API. 00027 template<typename T> 00028 void operator >> (const YAML::Node& node, T& i) 00029 { 00030 i = node.as<T>(); 00031 } 00032 #endif 00033 00034 /***************************************************************************** 00035 ** Preprocessing 00036 *****************************************************************************/ 00037 00038 // move to a static const? 00039 #define VACANT std::numeric_limits<unsigned int>::max() 00040 00041 /***************************************************************************** 00042 ** Namespaces 00043 *****************************************************************************/ 00044 00045 namespace yocs_cmd_vel_mux { 00046 00047 /***************************************************************************** 00048 ** CmdVelSubscribers 00049 *****************************************************************************/ 00050 00054 class CmdVelSubscribers 00055 { 00056 public: 00057 00061 class CmdVelSubs 00062 { 00063 public: 00064 unsigned int idx; 00065 std::string name; 00066 ros::Subscriber subs; 00067 std::string topic; 00068 ros::Timer timer; 00069 double timeout; 00070 unsigned int priority; 00071 std::string short_desc; 00072 bool active; 00074 CmdVelSubs(unsigned int idx) : idx(idx), active(false) {}; 00075 00076 void operator << (const YAML::Node& node); 00077 }; 00078 00079 CmdVelSubscribers() : allowed(VACANT) { } 00080 ~CmdVelSubscribers() { } 00081 00082 std::vector<CmdVelSubs>::size_type size() { return list.size(); }; 00083 CmdVelSubs& operator [] (unsigned int idx) { return list[idx]; }; 00084 00093 void configure(const YAML::Node& node); 00094 00095 unsigned int allowed; 00096 00097 private: 00098 std::vector<CmdVelSubs> list; 00099 }; 00100 00101 } // namespace yocs_cmd_vel_mux 00102 00103 #endif /* CMD_VEL_SUBSCRIBERS_HPP_ */