36 #define BOOST_PARAMETER_MAX_ARITY 7 46 DiagnosticNodelet::onInit();
47 pub_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
62 const sensor_msgs::PointCloud2::ConstPtr& msg)
65 pcl::PointCloud<pcl::PointXYZ>
cloud;
68 pcl::PointCloud<pcl::PointXYZI> colorized_cloud;
69 for (
size_t i = 0; i < cloud.points.size(); i++) {
70 pcl::PointXYZ in = cloud.points[i];
71 if (isnan(in.x) || isnan(in.y) || isnan(in.z)) {
79 colorized_cloud.points.push_back(out);
81 sensor_msgs::PointCloud2 ros_cloud;
83 ros_cloud.header = msg->header;
void publish(const boost::shared_ptr< M > &message) const
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::ColorizeHeight2DMapping, nodelet::Nodelet)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void colorize(const sensor_msgs::PointCloud2::ConstPtr &msg)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
virtual void unsubscribe()