void jsk_pcl_ros::PointcloudScreenpoint::point_array_cb (const sensor_msgs::PointCloud2ConstPtr& pt_arr_ptr) {
if (publish_points_) {
pcl::PointCloud<pcl::PointXY>::Ptr point_array_cloud(new pcl::PointCloud<pcl::PointXY>);
pcl::fromROSMsg(*pt_arr_ptr, *point_array_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr result_cloud(new pcl::PointCloud<pcl::PointXYZ>);
result_cloud->header = pcl_conversions::toPCL(header_); // if hydro
for (size_t i = 0; i < point_array_cloud->points.size(); i++) {
pcl::PointXY point = point_array_cloud->points[i];
geometry_msgs::PointStamped ps;
bool ret; float rx, ry, rz;
ret = extract_point (pts_, point.x, point.y, rx, ry, rz);
if (ret) {
pcl::PointXYZ point_on_screen;
point_on_screen.x = rx;
point_on_screen.y = ry;
point_on_screen.z = rz;
result_cloud->points.push_back(point_on_screen);
}
}
sensor_msgs::PointCloud2::Ptr ros_cloud(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*result_cloud, *ros_cloud);
ros_cloud->header = header_;
pub_points_.publish(ros_cloud);
}
}
// this function gets called every time new pcl data comes in
void cloudcb(const sensor_msgs::PointCloud2ConstPtr &scan)
{
ROS_INFO("Kinect point cloud receieved");
ros::Time start_time = ros::Time::now();
ros::Time tcur = ros::Time::now();
Eigen::Vector4f centroid;
geometry_msgs::Point point;
pcl::PassThrough<pcl::PointXYZ> pass;
Eigen::VectorXf lims(6);
sensor_msgs::PointCloud2::Ptr
ros_cloud (new sensor_msgs::PointCloud2 ()),
ros_cloud_filtered (new sensor_msgs::PointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ> ()),
cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
// set a parameter telling the world that I am tracking the robot
ros::param::set("/tracking_robot", true);
ROS_DEBUG("finished declaring vars : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ROS_DEBUG("About to transform cloud");
// New sensor message for holding the transformed data
sensor_msgs::PointCloud2::Ptr scan_transformed
(new sensor_msgs::PointCloud2());
try{
pcl_ros::transformPointCloud("/oriented_optimization_frame",
tf, *scan, *scan_transformed);
}
catch(tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
}
scan_transformed->header.frame_id = "/oriented_optimization_frame";
// Convert to pcl
ROS_DEBUG("Convert cloud to pcd from rosmsg");
pcl::fromROSMsg(*scan_transformed, *cloud);
ROS_DEBUG("cloud transformed and converted to pcl : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// set time stamp and frame id
ros::Time tstamp = ros::Time::now();
// run through pass-through filter to eliminate tarp and below robots.
ROS_DEBUG("Pass-through filter");
pass_through(cloud, cloud_filtered, robot_limits);
ROS_DEBUG("done with pass-through : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// now let's publish that filtered cloud
ROS_DEBUG("Converting and publishing cloud");
pcl::toROSMsg(*cloud_filtered, *ros_cloud_filtered);
ros_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
cloud_pub[0].publish(ros_cloud_filtered);
ROS_DEBUG("published filtered cloud : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// Let's do a downsampling before doing cluster extraction
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud (cloud_filtered);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_downsampled);
ROS_DEBUG("finished downsampling : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ROS_DEBUG("Begin extraction filtering");
// build a KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree
(new pcl::search::KdTree<pcl::PointXYZ> ());
tree->setInputCloud (cloud_downsampled);
ROS_DEBUG("done with KdTree initialization : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// create a vector for storing the indices of the clusters
std::vector<pcl::PointIndices> cluster_indices;
// setup extraction:
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // cm
ec.setMinClusterSize (50);
ec.setMaxClusterSize (3000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_downsampled);
// now we can perform cluster extraction
ec.extract (cluster_indices);
ROS_DEBUG("finished extraction : %f", (ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
// run through the indices, create new clouds, and then publish them
int j=1;
int number_clusters=0;
geometry_msgs::PointStamped pt;
puppeteer_msgs::Robots robots;
std::vector<int> num;
for (std::vector<pcl::PointIndices>::const_iterator
it=cluster_indices.begin();
it!=cluster_indices.end (); ++it)
{
number_clusters = (int) cluster_indices.size();
ROS_DEBUG("Number of clusters found: %d",number_clusters);
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator
pit = it->indices.begin ();
pit != it->indices.end (); pit++)
{
cloud_cluster->points.push_back(cloud_downsampled->points[*pit]);
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// convert to rosmsg and publish:
ROS_DEBUG("Publishing extracted cloud");
pcl::toROSMsg(*cloud_cluster, *ros_cloud_filtered);
ros_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
if(j < MAX_CLUSTERS+1)
cloud_pub[j].publish(ros_cloud_filtered);
j++;
// compute centroid and add to Robots:
pcl::compute3DCentroid(*cloud_cluster, centroid);
pt.point.x = centroid(0);
pt.point.y = centroid(1);
pt.point.z = centroid(2);
pt.header.stamp = tstamp;
pt.header.frame_id = "/oriented_optimization_frame";
robots.robots.push_back(pt);
// add number of points in cluster to num:
num.push_back(cloud_cluster->points.size());
}
ROS_DEBUG("finished creating and publishing clusters : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
if (number_clusters > number_robots)
{
ROS_WARN("Number of clusters found is greater "
"than the number of robots");
// pop minimum cluster count
remove_least_likely(&robots, &num);
}
robots.header.stamp = tstamp;
robots.header.frame_id = "/oriented_optimization_frame";
robots.number = number_clusters;
robots_pub.publish(robots);
ROS_DEBUG("removed extra clusters, and published : %f",
(ros::Time::now()-tcur).toSec());
tcur = ros::Time::now();
ros::Duration d = ros::Time::now() - start_time;
ROS_DEBUG("End of cloudcb; time elapsed = %f (%f Hz)",
d.toSec(), 1/d.toSec());
}
void SnapIt::publishPointCloud(ros::Publisher pub, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
sensor_msgs::PointCloud2::Ptr ros_cloud(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*cloud, *ros_cloud);
pub.publish(ros_cloud);
}
