36 #define BOOST_PARAMETER_MAX_ARITY 7 38 #include <pcl/point_types.h> 39 #include <pcl/features/normal_3d_omp.h> 45 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
46 DiagnosticNodelet::onInit();
49 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
50 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
52 srv_->setCallback (f);
53 pub_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
56 *
pnh_,
"output/latest_time", 1);
58 *
pnh_,
"output/average_time", 1);
73 Config& config, uint32_t level)
81 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
88 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
89 cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
91 pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> impl(
num_of_threads_);
92 impl.setInputCloud(cloud);
93 pcl::search::KdTree<pcl::PointXYZRGB>::Ptr
94 tree (
new pcl::search::KdTree<pcl::PointXYZRGB> ());
95 impl.setSearchMethod (tree);
98 pcl::PointCloud<pcl::Normal>::Ptr
99 normal_cloud(
new pcl::PointCloud<pcl::Normal>);
100 impl.compute(*normal_cloud);
102 sensor_msgs::PointCloud2 ros_normal_cloud;
104 ros_normal_cloud.header = cloud_msg->header;
107 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
108 normal_xyz(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
109 normal_xyz->points.resize(cloud->points.size());
110 for (
size_t i = 0; i < normal_xyz->points.size(); i++) {
111 pcl::PointXYZRGBNormal
p;
112 p.x = cloud->points[i].x;
113 p.y = cloud->points[i].y;
114 p.z = cloud->points[i].z;
115 p.rgb = cloud->points[i].rgb;
116 p.normal_x = normal_cloud->points[i].normal_x;
117 p.normal_y = normal_cloud->points[i].normal_y;
118 p.normal_z = normal_cloud->points[i].normal_z;
119 normal_xyz->points[i] = p;
122 sensor_msgs::PointCloud2 ros_normal_xyz_cloud;
124 ros_normal_xyz_cloud.header = cloud_msg->header;
virtual void estimate(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
virtual void unsubscribe()
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
void publish(const boost::shared_ptr< M > &message) const
virtual ScopedWallDurationReporter reporter()
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
pcl_ros::FeatureConfig Config
virtual void configCallback(Config &config, uint32_t level)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::NormalEstimationOMP, nodelet::Nodelet)
ros::Publisher pub_with_xyz_
ros::Publisher pub_latest_time_
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
#define NODELET_DEBUG_STREAM(...)
ros::Publisher pub_average_time_
jsk_recognition_utils::WallDurationTimer timer_