torus_finder_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros/torus_finder.h"
00037 #include "jsk_recognition_utils/pcl_conversion_util.h"
00038 #include <jsk_topic_tools/rosparam_utils.h>
00039 #include <pcl/sample_consensus/method_types.h>
00040 #include <pcl/sample_consensus/model_types.h>
00041 #include <pcl/segmentation/sac_segmentation.h>
00042 #include <pcl/filters/voxel_grid.h>
00043 
00044 namespace jsk_pcl_ros
00045 {
00046   void TorusFinder::onInit()
00047   {
00048     DiagnosticNodelet::onInit();
00049     pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
00050     pnh_->param("use_hint", use_hint_, false);
00051     if (use_hint_) {
00052       if (pnh_->hasParam("initial_axis_hint")) {
00053         std::vector<double> axis;
00054         jsk_topic_tools::readVectorParameter(*pnh_, "initial_axis_hint", axis);
00055         if (axis.size() == 3) {
00056           hint_axis_[0] = axis[0];
00057           hint_axis_[1] = axis[1];
00058           hint_axis_[2] = axis[2];
00059         }
00060         else {
00061           hint_axis_[0] = 0;
00062           hint_axis_[1] = 0;
00063           hint_axis_[2] = 1;
00064         }
00065       }
00066     }
00067     pnh_->param("use_normal", use_normal_, false);
00068     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00069     typename dynamic_reconfigure::Server<Config>::CallbackType f =
00070       boost::bind (&TorusFinder::configCallback, this, _1, _2);
00071     srv_->setCallback (f);
00072 
00073     pub_torus_ = advertise<jsk_recognition_msgs::Torus>(*pnh_, "output", 1);
00074     pub_torus_array_ = advertise<jsk_recognition_msgs::TorusArray>(*pnh_, "output/array", 1);
00075     pub_torus_with_failure_ = advertise<jsk_recognition_msgs::Torus>(*pnh_, "output/with_failure", 1);
00076     pub_torus_array_with_failure_ = advertise<jsk_recognition_msgs::TorusArray>(*pnh_, "output/with_failure/array", 1);
00077     pub_inliers_ = advertise<PCLIndicesMsg>(*pnh_, "output/inliers", 1);
00078     pub_pose_stamped_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output/pose", 1);
00079     pub_coefficients_ = advertise<PCLModelCoefficientMsg>(
00080       *pnh_, "output/coefficients", 1);
00081     pub_latest_time_ = advertise<std_msgs::Float32>(
00082       *pnh_, "output/latest_time", 1);
00083     pub_average_time_ = advertise<std_msgs::Float32>(
00084       *pnh_, "output/average_time", 1);
00085 
00086     done_initialization_ = true;
00087     onInitPostProcess();
00088   }
00089 
00090   void TorusFinder::configCallback(Config &config, uint32_t level)
00091   {
00092     boost::mutex::scoped_lock lock(mutex_);
00093     min_radius_ = config.min_radius;
00094     max_radius_ = config.max_radius;
00095     outlier_threshold_ = config.outlier_threshold;
00096     max_iterations_ = config.max_iterations;
00097     min_size_ = config.min_size;
00098     eps_hint_angle_ = config.eps_hint_angle;
00099     algorithm_ = config.algorithm;
00100     voxel_grid_sampling_ = config.voxel_grid_sampling;
00101     voxel_size_ = config.voxel_size;
00102   }
00103 
00104   void TorusFinder::subscribe()
00105   {
00106     sub_ = pnh_->subscribe("input", 1, &TorusFinder::segment, this);
00107     sub_points_ = pnh_->subscribe("input/polygon", 1, &TorusFinder::segmentFromPoints, this);
00108   }
00109 
00110   void TorusFinder::unsubscribe()
00111   {
00112     sub_.shutdown();
00113     sub_points_.shutdown();
00114   }
00115 
00116   void TorusFinder::segmentFromPoints(
00117     const geometry_msgs::PolygonStamped::ConstPtr& polygon_msg)
00118   {
00119     if (!done_initialization_) {
00120       return;
00121     }
00122     // Convert to pointcloud and call it
00123     // No normal
00124     pcl::PointCloud<pcl::PointNormal>::Ptr cloud
00125       (new pcl::PointCloud<pcl::PointNormal>);
00126     for (size_t i = 0; i < polygon_msg->polygon.points.size(); i++) {
00127       geometry_msgs::Point32 p = polygon_msg->polygon.points[i];
00128       pcl::PointNormal pcl_point;
00129       pcl_point.x = p.x;
00130       pcl_point.y = p.y;
00131       pcl_point.z = p.z;
00132       cloud->points.push_back(pcl_point);
00133     }
00134     sensor_msgs::PointCloud2 ros_cloud;
00135     pcl::toROSMsg(*cloud, ros_cloud);
00136     ros_cloud.header = polygon_msg->header;
00137     segment(boost::make_shared<sensor_msgs::PointCloud2>(ros_cloud));
00138   }
00139   
00140   void TorusFinder::segment(
00141     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00142   {
00143     if (!done_initialization_) {
00144       return;
00145     }
00146     boost::mutex::scoped_lock lock(mutex_);
00147     vital_checker_->poke();
00148     pcl::PointCloud<pcl::PointNormal>::Ptr cloud
00149       (new pcl::PointCloud<pcl::PointNormal>);
00150     pcl::fromROSMsg(*cloud_msg, *cloud);
00151     jsk_recognition_utils::ScopedWallDurationReporter r
00152       = timer_.reporter(pub_latest_time_, pub_average_time_);
00153     if (voxel_grid_sampling_) {
00154       pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
00155       (new pcl::PointCloud<pcl::PointNormal>);
00156       pcl::VoxelGrid<pcl::PointNormal> sor;
00157       sor.setInputCloud (cloud);
00158       sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
00159       sor.filter (*downsampled_cloud);
00160       cloud = downsampled_cloud;
00161     }
00162     
00163     pcl::SACSegmentation<pcl::PointNormal> seg;
00164     if (use_normal_) {
00165       pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
00166       seg_normal.setInputNormals(cloud);
00167       seg = seg_normal;
00168     }
00169 
00170     
00171     seg.setOptimizeCoefficients (true);
00172     seg.setInputCloud(cloud);
00173     seg.setRadiusLimits(min_radius_, max_radius_);
00174     if (algorithm_ == "RANSAC") {
00175       seg.setMethodType(pcl::SAC_RANSAC);
00176     }
00177     else if (algorithm_ == "LMEDS") {
00178       seg.setMethodType(pcl::SAC_LMEDS);
00179     }
00180     else if (algorithm_ == "MSAC") {
00181       seg.setMethodType(pcl::SAC_MSAC);
00182     }
00183     else if (algorithm_ == "RRANSAC") {
00184       seg.setMethodType(pcl::SAC_RRANSAC);
00185     }
00186     else if (algorithm_ == "RMSAC") {
00187       seg.setMethodType(pcl::SAC_RMSAC);
00188     }
00189     else if (algorithm_ == "MLESAC") {
00190       seg.setMethodType(pcl::SAC_MLESAC);
00191     }
00192     else if (algorithm_ == "PROSAC") {
00193       seg.setMethodType(pcl::SAC_PROSAC);
00194     }
00195     
00196     seg.setDistanceThreshold (outlier_threshold_);
00197     seg.setMaxIterations (max_iterations_);
00198     seg.setModelType(pcl::SACMODEL_CIRCLE3D);
00199     if (use_hint_) {
00200       seg.setAxis(hint_axis_);
00201       seg.setEpsAngle(eps_hint_angle_);
00202     }
00203     pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00204     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00205     seg.segment(*inliers, *coefficients);
00206     NODELET_INFO("input points: %lu", cloud->points.size());
00207     if (inliers->indices.size() > min_size_) {
00208       // check direction. Torus should direct to origin of pointcloud
00209       // always.
00210       Eigen::Vector3f dir(coefficients->values[4],
00211                           coefficients->values[5],
00212                           coefficients->values[6]);
00213       if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
00214         dir = - dir;
00215       }
00216       
00217       Eigen::Affine3f pose = Eigen::Affine3f::Identity();
00218       Eigen::Vector3f pos = Eigen::Vector3f(coefficients->values[0],
00219                                             coefficients->values[1],
00220                                             coefficients->values[2]);
00221       Eigen::Quaternionf rot;
00222       rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
00223       pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
00224       PCLIndicesMsg ros_inliers;
00225       ros_inliers.indices = inliers->indices;
00226       ros_inliers.header = cloud_msg->header;
00227       pub_inliers_.publish(ros_inliers);
00228       PCLModelCoefficientMsg ros_coefficients;
00229       ros_coefficients.values = coefficients->values;
00230       ros_coefficients.header = cloud_msg->header;
00231       pub_coefficients_.publish(ros_coefficients);
00232       jsk_recognition_msgs::Torus torus_msg;
00233       torus_msg.header = cloud_msg->header;
00234       tf::poseEigenToMsg(pose, torus_msg.pose);
00235       torus_msg.small_radius = 0.01;
00236       torus_msg.large_radius = coefficients->values[3];
00237       pub_torus_.publish(torus_msg);
00238       jsk_recognition_msgs::TorusArray torus_array_msg;
00239       torus_array_msg.header = cloud_msg->header;
00240       torus_array_msg.toruses.push_back(torus_msg);
00241       pub_torus_array_.publish(torus_array_msg);
00242       // publish pose stamped
00243       geometry_msgs::PoseStamped pose_stamped;
00244       pose_stamped.header = torus_msg.header;
00245       pose_stamped.pose = torus_msg.pose;
00246       pub_pose_stamped_.publish(pose_stamped);
00247       pub_torus_array_with_failure_.publish(torus_array_msg);
00248       pub_torus_with_failure_.publish(torus_msg);
00249     }
00250     else {
00251       jsk_recognition_msgs::Torus torus_msg;
00252       torus_msg.header = cloud_msg->header;
00253       torus_msg.failure = true;
00254       pub_torus_with_failure_.publish(torus_msg);
00255       jsk_recognition_msgs::TorusArray torus_array_msg;
00256       torus_array_msg.header = cloud_msg->header;
00257       torus_array_msg.toruses.push_back(torus_msg);
00258       pub_torus_array_with_failure_.publish(torus_array_msg);
00259       NODELET_INFO("failed to find torus");
00260     }
00261   }
00262 }
00263 
00264 #include <pluginlib/class_list_macros.h>
00265 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::TorusFinder, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:50