Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <boost/thread/mutex.hpp>
00038 #include <dynamic_reconfigure/server.h>
00039 #include <geometry_msgs/PointStamped.h>
00040 #include <geometry_msgs/PolygonStamped.h>
00041 #include <jsk_pcl_ros/PointcloudScreenpointConfig.h>
00042 #include <jsk_recognition_msgs/TransformScreenpoint.h>
00043 #include <jsk_topic_tools/connection_based_nodelet.h>
00044 #include <message_filters/subscriber.h>
00045 #include <message_filters/sync_policies/approximate_time.h>
00046 #include <message_filters/sync_policies/exact_time.h>
00047 #include <message_filters/synchronizer.h>
00048 #include <pcl/features/normal_3d.h>
00049 #include <pcl/kdtree/kdtree_flann.h>
00050 #include <sensor_msgs/PointCloud2.h>
00051
00052 namespace mf = message_filters;
00053
00054 namespace jsk_pcl_ros
00055 {
00056 class PointcloudScreenpoint : public jsk_topic_tools::ConnectionBasedNodelet
00057 {
00058 protected:
00059 typedef PointcloudScreenpointConfig Config;
00060
00061
00062 typedef mf::sync_policies::ApproximateTime<
00063 sensor_msgs::PointCloud2,
00064 geometry_msgs::PolygonStamped > PolygonApproxSyncPolicy;
00065
00066 typedef mf::sync_policies::ApproximateTime<
00067 sensor_msgs::PointCloud2,
00068 geometry_msgs::PointStamped > PointApproxSyncPolicy;
00069
00070 typedef mf::sync_policies::ApproximateTime<
00071 sensor_msgs::PointCloud2,
00072 sensor_msgs::PointCloud2 > PointCloudApproxSyncPolicy;
00073
00074
00075 typedef mf::sync_policies::ExactTime<
00076 sensor_msgs::PointCloud2,
00077 geometry_msgs::PolygonStamped > PolygonExactSyncPolicy;
00078
00079 typedef mf::sync_policies::ExactTime<
00080 sensor_msgs::PointCloud2,
00081 geometry_msgs::PointStamped > PointExactSyncPolicy;
00082
00083 typedef mf::sync_policies::ExactTime<
00084 sensor_msgs::PointCloud2,
00085 sensor_msgs::PointCloud2 > PointCloudExactSyncPolicy;
00086
00087 virtual void onInit();
00088 virtual void subscribe();
00089 virtual void unsubscribe();
00090 virtual void configCallback(Config &config, uint32_t level);
00091
00092
00093 bool screenpoint_cb(jsk_recognition_msgs::TransformScreenpoint::Request &req,
00094 jsk_recognition_msgs::TransformScreenpoint::Response &res);
00095
00096
00097 void points_cb (const sensor_msgs::PointCloud2::ConstPtr &msg);
00098 void point_cb (const geometry_msgs::PointStamped::ConstPtr &pt_ptr);
00099 void point_array_cb (const sensor_msgs::PointCloud2::ConstPtr &pt_arr_ptr);
00100 void rect_cb (const geometry_msgs::PolygonStamped::ConstPtr &array_ptr);
00101 void poly_cb (const geometry_msgs::PolygonStamped::ConstPtr &array_ptr);
00102
00103
00104 void sync_point_cb (const sensor_msgs::PointCloud2::ConstPtr &points_ptr,
00105 const geometry_msgs::PointStamped::ConstPtr &pt_ptr);
00106 void sync_point_array_cb (const sensor_msgs::PointCloud2::ConstPtr &points_ptr,
00107 const sensor_msgs::PointCloud2::ConstPtr &pt_arr_ptr);
00108 void sync_rect_cb (const sensor_msgs::PointCloud2ConstPtr &points_ptr,
00109 const geometry_msgs::PolygonStamped::ConstPtr &array_ptr);
00110 void sync_poly_cb (const sensor_msgs::PointCloud2::ConstPtr &points_ptr,
00111 const geometry_msgs::PolygonStamped::ConstPtr &array_ptr);
00112
00113
00114 bool checkpoint (const pcl::PointCloud< pcl::PointXYZ > &in_pts,
00115 int x, int y,
00116 float &resx, float &resy, float &resz);
00117 bool extract_point (const pcl::PointCloud< pcl::PointXYZ > &in_pts,
00118 int reqx, int reqy,
00119 float &resx, float &resy, float &resz);
00120 void extract_rect (const pcl::PointCloud< pcl::PointXYZ > &in_pts,
00121 int st_x, int st_y,
00122 int ed_x, int ed_y,
00123 sensor_msgs::PointCloud2 &out_pts);
00124
00125
00126 boost::mutex mutex_;
00127 boost::shared_ptr<dynamic_reconfigure::Server<Config> > dyn_srv_;
00128 ros::Publisher pub_points_;
00129 ros::Publisher pub_point_;
00130 ros::Publisher pub_polygon_;
00131 ros::ServiceServer srv_;
00132 mf::Subscriber < sensor_msgs::PointCloud2 > points_sub_;
00133 mf::Subscriber < geometry_msgs::PolygonStamped > rect_sub_;
00134 mf::Subscriber < geometry_msgs::PointStamped > point_sub_;
00135 mf::Subscriber < sensor_msgs::PointCloud2 > point_array_sub_;
00136 mf::Subscriber < geometry_msgs::PolygonStamped > poly_sub_;
00137 boost::shared_ptr < mf::Synchronizer < PolygonApproxSyncPolicy > > async_rect_;
00138 boost::shared_ptr < mf::Synchronizer < PointApproxSyncPolicy > > async_point_;
00139 boost::shared_ptr < mf::Synchronizer < PointCloudApproxSyncPolicy > > async_point_array_;
00140 boost::shared_ptr < mf::Synchronizer < PolygonApproxSyncPolicy > > async_poly_;
00141
00142 boost::shared_ptr < mf::Synchronizer < PolygonExactSyncPolicy > > sync_rect_;
00143 boost::shared_ptr < mf::Synchronizer < PointExactSyncPolicy > > sync_point_;
00144 boost::shared_ptr < mf::Synchronizer < PointCloudExactSyncPolicy > > sync_point_array_;
00145 boost::shared_ptr < mf::Synchronizer < PolygonExactSyncPolicy > > sync_poly_;
00146
00147
00148 pcl::NormalEstimation< pcl::PointXYZ, pcl::Normal > n3d_;
00149 pcl::search::KdTree< pcl::PointXYZ >::Ptr normals_tree_;
00150
00151
00152 bool synchronization_, approximate_sync_;
00153 int queue_size_;
00154 int search_size_;
00155 int crop_size_;
00156 double timeout_;
00157
00158 std_msgs::Header latest_cloud_header_;
00159 pcl::PointCloud<pcl::PointXYZ> latest_cloud_;
00160
00161 };
00162 }
00163