35 #define BOOST_PARAMETER_MAX_ARITY 7 45 DiagnosticNodelet::onInit();
47 pub_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
62 const visualization_msgs::MarkerArray::ConstPtr& marker_array_msg)
66 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
67 cloud->is_dense =
true;
68 for (
size_t i=0; i<marker_array_msg->markers.size(); i++) {
69 visualization_msgs::Marker
marker = marker_array_msg->markers[i];
70 for (
size_t j=0; j<marker.points.size(); j++) {
72 pt.x = marker.points[j].x;
73 pt.y = marker.points[j].y;
74 pt.z = marker.points[j].z;
75 pt.r = marker.color.r;
76 pt.g = marker.color.g;
77 pt.b = marker.color.b;
78 cloud->points.push_back(pt);
82 sensor_msgs::PointCloud2 ros_cloud;
84 ros_cloud.header = marker_array_msg->markers[0].header;
void publish(const boost::shared_ptr< M > &message) const
virtual void convert(const visualization_msgs::MarkerArray::ConstPtr &marker_array_msg)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
virtual void unsubscribe()
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::MarkerArrayVoxelToPointCloud, nodelet::Nodelet)