void OctoCostmap::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
ros::WallTime start = ros::WallTime::now();
sensor_msgs::PointCloud2 cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
/*octomap::point3d octomap_3d_point;
octomap::Pointcloud octomap_pointcloud;
sensor_msgs::PointCloud2 cloud;
try {
projector_.projectLaser(*scan, cloud);
//projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, cloud, tfl_);
for(size_t i = 0; i < cloud.points.size(); i++) {
octomap_3d_point(0) = cloud.points[i].x;
octomap_3d_point(1) = cloud.points[i].y;
octomap_3d_point(2) = cloud.points[i].z;
octomap_pointcloud.push_back(octomap_3d_point);
}
tf::StampedTransform transform;
tfl_.lookupTransform(map_frame_, scan->header.frame_id, scan->header.stamp, transform);
octomath::Vector3 laser_point(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
octomath::Quaternion laser_quat(transform.getRotation().getW(), transform.getRotation().getX(), transform.getRotation().getY(), transform.getRotation().getZ());
octomath::Pose6D laser_pose(laser_point, laser_quat);
//octree_->insertScan(octomap_pointcloud, laser_pose);
} catch (tf::TransformException ex) {
ROS_ERROR("Error finding origin of the laser in the map frame. Error was %s", ex.what());
} */
projector_.projectLaser(*scan, cloud);
pcl::fromROSMsg(cloud, *pcl_cloud);
insertPointCloudXYZ(pcl_cloud);
ROS_DEBUG("Laser callback took %f milliseconds", (ros::WallTime::now() - start).toSec() * 1000.0);
publishOctomapMsg();
publishVisualizationPointCloud();
}
void App::doFileProcessing(std::string ply_filename){
std::cout << "ply_filename: " << ply_filename << "\n";
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
/*
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (ply_filename, *pcl_cloud) == -1){ //* load the file
std::cout << "Couldn't read pcd file\n";
exit(-1);
} */
if (pcl::io::loadPLYFile<pcl::PointXYZRGB> (ply_filename, *pcl_cloud) == -1){ //* load the file
std::cout << "Couldn't read ply file\n";
exit(-1);
}
std::cout << "Started doProcessing\n";
pronto::PointCloud* cloud (new pronto::PointCloud ());
pronto::PointCloud* sub_cloud (new pronto::PointCloud ());
pronto_vis_->convertCloudPclToPronto(*pcl_cloud, *cloud);
convert_->doWork(cloud);
convert_->publishOctree( convert_->getTree(),"OCTOMAP");
exit(-1);
}
bool sm2ccConverter::addXYZ(ccPointCloud *cloud)
{
assert(m_sm_cloud && cloud);
if (!m_sm_cloud || !cloud)
return false;
size_t pointCount = GetNumberOfPoints(m_sm_cloud);
if (!cloud->reserve(static_cast<unsigned>(pointCount)))
return false;
//add xyz to the given cloud taking xyz infos from the sm cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
FROM_PCL_CLOUD(*m_sm_cloud, *pcl_cloud);
//loop
for (size_t i = 0; i < pointCount; ++i)
{
CCVector3 P(pcl_cloud->at(i).x,
pcl_cloud->at(i).y,
pcl_cloud->at(i).z);
cloud->addPoint(P);
}
return true;
}
void ObjectRecognition::rgbdImageCallback(const sensor_msgs::PointCloud2ConstPtr& input_cloud,
const sensor_msgs::CameraInfoConstPtr& camera_info) {
camera_model.fromCameraInfo(camera_info);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::fromROSMsg(*input_cloud, *pcl_cloud);
this->pipeline(pcl_cloud);
}
bool EuclideanClustering::serviceCallback(
jsk_recognition_msgs::EuclideanSegment::Request &req,
jsk_recognition_msgs::EuclideanSegment::Response &res)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(req.input, *cloud);
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > ();
#else
pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
#endif
vector<pcl::PointIndices> cluster_indices;
EuclideanClusterExtraction<pcl::PointXYZ> impl;
double tor;
if ( req.tolerance < 0) {
tor = tolerance;
} else {
tor = req.tolerance;
}
impl.setClusterTolerance (tor);
impl.setMinClusterSize (minsize_);
impl.setMaxClusterSize (maxsize_);
impl.setSearchMethod (tree);
impl.setInputCloud (cloud);
impl.extract (cluster_indices);
res.output.resize( cluster_indices.size() );
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7 )
pcl::PCLPointCloud2::Ptr pcl_cloud(new pcl::PCLPointCloud2);
pcl_conversions::toPCL(req.input, *pcl_cloud);
pcl::ExtractIndices<pcl::PCLPointCloud2> ex;
ex.setInputCloud(pcl_cloud);
#else
pcl::ExtractIndices<sensor_msgs::PointCloud2> ex;
ex.setInputCloud ( boost::make_shared< sensor_msgs::PointCloud2 > (req.input) );
#endif
for ( size_t i = 0; i < cluster_indices.size(); i++ ) {
ex.setIndices ( boost::make_shared< pcl::PointIndices > (cluster_indices[i]) );
ex.setNegative ( false );
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7 )
pcl::PCLPointCloud2 output_cloud;
ex.filter ( output_cloud );
pcl_conversions::fromPCL(output_cloud, res.output[i]);
#else
ex.filter ( res.output[i] );
#endif
}
return true;
}
void topicCallback_input(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
/* protected region user implementation of subscribe callback for input on begin */
pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());
// Transformation into PCL type PointCloud2
pcl_conversions::toPCL((*msg), *(pcl_pc));
// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud));
////////////////////////
// PassThrough filter //
////////////////////////
pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ());
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, robot_pose_x+localconfig.inhib_size)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, robot_pose_x-localconfig.inhib_size)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, robot_pose_y+localconfig.inhib_size)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, robot_pose_y-localconfig.inhib_size)));
// build the filter
pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond);
condrem.setInputCloud (pcl_cloud);
condrem.setKeepOrganized(true);
// apply filter
condrem.filter (*pcl_cloud);
// Transformation into ROS type
pcl::toPCLPointCloud2(*(pcl_cloud), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), output_pcloud2);
output_ready = true;
/* protected region user implementation of subscribe callback for input end */
}
void ColorHistogramMatcher::feature(
const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
const jsk_pcl_ros::ClusterPointIndices::ConstPtr& input_indices)
{
boost::mutex::scoped_lock(mutex_);
if (!reference_set_) {
NODELET_WARN("reference histogram is not available yet");
return;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*input_cloud, *pcl_cloud);
pcl::PointCloud<pcl::PointXYZHSV>::Ptr hsv_cloud (new pcl::PointCloud<pcl::PointXYZHSV>);
pcl::PointCloudXYZRGBtoXYZHSV(*pcl_cloud, *hsv_cloud);
// compute histograms first
std::vector<std::vector<float> > histograms;
histograms.resize(input_indices->cluster_indices.size());
pcl::ExtractIndices<pcl::PointXYZHSV> extract;
extract.setInputCloud(hsv_cloud);
// for debug
jsk_pcl_ros::ColorHistogramArray histogram_array;
histogram_array.header = input_cloud->header;
for (size_t i = 0; i < input_indices->cluster_indices.size(); i++) {
pcl::IndicesPtr indices (new std::vector<int>(input_indices->cluster_indices[i].indices));
extract.setIndices(indices);
pcl::PointCloud<pcl::PointXYZHSV> segmented_cloud;
extract.filter(segmented_cloud);
std::vector<float> histogram;
computeHistogram(segmented_cloud, histogram, policy_);
histograms[i] = histogram;
ColorHistogram ros_histogram;
ros_histogram.header = input_cloud->header;
ros_histogram.histogram = histogram;
histogram_array.histograms.push_back(ros_histogram);
}
all_histogram_pub_.publish(histogram_array);
// compare histograms
jsk_pcl_ros::ClusterPointIndices result;
result.header = input_indices->header;
for (size_t i = 0; i < input_indices->cluster_indices.size(); i++) {
const double coefficient = bhattacharyyaCoefficient(histograms[i], reference_histogram_);
NODELET_DEBUG_STREAM("coefficient: " << i << "::" << coefficient);
if (coefficient > coefficient_thr_) {
result.cluster_indices.push_back(input_indices->cluster_indices[i]);
}
}
result_pub_.publish(result);
}
int NormalEstimation::compute()
{
//pointer to selected cloud
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
//if we have normals delete them!
if (cloud->hasNormals())
cloud->unallocateNorms();
if (cloud->hasNormals())
cloud->unallocateNorms();
//get xyz in sensor_msgs format
cc2smReader converter;
converter.setInputCloud(cloud);
sensor_msgs::PointCloud2 sm_cloud = converter.getXYZ();
//get as pcl point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(sm_cloud, *pcl_cloud);
//create storage for normals
pcl::PointCloud<pcl::PointNormal>::Ptr normals (new pcl::PointCloud<pcl::PointNormal>);
//now compute
int result = compute_normals<pcl::PointXYZ, pcl::PointNormal>(pcl_cloud, m_useKnn ? m_knn_radius: m_radius, m_useKnn, normals);
sensor_msgs::PointCloud2::Ptr sm_normals (new sensor_msgs::PointCloud2);
pcl::toROSMsg(*normals, *sm_normals);
sm2ccConverter converter2(sm_normals);
converter2.addNormals(cloud);
converter2.addScalarField(cloud, "curvature", m_overwrite_curvature);
emit entityHasChanged(cloud);
return 1;
}
void compute_normals(const cv::Mat& cloud, cv::Mat& normals)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud( new pcl::PointCloud<pcl::PointXYZ> );
pcl_cloud->clear();
pcl_cloud->width = cloud.cols;
pcl_cloud->height = cloud.rows;
pcl_cloud->points.resize( pcl_cloud->width * pcl_cloud->height);
for(int y = 0; y < cloud.rows; ++y)
for(int x = 0; x < cloud.cols; ++x)
{
pcl_cloud->at(x,y).x = cloud.at<cv::Point3f>(y,x).x;
pcl_cloud->at(x,y).y = cloud.at<cv::Point3f>(y,x).y;
pcl_cloud->at(x,y).z = cloud.at<cv::Point3f>(y,x).z;
}
pcl::PointCloud<pcl::Normal>::Ptr pcl_normals (new pcl::PointCloud<pcl::Normal>);
pcl_normals->clear();
pcl_normals->width = pcl_cloud->width;
pcl_normals->height = pcl_cloud->height;
pcl_normals->points.resize(pcl_cloud->width * pcl_cloud->height);
pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud( pcl_cloud );
ne.setNormalSmoothingSize( 5 );
ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
ne.compute( *pcl_normals );
normals.create( cloud.size(), CV_32FC3 );
for(int y = 0; y < pcl_normals->height; ++y)
for(int x = 0; x < pcl_normals->width; ++x)
{
normals.at<cv::Point3f>(y,x).x = pcl_normals->at(x,y).normal_x;
normals.at<cv::Point3f>(y,x).y = pcl_normals->at(x,y).normal_y;
normals.at<cv::Point3f>(y,x).z = pcl_normals->at(x,y).normal_z;
}
}
void My_Filter::pclCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){
pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final2 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());
sensor_msgs::PointCloud2 pc2;
std::vector<int> inliers;
// Transformation into PCL type PointCloud2
pcl_conversions::toPCL((*cloud), *(pcl_pc));
// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud));
std::list<geometry_msgs::PoseStamped> tmp_list;
int robot_counter = 0;
for (std::list<geometry_msgs::PoseStamped>::iterator it = robots.begin() ; it != robots.end(); ++it)
{
if(it->pose.orientation.x > 0.001)
{ // Robot currently not seen
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (pcl_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (it->pose.position.x-DETECTION_DISTANCE-it->pose.orientation.x*0.002,
it->pose.position.x+DETECTION_DISTANCE+it->pose.orientation.x*0.002);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pass.setInputCloud (final2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (it->pose.position.y-DETECTION_DISTANCE-it->pose.orientation.x*0.002,
it->pose.position.y+DETECTION_DISTANCE+it->pose.orientation.x*0.002);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
}
else
{ // Robot seen
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (pcl_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (it->pose.position.x-DETECTION_DISTANCE, it->pose.position.x+DETECTION_DISTANCE);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pass.setInputCloud (final2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (it->pose.position.y-DETECTION_DISTANCE, it->pose.position.y+DETECTION_DISTANCE);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ());
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, it->pose.position.x+DETECTION_DISTANCE)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, it->pose.position.x-DETECTION_DISTANCE)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, it->pose.position.y+DETECTION_DISTANCE)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, it->pose.position.y-DETECTION_DISTANCE)));
// build the filter
pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond);
condrem.setInputCloud (pcl_cloud);
condrem.setKeepOrganized(true);
// apply filter
condrem.filter (*pcl_cloud);
}
if(final2->size() > 2)
{ // Something in the box
double meanX = 0.0;
double meanY = 0.0;
for(int i = 0; i < final2->size(); i++)
{
meanX += final2->at(i).x;
meanY += final2->at(i).y;
}
meanX = meanX / final2->size();
meanY = meanY / final2->size();
//robots.at(robot_counter).pose.position.x = meanX;
//robots.at(robot_counter).pose.position.y = meanY;
it->pose.position.x = meanX;
it->pose.position.y = meanY;
char numstr[50];
sprintf(numstr, "/robot_%s_link", it->header.frame_id.c_str());
t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(meanX, meanY, 0.0)),
ros::Time::now(), "/world", numstr);
t.stamp_ = ros::Time::now();
broadcaster.sendTransform(t);
it->pose.orientation.x = 0.0;
tmp_list.push_front(*it);
//std::cout << "size : " << final2->size() << std::endl;
}
else
{ // Nothing found in the box
it->pose.orientation.x += 1.0;
if(it->pose.orientation.x > 100.0)
{
it->pose.orientation.x = 100.0;
}
//std::cout << "coef : " << it->pose.orientation.x << std::endl;
geometry_msgs::PoseStamped tmp = *it;
//robots.push_back(*it);
//robots.erase(it);
//robots.push_back(tmp);
tmp_list.push_back(*it);
}
robot_counter++;
}
robots = tmp_list;
/////////////////////////////////
// Statistical Outlier Removal //
/////////////////////////////////
/* pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud (pcl_cloud);
sor.setMeanK (200);
sor.setStddevMulThresh (1.0);
sor.filter (*final);
*/
//////////////////////////////////
// Euclidian Cluster Extraction //
//////////////////////////////////
// Creating the KdTree object for the search method of the extraction
/* pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (pcl_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.05); // 2cm
ec.setMinClusterSize (5);
ec.setMaxClusterSize (50);
ec.setSearchMethod (tree);
ec.setInputCloud (pcl_cloud);
ec.extract (cluster_indices);
std::cout << cluster_indices.size() << std::endl;
double x = 0.0;
double y = 0.0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
x = 0.0;
y = 0.0;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
x +=pcl_cloud->points[*pit].x;
y +=pcl_cloud->points[*pit].y;
}
x = x / it->indices.size();
y = y / it->indices.size();
std::cout << "x : " << x << " y : " << y << " size : " << it->indices.size() << std::endl;
}
*/
// Transformation into ROS type
pcl::toPCLPointCloud2(*(pcl_cloud), *(cloud_out));
pcl_conversions::moveFromPCL(*(cloud_out), pc2);
point_cloud_publisher_.publish(pc2);
}
void topicCallback_input_cloud(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
/* protected region user implementation of subscribe callback for input_cloud on begin */
pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ());
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final2 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());
sensor_msgs::PointCloud2 pc2;
std::vector<int> inliers;
// Transformation into PCL type PointCloud2
pcl_conversions::toPCL((*msg), *(pcl_pc));
// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud));
std::list<geometry_msgs::PoseStamped> tmp_list;
int robot_counter = 0;
for (std::list<geometry_msgs::PoseStamped>::iterator it = robots.begin() ; it != robots.end(); ++it)
{
if(it->pose.orientation.x > 0.001)
{ // Robot currently not seen
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (pcl_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (it->pose.position.x-localconfig.detection_distance-it->pose.orientation.x*0.002,
it->pose.position.x+localconfig.detection_distance+it->pose.orientation.x*0.002);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pass.setInputCloud (final2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (it->pose.position.y-localconfig.detection_distance-it->pose.orientation.x*0.002,
it->pose.position.y+localconfig.detection_distance+it->pose.orientation.x*0.002);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
}
else
{ // Robot seen
////////////////////////
// PassThrough filter //
////////////////////////
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (pcl_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (it->pose.position.x-localconfig.detection_distance, it->pose.position.x+localconfig.detection_distance);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pass.setInputCloud (final2);
pass.setFilterFieldName ("y");
pass.setFilterLimits (it->pose.position.y-localconfig.detection_distance, it->pose.position.y+localconfig.detection_distance);
//pass.setFilterLimitsNegative (true);
pass.filter (*final2);
pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ());
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, it->pose.position.x+localconfig.detection_distance)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, it->pose.position.x-localconfig.detection_distance)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, it->pose.position.y+localconfig.detection_distance)));
range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, it->pose.position.y-localconfig.detection_distance)));
// build the filter
pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond);
condrem.setInputCloud (pcl_cloud);
condrem.setKeepOrganized(true);
// apply filter
condrem.filter (*pcl_cloud);
}
if(final2->size() > 2)
{ // Something in the box
double meanX = 0.0;
double meanY = 0.0;
for(int i = 0; i < final2->size(); i++)
{
meanX += final2->at(i).x;
meanY += final2->at(i).y;
}
meanX = meanX / final2->size();
meanY = meanY / final2->size();
//robots.at(robot_counter).pose.position.x = meanX;
//robots.at(robot_counter).pose.position.y = meanY;
it->pose.position.x = (meanX + it->pose.position.x) / 2.0;
it->pose.position.y = (meanY + it->pose.position.y) / 2.0;
char numstr[50];
sprintf(numstr, "/robot_%s_link", it->header.frame_id.c_str());
t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(meanX, meanY, 0.0)),
ros::Time::now(), "/world", numstr);
t.stamp_ = ros::Time::now();
broadcaster.sendTransform(t);
it->pose.orientation.x = 0.0;
tmp_list.push_front(*it);
if(it->header.frame_id == "0")
{
robot1_output_ready = true;
}
else
{
robot2_output_ready = true;
}
//std::cout << "size : " << final2->size() << std::endl;
}
else
{ // Nothing found in the box
it->pose.orientation.x += 1.0;
if(it->pose.orientation.x > 100.0)
{
it->pose.orientation.x = 100.0;
}
//std::cout << "coef : " << it->pose.orientation.x << std::endl;
geometry_msgs::PoseStamped tmp = *it;
//robots.push_back(*it);
//robots.erase(it);
//robots.push_back(tmp);
tmp_list.push_back(*it);
}
robot_counter++;
}
robots = tmp_list;
/* protected region user implementation of subscribe callback for input_cloud end */
}
void Cup_Segmentation::cloudCallback (const sensor_msgs::PointCloud2::ConstPtr& cloud_in)
{
ROS_INFO("Processing!");
/************************ CENTER AND LEFT BOXES ***************************************/
//Creating point cloud and convert ROS Message
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZ>::Ptr seg_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_in, *pcl_cloud);
//Create and define filter parameters
pcl::PassThrough<pcl::PointXYZ> pass3;
pass3.setFilterFieldName("x");
pass3.setFilterLimits(0, 1.2); //-0.5 0.5
pass3.setInputCloud(pcl_cloud);
pass3.filter(*pcl_cloud);
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setFilterFieldName("y");
pass.setFilterLimits(-0.7, 0.1); //-0.5 0.5
pass.setInputCloud(pcl_cloud);
pass.filter(*pcl_cloud);
pcl::PassThrough<pcl::PointXYZ> pass2;
pass2.setFilterFieldName("z");
pass2.setFilterLimits(0.0, 1.0); //-0.5 0.5
pass2.setInputCloud(pcl_cloud);
pass2.filter(*pcl_cloud);
//Model fitting process ->RANSAC
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PARALLEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.03); //0.03
seg.setAxis (Eigen::Vector3f(1, 0, 0));
seg.setEpsAngle (0.1); //0.02
seg.setInputCloud (pcl_cloud);
seg.segment (*inliers, *coefficients);
//Verify if inliers is not empty
if (inliers->indices.size () == 0)
return;
pcl::PointCloud<pcl::PointXYZ>::Ptr treated_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
for (std::vector<int>::const_iterator it = inliers->indices.begin(); it != inliers->indices.end (); ++it)
treated_cloud->push_back(pcl_cloud->points[*it]);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(pcl_cloud);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*seg_cloud);
//Create and define radial filter parameters
//pcl::RadiusOutlierRemoval<pcl::PointXYZ> radialFilter;
//radialFilter.setInputCloud(treated_cloud);
//radialFilter.setRadiusSearch(0.03);
//radialFilter.setMinNeighborsInRadius (20);
//radialFilter.filter (*treated_cloud);
//Apply clustering algorithm
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree (new pcl::search::KdTree<pcl::PointXYZ>);
kdtree->setInputCloud (seg_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.06);
ec.setMinClusterSize (6);
ec.setMaxClusterSize (150);
ec.setSearchMethod (kdtree);
ec.setInputCloud (seg_cloud);
ec.extract (cluster_indices);
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
pcl::PointXYZI cluster_point;
double cluster_final_average;
int cluster_id=0;
float x1 = 0.0, y1 = 0.0, z1 = 0.0;
float x2 = 0.0, y2 = 0.0, z2 = 0.0;
float x3 = 0.0, y3 = 0.0, z3 = 0.0;
int total1 = 0, total2 = 0, total3 = 0;
bool hasCup1, hasCup2, hasCup3;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end ();
++it, cluster_id+=1)
{
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
cluster_point.x = seg_cloud->points[*pit].x;
cluster_point.y = seg_cloud->points[*pit].y;
cluster_point.z = seg_cloud->points[*pit].z;
cluster_point.intensity = cluster_id;
cluster_cloud->push_back(cluster_point);
if(cluster_id == 0){
x1 += seg_cloud->points[*pit].x;
y1 += seg_cloud->points[*pit].y;
z1 += seg_cloud->points[*pit].z;
total1++;
} else if (cluster_id == 1){
x2 += seg_cloud->points[*pit].x;
y2 += seg_cloud->points[*pit].y;
z2 += seg_cloud->points[*pit].z;
total2++;
} else if (cluster_id == 2){
x3 += seg_cloud->points[*pit].x;
y3 += seg_cloud->points[*pit].y;
z3 += seg_cloud->points[*pit].z;
total3++;
}
}
}
if(total1 != 0 ){
x1 = x1/total1;
y1 = y1/total1;
z1 = z1/total1;
hasCup1=true;
}else{
hasCup1 = false;
}
if(total2 != 0 ){
x2 = x2/total2;
y2 = y2/total2;
z2 = z2/total2;
hasCup2 = true;
} else {
hasCup2 = false;
}
if(total3 != 0){
x3 = x3/total2;
y3 = y3/total2;
z3 = z3/total2;
hasCup3 = true;
} else{
hasCup3 = false;
}
//Publish message
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(*cluster_cloud, cloud);
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = cloud_in->header.frame_id;
treated_cloud_pub_.publish(cloud);
//Geometry
geometry_msgs::PointStamped cupPoint1;
geometry_msgs::PointStamped cupPoint2;
geometry_msgs::PointStamped cupPoint3;
tf::Quaternion q;
geometry_msgs::PointStamped cupSensorPoint1;
geometry_msgs::PointStamped cupSensorPoint2;
geometry_msgs::PointStamped cupSensorPoint3;
static tf::TransformBroadcaster tfBc1;
static tf::TransformBroadcaster tfBc2;
static tf::TransformBroadcaster tfBc3;
tf::Transform tfCup1;
tf::Transform tfCup2;
tf::Transform tfCup3;
cupPoint1.header.frame_id = "base_footprint";
cupPoint2.header.frame_id = "base_footprint";
cupPoint3.header.frame_id = "base_footprint";
cupPoint1.header.stamp = cloud_in->header.stamp;
cupPoint2.header.stamp = cloud_in->header.stamp;
cupPoint3.header.stamp = cloud_in->header.stamp;
cupPoint1.point.x = x1;
cupPoint2.point.x = x2;
cupPoint3.point.x = x3;
cupPoint1.point.y = y1;
cupPoint2.point.y = y2;
cupPoint3.point.y = y3;
cupPoint1.point.z = z1;
cupPoint2.point.z = z2;
cupPoint3.point.z = z3;
ROS_INFO("Cup 1: X=%f Y=%f Z=%f", cupPoint1.point.x, cupPoint1.point.y, cupPoint1.point.z);
ROS_INFO("Cup 2: X=%f Y=%f Z=%f", cupPoint2.point.x, cupPoint2.point.y, cupPoint2.point.z);
ROS_INFO("Cup 3: X=%f Y=%f Z=%f", cupPoint3.point.x, cupPoint3.point.y, cupPoint3.point.z);
tf_listener.transformPoint("sensors_frame", cupPoint1, cupSensorPoint1);
tf_listener.transformPoint("sensors_frame", cupPoint2, cupSensorPoint2);
tf_listener.transformPoint("sensors_frame", cupPoint3, cupSensorPoint3);
tfCup1.setOrigin( tf::Vector3(cupSensorPoint1.point.x, cupSensorPoint1.point.y, cupSensorPoint1.point.z) );
tfCup2.setOrigin( tf::Vector3(cupSensorPoint2.point.x, cupSensorPoint2.point.y, cupSensorPoint2.point.z) );
tfCup3.setOrigin( tf::Vector3(cupSensorPoint3.point.x, cupSensorPoint3.point.y, cupSensorPoint3.point.z) );
ROS_INFO("Sensor Frame Cup 1: X=%f Y=%f Z=%f", cupSensorPoint1.point.x, cupSensorPoint1.point.y, cupSensorPoint1.point.z);
ROS_INFO("Sensor Frame Cup 2: X=%f Y=%f Z=%f", cupSensorPoint2.point.x, cupSensorPoint2.point.y, cupSensorPoint2.point.z);
ROS_INFO("Sensor Frame Cup 3: X=%f Y=%f Z=%f", cupSensorPoint3.point.x, cupSensorPoint3.point.y, cupSensorPoint3.point.z);
q.setRPY(0, 0, 0);
tfCup1.setRotation(q);
tfCup2.setRotation(q);
tfCup3.setRotation(q);
tfBc1.sendTransform(tf::StampedTransform(tfCup1, cloud_in->header.stamp, "sensors_frame", "cup1"));
tfBc2.sendTransform(tf::StampedTransform(tfCup2, cloud_in->header.stamp, "sensors_frame", "cup2"));
tfBc3.sendTransform(tf::StampedTransform(tfCup3, cloud_in->header.stamp, "sensors_frame", "cup3"));
ROS_INFO("All Sent!");
}
int MLSSmoothingUpsampling::compute()
{
//pointer to selected cloud
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
//get xyz in sensor_msgs format
cc2smReader converter;
converter.setInputCloud(cloud);
//take out the xyz info
sensor_msgs::PointCloud2 sm_xyz = converter.getXYZ();
sensor_msgs::PointCloud2 sm_cloud;
//take out the current scalar field (if any)
if (cloud->getCurrentDisplayedScalarField())
{
const char* current_sf_name = cloud->getCurrentDisplayedScalarField()->getName();
sensor_msgs::PointCloud2 sm_field = converter.getFloatScalarField(current_sf_name);
//change its name
std::string new_name = "scalar";
sm_field.fields[0].name = new_name.c_str();
//put everithing together
pcl::concatenateFields(sm_xyz, sm_field, sm_cloud);
}
else
{
sm_cloud = sm_xyz;
}
//get as pcl point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(sm_cloud, *pcl_cloud);
//create storage for outcloud
pcl::PointCloud<pcl::PointNormal>::Ptr normals (new pcl::PointCloud<pcl::PointNormal>);
#ifdef LP_PCL_PATCH_ENABLED
pcl::PointIndicesPtr mapping_indices;
smooth_mls<pcl::PointXYZ, pcl::PointNormal> (pcl_cloud, *m_parameters, normals, mapping_indices);
#else
smooth_mls<pcl::PointXYZ, pcl::PointNormal> (pcl_cloud, *m_parameters, normals);
#endif
sensor_msgs::PointCloud2::Ptr sm_normals (new sensor_msgs::PointCloud2);
pcl::toROSMsg(*normals, *sm_normals);
ccPointCloud* new_cloud = sm2ccConverter(sm_normals).getCCloud();
if (!new_cloud)
{
//conversion failed (not enough memory?)
return -1;
}
new_cloud->setName(cloud->getName()+QString("_smoothed")); //original name + suffix
new_cloud->setDisplay(cloud->getDisplay());
#ifdef LP_PCL_PATCH_ENABLED
//copy the original scalar fields here
copyScalarFields(cloud, new_cloud, mapping_indices, true);
#endif
//disable original cloud
cloud->setEnabled(false);
if (cloud->getParent())
cloud->getParent()->addChild(new_cloud);
emit newEntity(new_cloud);
return 1;
}
void GridSampler::sample(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock(mutex_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*msg, *pcl_cloud);
Eigen::Vector4f minpt, maxpt;
pcl::getMinMax3D<pcl::PointXYZRGB>(*pcl_cloud, minpt, maxpt);
int x_bin_num = ceil((maxpt[0] - minpt[0]) / grid_size_);
int y_bin_num = ceil((maxpt[1] - minpt[1]) / grid_size_);
int z_bin_num = ceil((maxpt[2] - minpt[2]) / grid_size_);
// x y z
std::map<int, std::map<int, std::map<int, std::vector<size_t > > > > grid;
for (size_t i = 0; i < pcl_cloud->points.size(); i++) {
pcl::PointXYZRGB point = pcl_cloud->points[i];
if (isnan(point.x) || isnan(point.y) || isnan(point.z)) {
// skip nan
continue;
}
int xbin = int((point.x - minpt[0]) / grid_size_);
int ybin = int((point.y - minpt[1]) / grid_size_);
int zbin = int((point.z - minpt[2]) / grid_size_);
std::map<int, std::map<int, std::map<int, std::vector<size_t> > > >::iterator xit
= grid.find(xbin);
if (xit == grid.end()) { // cannot find x bin
JSK_NODELET_DEBUG_STREAM("no x bin" << xbin);
std::map<int, std::vector<size_t> > new_z;
std::vector<size_t> new_indices;
new_indices.push_back(i);
new_z[zbin] = new_indices;
std::map<int, std::map<int, std::vector<size_t> > > new_y;
new_y[ybin] = new_z;
grid[xbin] = new_y;
}
else {
JSK_NODELET_DEBUG_STREAM("found x bin" << xbin);
std::map<int, std::map<int, std::vector<size_t> > > ybins = xit->second;
std::map<int, std::map<int, std::vector<size_t> > >::iterator yit
= ybins.find(ybin);
if (yit == ybins.end()) { // cannot find y bin
JSK_NODELET_DEBUG_STREAM("no y bin" << ybin);
std::map<int, std::vector<size_t> > new_z;
std::vector<size_t> new_indices;
new_indices.push_back(i);
new_z[zbin] = new_indices;
xit->second[ybin] = new_z;
}
else {
JSK_NODELET_DEBUG_STREAM("found y bin" << ybin);
std::map<int, std::vector<size_t> > zbins = yit->second;
std::map<int, std::vector<size_t> >::iterator zit
= zbins.find(zbin);
if (zit == zbins.end()) {
JSK_NODELET_DEBUG_STREAM("no z bin" << zbin);
std::vector<size_t> new_indices;
new_indices.push_back(i);
xit->second[ybin][zbin] = new_indices;
}
else {
JSK_NODELET_DEBUG_STREAM("found z bin" << zbin);
xit->second[ybin][zbin].push_back(i);
}
}
}
}
// publish the result
jsk_recognition_msgs::ClusterPointIndices output;
output.header = msg->header;
for (std::map<int, std::map<int, std::map<int, std::vector<size_t> > > >::iterator xit = grid.begin();
xit != grid.end();
xit++) {
std::map<int, std::map<int, std::vector<size_t> > > ybins = xit->second;
for (std::map<int, std::map<int, std::vector<size_t> > >::iterator yit = ybins.begin();
yit != ybins.end();
yit++) {
std::map<int, std::vector<size_t> > zbins = yit->second;
for (std::map<int, std::vector<size_t> >::iterator zit = zbins.begin();
zit != zbins.end();
zit++) {
std::vector<size_t> indices = zit->second;
JSK_NODELET_DEBUG_STREAM("size: " << indices.size());
if (indices.size() > min_indices_) {
PCLIndicesMsg ros_indices;
ros_indices.header = msg->header;
for (size_t j = 0; j < indices.size(); j++) {
ros_indices.indices.push_back(indices[j]);
}
output.cluster_indices.push_back(ros_indices);
}
}
}
}
pub_.publish(output);
}
