cloud_algos.h
Go to the documentation of this file.
00001 #ifndef CLOUD_ALGOS_H
00002 #define CLOUD_ALGOS_H
00003 //#include <Eigen/StdVector>
00004 
00005 #include <string>
00006 #include <vector>
00007 
00008 #include <ros/ros.h>
00009 #include <ros/node_handle.h>
00010 #include <sensor_msgs/PointCloud2.h>
00011 
00012 namespace pcl_cloud_algos
00013 {
00014 
00015 //int getChannelIndex
00016 //      (const sensor_msgs::PointCloud &points,
00017 //       std::string value);
00018 //int getChannelIndex
00019 //      (const boost::shared_ptr<const sensor_msgs::PointCloud> points,
00020 //       std::string value);
00021 
00022 class CloudAlgo
00023 {
00024  public:
00025   int verbosity_level_;
00026   bool output_valid_;
00027 
00028   CloudAlgo () { verbosity_level_ = INT_MAX; output_valid_ = true; };
00029 
00030   typedef void OutputType;
00031   typedef sensor_msgs::PointCloud2 InputType;
00032   static std::string default_output_topic () {return std::string("");};
00033   static std::string default_input_topic () {return std::string("");};
00034   static std::string default_node_name () {return std::string("");};
00035 
00036   virtual void init (ros::NodeHandle&) = 0;
00037 
00038   virtual void pre () = 0;
00039   virtual void post () = 0;
00040   virtual std::vector<std::string> requires () = 0;
00041   virtual std::vector<std::string> provides () = 0;
00042 //  virtual std::string process (const boost::shared_ptr<const InputType>&) = ;
00043   OutputType output ();
00044   virtual ros::Publisher createPublisher (ros::NodeHandle& nh) = 0;
00045 };
00046 
00047 template <class algo>
00048   class CloudAlgoNode
00049 {
00050  public:
00051   CloudAlgoNode (ros::NodeHandle& nh, algo &alg)
00052   : nh_ (nh), a (alg)
00053   {
00054     //check if input topic is advertised
00055     std::vector<ros::master::TopicInfo> t_list;
00056     bool topic_found = false;
00057     ros::master::getTopics (t_list);
00058     for (std::vector<ros::master::TopicInfo>::iterator it = t_list.begin (); it != t_list.end (); it++)
00059     {
00060       if (it->name == a.default_input_topic())
00061       {
00062         topic_found = true;
00063         break;
00064       }
00065     }
00066     if (!topic_found)
00067       ROS_WARN ("Trying to subscribe to %s, but the topic doesn't exist!", a.default_input_topic().c_str());
00068 
00069     pub_ = nh_.advertise <typename algo::OutputType> (a.default_output_topic (), 5);
00070     sub_ = nh_.subscribe (a.default_input_topic (), 1, &CloudAlgoNode<algo>::input_cb, this);
00071 
00072     ROS_INFO("CloudAlgoNode (%s) created. SUB [%s], PUB[%s]",
00073              //a.default_node_name().c_str(),
00074              ros::this_node::getName().c_str(),
00075              a.default_input_topic().c_str(),
00076              a.default_output_topic().c_str());
00077     a.init (nh_);
00078   }
00079 
00080   void input_cb (const boost::shared_ptr<const typename algo::InputType> &input)
00081   {
00082     ROS_INFO("Received message.");
00083     a.pre();
00084     ROS_INFO("Algo.pre () returned.");
00085     ROS_INFO(" ");
00086     std::string result = a.process (input);
00087     ROS_INFO(" ");
00088     ROS_INFO("Result got after processed message: %s", result.c_str ());
00089     if (a.output_valid_)
00090     {
00091       pub_.publish (a.output());
00092       ROS_INFO("Published result message.");
00093     }
00094     else
00095       ROS_ERROR("Not publishing result as it is invalid!");
00096     a.post();
00097     ROS_INFO("Algo.post () returned.\n");
00098   }
00099 
00100   ros::NodeHandle& nh_;
00101   ros::Publisher pub_;
00102   ros::Subscriber sub_;
00103 
00104   algo& a;
00105 };
00106 
00107 template <class algo>
00108   int standalone_node (int argc, char* argv[])
00109 {
00110   ros::init (argc, argv, algo::default_node_name());
00111   algo a;
00112   ros::NodeHandle nh ("~");
00113   CloudAlgoNode<algo> c(nh, a);
00114 
00115   ros::spin ();
00116 
00117   return (0);
00118 }
00119 
00120 }
00121 #endif
00122 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_algos
Author(s): Nico Blodow, Dejan Pangercic, Zoltan-Csaba Marton
autogenerated on Thu May 23 2013 15:44:48