36 #define BOOST_PARAMETER_MAX_ARITY 7 38 #include <std_msgs/Float32.h> 40 #include <pcl/common/common.h> 45 template <
class Po
intT>
49 typename pcl::PointCloud<PointT>::Ptr
cloud (
new pcl::PointCloud<PointT> ());
50 typename pcl::PointCloud<PointT>::Ptr cloud_voxeled (
new pcl::PointCloud<PointT> ());
54 typename pcl::octree::OctreePointCloud<PointT> octree(
resolution_);
56 octree.setInputCloud(cloud);
57 octree.addPointsFromInputCloud();
59 typename pcl::octree::OctreePointCloud<PointT>::AlignedPointTVector point_vec;
60 octree.getOccupiedVoxelCenters(point_vec);
62 cloud_voxeled->width = point_vec.size();
63 cloud_voxeled->height = 1;
64 for (
int i = 0; i < point_vec.size(); i++) {
65 cloud_voxeled->push_back(point_vec[i]);
69 sensor_msgs::PointCloud2 output_msg;
70 toROSMsg(*cloud_voxeled, output_msg);
71 output_msg.header = input_msg->header;
76 visualization_msgs::Marker marker_msg;
77 marker_msg.type = visualization_msgs::Marker::CUBE_LIST;
81 marker_msg.header = input_msg->header;
82 marker_msg.pose.orientation.w = 1.0;
89 Eigen::Vector4f minpt, maxpt;
90 pcl::getMinMax3D<PointT>(*cloud_voxeled, minpt, maxpt);
92 for (
size_t i = 0; i < cloud_voxeled->size(); i++) {
93 p = cloud_voxeled->at(i);
94 geometry_msgs::Point point_ros;
98 marker_msg.points.push_back(point_ros);
116 visualization_msgs::MarkerArray marker_array_msg;
117 marker_array_msg.markers.push_back(marker_msg);
134 generateVoxelCloudImpl<pcl::PointXYZRGBNormal>(input_msg);
137 generateVoxelCloudImpl<pcl::PointXYZRGB>(input_msg);
142 generateVoxelCloudImpl<pcl::PointNormal>(input_msg);
145 generateVoxelCloudImpl<pcl::PointXYZ>(input_msg);
174 DiagnosticNodelet::onInit();
175 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
176 dynamic_reconfigure::Server<Config>::CallbackType
f =
178 srv_->setCallback (f);
180 pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
181 pub_marker_ = advertise<visualization_msgs::Marker>(*
pnh_,
"output_marker", 1);
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher pub_marker_
bool publish_marker_flag_
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
bool hasField(const std::string &field_name, const sensor_msgs::PointCloud2 &msg)
ros::Subscriber sub_input_
virtual void configCallback(Config &config, uint32_t level)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
std::string marker_color_
ros::Publisher pub_marker_array_
ros::Publisher pub_cloud_
jsk_pcl_ros::OctreeVoxelGridConfig Config
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::OctreeVoxelGrid, nodelet::Nodelet)
double marker_color_alpha_
void generateVoxelCloudImpl(const sensor_msgs::PointCloud2ConstPtr &input)
virtual void unsubscribe()
ros::Publisher pub_octree_resolution_
virtual void generateVoxelCloud(const sensor_msgs::PointCloud2ConstPtr &input)