my_Dvector convolution_same_orig(const my_Dvector& vec1, const my_Dvector& vec2, ThreadPool& pool)
{
my_Dvector conv (vec1.size(),0.0);
int init = vec2.size()/2;
unsigned int nr_th = get_num_threads();
my_Ivector lims (get_bounds(0,conv.size()));
std::vector< std::future<int> > results;
//lambda function
auto func = [&](my_Dvector& v, int com, int ter)
{
for (int ii=com;ii<ter;++ii){
for (int j=0,k=(ii+init);j<vec2.size() && k>=0;++j,--k){
if (k<vec1.size()){ v[ii] += vec1[k]*vec2[j];}
}
}
return 1;
}; //
for (unsigned int i=0;i<nr_th;++i) results.emplace_back(pool.enqueue(
func, ref(conv), lims[i], lims[i+1]));
for(auto && result: results) result.get();
return conv;
}
static void _convolution(const my_Dvector& vec1, const my_Dvector& vec2, my_Dvector& conv, int init, ThreadPool& pool)
{
// To do the convolution I trim the beginning and ending points if they are zero (1e-30);
int min1 (0), max1 (vec1.size());
_trim_vec(vec1, min1, max1);
int min2 (0), max2 (vec2.size());
_trim_vec(vec2, min2, max2);
int ini = max(min1+min2-init,0);
int fin = min(int(conv.size()), max1 + max2 - 1);
unsigned int nr_th = get_num_threads();
unsigned int nr_job = max((fin-ini)/100, min(int(nr_th), fin-ini));
my_Ivector lims (get_bounds(ini, fin, nr_job));
std::vector< std::future<int> > results;
//lambda function
auto func = [&](my_Dvector& v, int com, int ter)
{
for (int ii=com;ii<ter;++ii){
const int delta = max(ii+init-min2-max1,0);
for (int j=min2+delta,k=(ii+init-min2-delta);j<max2 && k>=min1;++j,--k){
v[ii] += vec1[k]*vec2[j];
}
}
return 1;
}; //
for (unsigned int i=0;i<nr_job;++i) results.emplace_back(pool.enqueue(
func, ref(conv), lims[i], lims[i+1]));
for(auto && result: results) result.get();
}
// 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());
}
// this function gets called every time new pcl data comes in
void cloudcb(const sensor_msgs::PointCloud2ConstPtr &scan)
{
Eigen::Vector4f centroid;
float xpos = 0.0;
float ypos = 0.0;
float zpos = 0.0;
float D_sphere = 0.05; //meters
float R_search = 2.0*D_sphere;
puppeteer_msgs::PointPlus pointplus;
geometry_msgs::Point point;
pcl::PassThrough<pcl::PointXYZ> pass;
Eigen::VectorXf lims(6);
sensor_msgs::PointCloud2::Ptr
robot_cloud (new sensor_msgs::PointCloud2 ()),
robot_cloud_filtered (new sensor_msgs::PointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud (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);
// 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
pcl::fromROSMsg(*scan_transformed, *cloud);
// set time stamp and frame id
pointplus.header.stamp = scan->header.stamp;
pointplus.header.frame_id = "/oriented_optimization_frame";
// do we need to find the object?
if (locate == true)
{
// lims << XMIN, XMAX, YMIN, YMAX, ZMIN, ZMAX;
lims << frame_limits;
pass_through(cloud, cloud_filtered, lims);
pcl::compute3DCentroid(*cloud_filtered, centroid);
xpos = centroid(0); ypos = centroid(1); zpos = centroid(2);
// Publish cloud:
pcl::toROSMsg(*cloud_filtered, *robot_cloud_filtered);
robot_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
cloud_pub[1].publish(robot_cloud_filtered);
// are there enough points in the point cloud?
if(cloud_filtered->points.size() > POINT_THRESHOLD)
{
locate = false; // We have re-found the object!
// set values to publish
pointplus.x = xpos; pointplus.y = ypos; pointplus.z = zpos;
pointplus.error = false;
pointplus_pub.publish(pointplus);
}
// otherwise we should publish a blank centroid
// position with an error flag
else
{
// set values to publish
pointplus.x = 0.0;
pointplus.y = 0.0;
pointplus.z = 0.0;
pointplus.error = true;
pointplus_pub.publish(pointplus);
}
}
// if "else", we are just going to calculate the centroid
// of the input cloud
else
{
lims <<
xpos_last-R_search, xpos_last+R_search,
ypos_last-R_search, ypos_last+R_search,
zpos_last-R_search, zpos_last+R_search;
pass_through(cloud, cloud_filtered, lims);
// are there enough points in the point cloud?
if(cloud_filtered->points.size() < POINT_THRESHOLD)
{
locate = true;
ROS_WARN("Lost Object at: x = %f y = %f z = %f\n",
xpos_last,ypos_last,zpos_last);
pointplus.x = 0.0;
pointplus.y = 0.0;
pointplus.z = 0.0;
pointplus.error = true;
pointplus_pub.publish(pointplus);
return;
}
pcl::compute3DCentroid(*cloud_filtered, centroid);
xpos = centroid(0); ypos = centroid(1); zpos = centroid(2);
pcl::toROSMsg(*cloud_filtered, *robot_cloud);
robot_cloud_filtered->header.frame_id =
"/oriented_optimization_frame";
cloud_pub[0].publish(robot_cloud);
tf::Transform transform;
transform.setOrigin(tf::Vector3(xpos, ypos, zpos));
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform
(transform,ros::Time::now(),
"/oriented_optimization_frame",
"/robot_kinect_frame"));
// set pointplus message values and publish
pointplus.x = xpos;
pointplus.y = ypos;
pointplus.z = zpos;
pointplus.error = false;
pointplus_pub.publish(pointplus);
}
xpos_last = xpos;
ypos_last = ypos;
zpos_last = zpos;
}
