37 #include <boost/thread/mutex.hpp> 38 #include <dynamic_reconfigure/server.h> 39 #include <geometry_msgs/PointStamped.h> 40 #include <geometry_msgs/PolygonStamped.h> 41 #include <jsk_pcl_ros/PointcloudScreenpointConfig.h> 42 #include <jsk_recognition_msgs/TransformScreenpoint.h> 48 #include <pcl/features/normal_3d.h> 49 #include <pcl/kdtree/kdtree_flann.h> 50 #include <sensor_msgs/PointCloud2.h> 59 typedef PointcloudScreenpointConfig
Config;
62 typedef mf::sync_policies::ApproximateTime<
63 sensor_msgs::PointCloud2,
66 typedef mf::sync_policies::ApproximateTime<
67 sensor_msgs::PointCloud2,
70 typedef mf::sync_policies::ApproximateTime<
71 sensor_msgs::PointCloud2,
75 typedef mf::sync_policies::ExactTime<
76 sensor_msgs::PointCloud2,
79 typedef mf::sync_policies::ExactTime<
80 sensor_msgs::PointCloud2,
83 typedef mf::sync_policies::ExactTime<
84 sensor_msgs::PointCloud2,
94 jsk_recognition_msgs::TransformScreenpoint::Response &
res);
97 void points_cb (
const sensor_msgs::PointCloud2::ConstPtr &
msg);
98 void point_cb (
const geometry_msgs::PointStamped::ConstPtr &pt_ptr);
99 void point_array_cb (
const sensor_msgs::PointCloud2::ConstPtr &pt_arr_ptr);
100 void rect_cb (
const geometry_msgs::PolygonStamped::ConstPtr &array_ptr);
101 void poly_cb (
const geometry_msgs::PolygonStamped::ConstPtr &array_ptr);
104 void sync_point_cb (
const sensor_msgs::PointCloud2::ConstPtr &points_ptr,
105 const geometry_msgs::PointStamped::ConstPtr &pt_ptr);
107 const sensor_msgs::PointCloud2::ConstPtr &pt_arr_ptr);
108 void sync_rect_cb (
const sensor_msgs::PointCloud2ConstPtr &points_ptr,
109 const geometry_msgs::PolygonStamped::ConstPtr &array_ptr);
110 void sync_poly_cb (
const sensor_msgs::PointCloud2::ConstPtr &points_ptr,
111 const geometry_msgs::PolygonStamped::ConstPtr &array_ptr);
114 bool checkpoint (
const pcl::PointCloud< pcl::PointXYZ > &in_pts,
116 float &resx,
float &resy,
float &resz);
117 bool extract_point (
const pcl::PointCloud< pcl::PointXYZ > &in_pts,
119 float &resx,
float &resy,
float &resz);
120 void extract_rect (
const pcl::PointCloud< pcl::PointXYZ > &in_pts,
123 sensor_msgs::PointCloud2 &out_pts);
148 pcl::NormalEstimation< pcl::PointXYZ, pcl::Normal >
n3d_;
virtual void unsubscribe()
pcl::NormalEstimation< pcl::PointXYZ, pcl::Normal > n3d_
boost::shared_ptr< mf::Synchronizer< PolygonExactSyncPolicy > > sync_rect_
boost::shared_ptr< mf::Synchronizer< PolygonApproxSyncPolicy > > async_poly_
PointcloudScreenpointConfig Config
void sync_rect_cb(const sensor_msgs::PointCloud2ConstPtr &points_ptr, const geometry_msgs::PolygonStamped::ConstPtr &array_ptr)
mf::sync_policies::ExactTime< sensor_msgs::PointCloud2, geometry_msgs::PointStamped > PointExactSyncPolicy
mf::Subscriber< geometry_msgs::PointStamped > point_sub_
mf::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > PointCloudApproxSyncPolicy
std_msgs::Header latest_cloud_header_
boost::shared_ptr< mf::Synchronizer< PointApproxSyncPolicy > > async_point_
bool extract_point(const pcl::PointCloud< pcl::PointXYZ > &in_pts, int reqx, int reqy, float &resx, float &resy, float &resz)
boost::shared_ptr< mf::Synchronizer< PointExactSyncPolicy > > sync_point_
void point_array_cb(const sensor_msgs::PointCloud2::ConstPtr &pt_arr_ptr)
pcl::search::KdTree< pcl::PointXYZ >::Ptr normals_tree_
bool checkpoint(const pcl::PointCloud< pcl::PointXYZ > &in_pts, int x, int y, float &resx, float &resy, float &resz)
boost::shared_ptr< mf::Synchronizer< PolygonExactSyncPolicy > > sync_poly_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > dyn_srv_
void sync_point_cb(const sensor_msgs::PointCloud2::ConstPtr &points_ptr, const geometry_msgs::PointStamped::ConstPtr &pt_ptr)
mf::Subscriber< geometry_msgs::PolygonStamped > rect_sub_
void point_cb(const geometry_msgs::PointStamped::ConstPtr &pt_ptr)
mf::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > PointCloudExactSyncPolicy
void sync_point_array_cb(const sensor_msgs::PointCloud2::ConstPtr &points_ptr, const sensor_msgs::PointCloud2::ConstPtr &pt_arr_ptr)
virtual void configCallback(Config &config, uint32_t level)
ros::Publisher pub_point_
bool screenpoint_cb(jsk_recognition_msgs::TransformScreenpoint::Request &req, jsk_recognition_msgs::TransformScreenpoint::Response &res)
boost::mutex mutex
global mutex.
mf::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, geometry_msgs::PolygonStamped > PolygonApproxSyncPolicy
mf::Subscriber< sensor_msgs::PointCloud2 > points_sub_
mf::sync_policies::ExactTime< sensor_msgs::PointCloud2, geometry_msgs::PolygonStamped > PolygonExactSyncPolicy
ros::Publisher pub_points_
void extract_rect(const pcl::PointCloud< pcl::PointXYZ > &in_pts, int st_x, int st_y, int ed_x, int ed_y, sensor_msgs::PointCloud2 &out_pts)
mf::Subscriber< geometry_msgs::PolygonStamped > poly_sub_
void points_cb(const sensor_msgs::PointCloud2::ConstPtr &msg)
void poly_cb(const geometry_msgs::PolygonStamped::ConstPtr &array_ptr)
boost::shared_ptr< mf::Synchronizer< PolygonApproxSyncPolicy > > async_rect_
pcl::PointCloud< pcl::PointXYZ > latest_cloud_
mf::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, geometry_msgs::PointStamped > PointApproxSyncPolicy
void rect_cb(const geometry_msgs::PolygonStamped::ConstPtr &array_ptr)
void sync_poly_cb(const sensor_msgs::PointCloud2::ConstPtr &points_ptr, const geometry_msgs::PolygonStamped::ConstPtr &array_ptr)
boost::shared_ptr< mf::Synchronizer< PointCloudApproxSyncPolicy > > async_point_array_
ros::Publisher pub_polygon_
boost::shared_ptr< mf::Synchronizer< PointCloudExactSyncPolicy > > sync_point_array_
mf::Subscriber< sensor_msgs::PointCloud2 > point_array_sub_