split_hypothesis_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <pcl17/console/print.h>
00003 #include <pcl17/console/parse.h>
00004 #include <furniture_classification/Hypothesis.h>
00005 
00006 class SplitHypothesis {
00007 public:
00008         SplitHypothesis(int num_splits) {
00009                 ros::NodeHandle nh;
00010 
00011                 for(int i=0; i<num_splits; i++){
00012                         pubs.push_back(nh.advertise<furniture_classification::Hypothesis>("/furniture_hypothesis" + boost::lexical_cast<std::string>(i), 1));
00013                 }
00014 
00015                 sub_hp = nh.subscribe < furniture_classification::Hypothesis
00016                                 > ("/furniture_hypothesis", 1, &SplitHypothesis::cloud_hp, this);
00017 
00018         }
00019 
00020         void cloud_hp(
00021                         const typename furniture_classification::Hypothesis::ConstPtr & msg) {
00022 
00023 
00024                 int num_points_in_message = msg->classes.size()/pubs.size();
00025 
00026                 for(int i=0; i<pubs.size()-1; i++){
00027                         furniture_classification::Hypothesis::Ptr h(new furniture_classification::Hypothesis);
00028 
00029                         h->classes.insert(h->classes.begin(), msg->classes.begin()+i*num_points_in_message, msg->classes.begin()+(i+1)*num_points_in_message);
00030                         h->poses.insert(h->poses.begin(), msg->poses.begin()+i*num_points_in_message, msg->poses.begin()+(i+1)*num_points_in_message);
00031                         pubs[i].publish(h);
00032                 }
00033 
00034                 furniture_classification::Hypothesis::Ptr h(new furniture_classification::Hypothesis);
00035 
00036                 h->classes.insert(h->classes.begin(), msg->classes.end()-num_points_in_message, msg->classes.end());
00037                 h->poses.insert(h->poses.begin(), msg->poses.end()-num_points_in_message, msg->poses.end());
00038                 pubs[pubs.size()-1].publish(h);
00039 
00040         }
00041 
00042         std::vector<ros::Publisher> pubs;
00043         ros::Subscriber sub_hp;
00044 
00045 };
00046 
00047 int main(int argc, char **argv) {
00048 
00049         if (argc < 3) {
00050                 PCL17_INFO(
00051                                 "Usage %s -num_splits N \n", argv[0]);
00052                 return -1;
00053         }
00054 
00055         ros::init(argc, argv, "split_hypothesis");
00056 
00057         int num_splits = 2;
00058         pcl17::console::parse_argument(argc, argv, "-num_splits", num_splits);
00059 
00060         SplitHypothesis sh(num_splits);
00061         ros::spin();
00062 
00063         return 0;
00064 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


furniture_classification
Author(s): Vladyslav Usenko
autogenerated on Thu May 23 2013 18:32:29