points_rectangle_extractor.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <message_filters/subscriber.h>
00004 #include <message_filters/synchronizer.h>
00005 #include <message_filters/sync_policies/exact_time.h>
00006 #include <message_filters/sync_policies/approximate_time.h>
00007 
00008 #include <sensor_msgs/Image.h>
00009 #include <sensor_msgs/CameraInfo.h>
00010 #include <sensor_msgs/PointCloud2.h>
00011 
00012 //#include <image_view2/PointArrayStamped.h>
00013 #include <geometry_msgs/PointStamped.h>
00014 #include <geometry_msgs/PolygonStamped.h>
00015 
00016 class PointsRectExtractor
00017 {
00018   typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2,
00019                                                            geometry_msgs::PolygonStamped > PolygonApproxSyncPolicy;
00020 
00021   typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2,
00022                                                            geometry_msgs::PointStamped > PointApproxSyncPolicy;
00023 
00024 private:
00025   ros::NodeHandle pnode_;
00026   ros::Publisher pub_;
00027   int queue_size;
00028   int crop_width;
00029 
00030   message_filters::Subscriber < sensor_msgs::PointCloud2 > points_sub_;
00031   message_filters::Subscriber < geometry_msgs::PolygonStamped > rect_sub_;
00032   message_filters::Subscriber < geometry_msgs::PointStamped > point_sub_;
00033 
00034   boost::shared_ptr < message_filters::Synchronizer < PolygonApproxSyncPolicy > > sync_a_polygon_;
00035   boost::shared_ptr < message_filters::Synchronizer < PointApproxSyncPolicy > > sync_a_point_;
00036 
00037 public:
00038   PointsRectExtractor() : pnode_("~"), queue_size(200), crop_width(2)
00039   {
00040     rect_sub_.subscribe (pnode_, "rect", queue_size);
00041     point_sub_.subscribe (pnode_, "point", queue_size);
00042     points_sub_.subscribe (pnode_, "points", queue_size);
00043 
00044     sync_a_polygon_ = boost::make_shared < message_filters::Synchronizer< PolygonApproxSyncPolicy > > (queue_size);
00045     sync_a_polygon_->connectInput (points_sub_, rect_sub_);
00046     sync_a_polygon_->registerCallback (boost::bind (&PointsRectExtractor::callback_polygon, this, _1, _2));
00047 
00048     sync_a_point_ = boost::make_shared < message_filters::Synchronizer< PointApproxSyncPolicy > > (queue_size);
00049     sync_a_point_->connectInput (points_sub_, point_sub_);
00050     sync_a_point_->registerCallback (boost::bind (&PointsRectExtractor::callback_point, this, _1, _2));
00051 
00052     pub_ = pnode_.advertise< sensor_msgs::PointCloud2 > ("output", 1);
00053   }
00054 
00055   void callback_point(const sensor_msgs::PointCloud2ConstPtr& points_ptr,
00056                       const geometry_msgs::PointStampedConstPtr& array_ptr) {
00057     int st_x = array_ptr->point.x - crop_width;
00058     int st_y = array_ptr->point.y - crop_width;
00059     int ed_x = array_ptr->point.x + crop_width;
00060     int ed_y = array_ptr->point.y + crop_width;
00061 
00062     int wd = points_ptr->width;
00063     int ht = points_ptr->height;
00064     int rstep = points_ptr->row_step;
00065     int pstep = points_ptr->point_step;
00066     if (st_x < 0) st_x = 0;
00067     if (st_y < 0) st_x = 0;
00068     if (ed_x >= wd) ed_x = wd - 1;
00069     if (ed_y >= ht) ed_y = ht - 1;
00070 
00071     sensor_msgs::PointCloud2 pt;
00072     pt.header = points_ptr->header;
00073     pt.width = ed_x - st_x + 1;
00074     pt.height = ed_y - st_y + 1;
00075     pt.row_step = pt.width * pstep;
00076     pt.point_step = pstep;
00077     pt.is_bigendian = false;
00078     pt.fields = points_ptr->fields;
00079     pt.is_dense = false;
00080     pt.data.resize(pt.row_step * pt.height);
00081 
00082     unsigned char * dst_ptr = &(pt.data[0]);
00083 
00084     for (size_t idx_y = st_y; idx_y <= ed_y; idx_y++) {
00085       for (size_t idx_x = st_x; idx_x <= ed_x; idx_x++) {
00086         const unsigned char * src_ptr = &(points_ptr->data[idx_y * rstep + idx_x * pstep]);
00087         memcpy(dst_ptr, src_ptr, pstep);
00088         dst_ptr += pstep;
00089       }
00090     }
00091     pub_.publish (pt);
00092   }
00093 
00094   void callback_polygon(const sensor_msgs::PointCloud2ConstPtr& points_ptr,
00095                         const geometry_msgs::PolygonStampedConstPtr& array_ptr) {
00096     // Solve all of perception here...
00097     ROS_DEBUG("callback");
00098 #if 0
00099     if(pub_.getNumSubscribers()==0 ){
00100       ROS_INFO("number of subscribers is 0, ignoring image");
00101       return;
00102     }
00103 #endif
00104 
00105     if (array_ptr->polygon.points.size() > 1) {
00106       int st_x = array_ptr->polygon.points[0].x;
00107       int st_y = array_ptr->polygon.points[0].y;
00108       int ed_x = array_ptr->polygon.points[1].x;
00109       int ed_y = array_ptr->polygon.points[1].y;
00110       int wd = points_ptr->width;
00111       int ht = points_ptr->height;
00112       int rstep = points_ptr->row_step;
00113       int pstep = points_ptr->point_step;
00114 
00115       sensor_msgs::PointCloud2 pt;
00116       pt.header = points_ptr->header;
00117       pt.width = ed_x - st_x + 1;
00118       pt.height = ed_y - st_y + 1;
00119       pt.row_step = pt.width * pstep;
00120       pt.point_step = pstep;
00121       pt.is_bigendian = false;
00122       pt.fields = points_ptr->fields;
00123       pt.is_dense = false;
00124       pt.data.resize(pt.row_step * pt.height);
00125 
00126       unsigned char * dst_ptr = &(pt.data[0]);
00127 
00128       for (size_t idx_y = st_y; idx_y <= ed_y; idx_y++) {
00129         for (size_t idx_x = st_x; idx_x <= ed_x; idx_x++) {
00130           const unsigned char * src_ptr = &(points_ptr->data[idx_y * rstep + idx_x * pstep]);
00131           memcpy(dst_ptr, src_ptr, pstep);
00132           dst_ptr += pstep;
00133         }
00134       }
00135       pub_.publish (pt);
00136     }
00137   }
00138 };
00139 
00140 int main (int argc, char **argv)
00141 {
00142   ros::init (argc, argv, "points_rectangle_extractor");
00143 
00144   if(!ros::master::check())
00145     return -1;
00146 
00147   PointsRectExtractor vs;
00148 
00149   ros::spin();
00150 
00151   return 0;
00152 }


image_view2
Author(s): Kei Okada
autogenerated on Sun Jan 25 2015 12:38:01