00001 #ifndef IMAGE_ALGOS_H
00002 #define IMAGE_ALGOS_H
00003
00004 #include <string>
00005 #include <vector>
00006
00007 #include <ros/ros.h>
00008 #include <ros/node_handle.h>
00009 #include <sensor_msgs/Image.h>
00010
00011 namespace image_algos
00012 {
00013
00014
00015 class ImageAlgo
00016 {
00017 public:
00018 ImageAlgo () {};
00019
00020 typedef void OutputType;
00021 typedef sensor_msgs::Image InputType;
00022 static std::string default_output_topic () {return std::string("");};
00023 static std::string default_input_topic () {return std::string("");};
00024 static std::string default_node_name () {return std::string("");};
00025
00026 virtual void init (ros::NodeHandle&) = 0;
00027
00028 virtual void pre () = 0;
00029 virtual void post () = 0;
00030 virtual std::vector<std::string> requires () = 0;
00031 virtual std::vector<std::string> provides () = 0;
00032
00033 OutputType output ();
00034 };
00035
00036 template <class algo>
00037 class ImageAlgoNode
00038 {
00039 public:
00040 ImageAlgoNode (ros::NodeHandle& nh, algo &alg)
00041 : nh_ (nh), a (alg)
00042 {
00043
00044 std::vector<ros::master::TopicInfo> t_list;
00045 bool topic_found = false;
00046 ros::master::getTopics (t_list);
00047 for (std::vector<ros::master::TopicInfo>::iterator it = t_list.begin (); it != t_list.end (); it++)
00048 {
00049 if (it->name == a.default_input_topic())
00050 {
00051 topic_found = true;
00052 break;
00053 }
00054 }
00055 if (!topic_found)
00056 ROS_WARN ("Trying to subscribe to %s, but the topic doesn't exist!", a.default_input_topic().c_str());
00057
00058 pub_ = nh_.advertise <typename algo::OutputType> (a.default_output_topic (), 5);
00059 sub_ = nh_.subscribe (a.default_input_topic (), 1, &ImageAlgoNode<algo>::input_cb, this);
00060
00061 ROS_INFO("ImageAlgoNode (%s) created. SUB [%s], PUB[%s]",
00062 a.default_node_name().c_str(),
00063 a.default_input_topic().c_str(),
00064 a.default_output_topic().c_str());
00065 a.init (nh_);
00066 }
00067
00068 void input_cb (const boost::shared_ptr<const typename algo::InputType> &input)
00069 {
00070 ROS_INFO("Received message.");
00071 a.pre();
00072 ROS_INFO("Algo.pre () returned.");
00073 ROS_INFO("");
00074 a.process (input);
00075 ROS_INFO("");
00076 ROS_INFO("Processed message.");
00077 a.post();
00078 ROS_INFO("Algo.post () returned.");
00079 pub_.publish (a.output());
00080 ROS_INFO("Published result message.\n");
00081 }
00082
00083 ros::NodeHandle& nh_;
00084 ros::Publisher pub_;
00085 ros::Subscriber sub_;
00086
00087 algo& a;
00088 };
00089
00090 template <class algo>
00091 int standalone_node (int argc, char* argv[])
00092 {
00093 ros::init (argc, argv, algo::default_node_name());
00094 algo a;
00095 ros::NodeHandle nh ("~");
00096 ImageAlgoNode<algo> c(nh, a);
00097
00098 ros::spin ();
00099
00100 return (0);
00101 }
00102
00103 }
00104 #endif
00105
