snapit_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include "jsk_pcl_ros/snapit.h"
00036 #include <pluginlib/class_list_macros.h>
00037 #include <geometry_msgs/PolygonStamped.h>
00038 #include <Eigen/StdVector>
00039 #include <pcl/ModelCoefficients.h>
00040 #include <pcl/filters/project_inliers.h>
00041 #include <pcl/filters/extract_indices.h>
00042 #include <pcl/sample_consensus/method_types.h>
00043 #include <pcl/sample_consensus/model_types.h>
00044 #include <pcl/segmentation/sac_segmentation.h>
00045 #include <pcl/common/distances.h>
00046 #include <pcl/features/normal_3d.h>
00047 #include <pcl/surface/concave_hull.h>
00048 #include <pcl/common/centroid.h>
00049 #include <visualization_msgs/Marker.h>
00050 #include <eigen_conversions/eigen_msg.h>
00051 #include <tf_conversions/tf_eigen.h>
00052 #include <eigen_conversions/eigen_msg.h>
00053 #include "jsk_recognition_utils/pcl_conversion_util.h"
00054 #include <visualization_msgs/MarkerArray.h>
00055 #include <algorithm>
00056 
00057 namespace jsk_pcl_ros
00058 {
00059   void SnapIt::onInit()
00060   {
00061     DiagnosticNodelet::onInit();
00062     tf_listener_ = TfListenerSingleton::getInstance();
00063     pnh_->param("use_service", use_service_, false);
00064     polygon_aligned_pub_ = advertise<geometry_msgs::PoseStamped>(
00065       *pnh_, "output/plane_aligned", 1);
00066     convex_aligned_pub_ = advertise<geometry_msgs::PoseStamped>(
00067       *pnh_, "output/convex_aligned", 1);
00068     convex_aligned_pose_array_pub_ = advertise<geometry_msgs::PoseArray>(
00069       *pnh_, "output/convex_aligned_pose_array", 1);
00070     if (use_service_) {
00071       subscribe();
00072       align_footstep_srv_ = pnh_->advertiseService(
00073         "align_footstep", &SnapIt::footstepAlignServiceCallback, this);
00074     }
00075     onInitPostProcess();
00076   }
00077 
00078   void SnapIt::subscribe()
00079   {
00080     sub_polygons_.subscribe(*pnh_, "input/polygons", 1);
00081     sub_coefficients_.subscribe(*pnh_, "input/polygon_coefficients", 1);
00082     sync_polygon_
00083       = boost::make_shared<message_filters::Synchronizer<SyncPolygonPolicy> >(100);
00084     sync_polygon_->connectInput(sub_polygons_, sub_coefficients_);
00085     sync_polygon_->registerCallback(
00086       boost::bind(&SnapIt::polygonCallback, this, _1, _2));
00087     polygon_align_sub_ = pnh_->subscribe("input/plane_align", 1,
00088                                          &SnapIt::polygonAlignCallback, this);
00089     convex_align_sub_ = pnh_->subscribe("input/convex_align", 1,
00090                                         &SnapIt::convexAlignCallback, this);
00091     convex_align_polygon_sub_ = pnh_->subscribe(
00092       "input/convex_align_polygon", 1,
00093       &SnapIt::convexAlignPolygonCallback, this);
00094   }
00095 
00096   void SnapIt::unsubscribe()
00097   {
00098     if (!use_service_) {
00099       sub_polygons_.unsubscribe();
00100       sub_coefficients_.unsubscribe();
00101       polygon_align_sub_.shutdown();
00102       convex_align_sub_.shutdown();
00103     }
00104     polygons_.reset();
00105   }
00106   
00107   void SnapIt::polygonCallback(
00108     const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
00109     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg)
00110   {
00111     boost::mutex::scoped_lock lock(mutex_);
00112     vital_checker_->poke();
00113     polygons_ = polygon_msg;
00114   }
00115 
00116   void SnapIt::polygonAlignCallback(
00117     const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
00118   {
00119     boost::mutex::scoped_lock lock(mutex_);
00120     if (!polygons_) {
00121       NODELET_ERROR("no polygon is ready");
00122       polygon_aligned_pub_.publish(pose_msg);
00123       return;
00124     }
00125     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
00126       = createConvexes(pose_msg->header.frame_id,
00127                        pose_msg->header.stamp,
00128                        polygons_);
00129     Eigen::Affine3d pose_eigend;
00130     Eigen::Affine3f pose_eigen;
00131     tf::poseMsgToEigen(pose_msg->pose, pose_eigend);
00132     convertEigenAffine3(pose_eigend, pose_eigen);
00133     Eigen::Vector3f pose_point(pose_eigen.translation());
00134     double min_distance = DBL_MAX;
00135     jsk_recognition_utils::ConvexPolygon::Ptr min_convex;
00136     for (size_t i = 0; i < convexes.size(); i++) {
00137       jsk_recognition_utils::ConvexPolygon::Ptr convex = convexes[i];
00138       double d = convex->distanceToPoint(pose_point);
00139       if (d < min_distance) {
00140         min_convex = convex;
00141         min_distance = d;
00142       }
00143     }
00144     
00145     if (min_convex) {
00146       geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
00147       aligned_pose.header = pose_msg->header;
00148       polygon_aligned_pub_.publish(aligned_pose);
00149     }
00150     else {
00151       polygon_aligned_pub_.publish(pose_msg);
00152     }
00153   }
00154 
00155   bool SnapIt::footstepAlignServiceCallback(
00156     jsk_recognition_msgs::SnapFootstep::Request& req,
00157     jsk_recognition_msgs::SnapFootstep::Response& res)
00158   {
00159     boost::mutex::scoped_lock lock(mutex_);
00160     jsk_footstep_msgs::FootstepArray input_footsteps = req.input;
00161     res.output.header = input_footsteps.header;
00162     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
00163       = createConvexes(input_footsteps.header.frame_id,
00164                        input_footsteps.header.stamp,
00165                        polygons_);
00166     for (size_t i = 0; i < input_footsteps.footsteps.size(); i++) {
00167       jsk_footstep_msgs::Footstep footstep = input_footsteps.footsteps[i];
00168       Eigen::Affine3d pose_eigend;
00169       Eigen::Affine3f pose_eigen;
00170       tf::poseMsgToEigen(footstep.pose, pose_eigen);
00171       Eigen::Vector3f pose_point(pose_eigen.translation());
00172       int min_index = findNearestConvex(pose_point, convexes);
00173       jsk_footstep_msgs::Footstep aligned_footstep;
00174       if (min_index != -1) {
00175         jsk_recognition_utils::ConvexPolygon::Ptr min_convex = convexes[min_index];
00176         geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
00177         aligned_footstep.pose = aligned_pose.pose;
00178       }
00179       else {
00180         aligned_footstep.pose = footstep.pose;
00181         //convex_aligned_pub_.publish(pose_msg); // shoud we publish this?
00182       }
00183       aligned_footstep.leg = footstep.leg;
00184       aligned_footstep.dimensions = footstep.dimensions;
00185       aligned_footstep.duration = footstep.duration;
00186       aligned_footstep.footstep_group = footstep.footstep_group;
00187       res.output.footsteps.push_back(aligned_footstep);
00188     }
00189     return true;
00190   }
00191 
00192   void SnapIt::convexAlignPolygonCallback(
00193     const geometry_msgs::PolygonStamped::ConstPtr& poly_msg)
00194   {
00195     boost::mutex::scoped_lock lock(mutex_);
00196     geometry_msgs::PoseArray pose_array;
00197     pose_array.header = poly_msg->header;
00198     if (!polygons_) {
00199       NODELET_ERROR("no polygon is ready");
00200       return;
00201     }
00202     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
00203       = createConvexes(poly_msg->header.frame_id,
00204                        poly_msg->header.stamp,
00205                        polygons_);
00206     for (size_t i = 0; i < poly_msg->polygon.points.size(); i++) {
00207       geometry_msgs::Point32 p = poly_msg->polygon.points[i];
00208       Eigen::Vector3f pose_point(p.x, p.y, p.z);
00209       int min_index = findNearestConvex(pose_point, convexes);
00210       if (min_index == -1) {
00211         NODELET_ERROR("cannot project onto convex");
00212         return;
00213       }
00214       else {
00215         jsk_recognition_utils::ConvexPolygon::Ptr min_convex = convexes[min_index];
00216         Eigen::Affine3f pose_eigen = Eigen::Affine3f::Identity();
00217         pose_eigen.translate(pose_point);
00218         geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
00219         aligned_pose.header = poly_msg->header;
00220         pose_array.poses.push_back(aligned_pose.pose);
00221       }
00222     }
00223     convex_aligned_pose_array_pub_.publish(pose_array);
00224   }
00225   
00226   int SnapIt::findNearestConvex(
00227     const Eigen::Vector3f& pose_point, 
00228     const std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes)
00229   {
00230     int min_index = -1;
00231     double min_distance = DBL_MAX;
00232     jsk_recognition_utils::ConvexPolygon::Ptr min_convex;
00233     for (size_t i = 0; i < convexes.size(); i++) {
00234       jsk_recognition_utils::ConvexPolygon::Ptr convex = convexes[i];
00235       if (convex->isProjectableInside(pose_point)) {
00236         double d = convex->distanceToPoint(pose_point);
00237         if (d < min_distance) {
00238           min_distance = d;
00239           min_convex = convex;
00240           min_index = i;
00241         }
00242       }
00243     }
00244     return min_index;
00245   }
00246 
00247   void SnapIt::convexAlignCallback(
00248       const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
00249   {
00250     boost::mutex::scoped_lock lock(mutex_);
00251     if (!polygons_) {
00252       NODELET_ERROR("no polygon is ready");
00253       convex_aligned_pub_.publish(pose_msg);
00254       return;
00255     }
00256     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes
00257       = createConvexes(pose_msg->header.frame_id,
00258                        pose_msg->header.stamp,
00259                        polygons_);
00260     Eigen::Affine3d pose_eigend;
00261     Eigen::Affine3f pose_eigen;
00262     tf::poseMsgToEigen(pose_msg->pose, pose_eigend);
00263     convertEigenAffine3(pose_eigend, pose_eigen);
00264     Eigen::Vector3f pose_point(pose_eigen.translation());
00265     int min_index = findNearestConvex(pose_point, convexes);
00266     if (min_index != -1) {
00267       jsk_recognition_utils::ConvexPolygon::Ptr min_convex = convexes[min_index];
00268       geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex);
00269       aligned_pose.header = pose_msg->header;
00270       convex_aligned_pub_.publish(aligned_pose);
00271     }
00272     else {
00273       convex_aligned_pub_.publish(pose_msg); // shoud we publish this?
00274     }
00275   }
00276 
00277   geometry_msgs::PoseStamped SnapIt::alignPose(
00278     Eigen::Affine3f& pose, jsk_recognition_utils::ConvexPolygon::Ptr convex)
00279   {
00280     Eigen::Affine3f aligned_pose(pose);
00281     Eigen::Vector3f original_point(pose.translation());
00282     Eigen::Vector3f projected_point;
00283     convex->project(original_point, projected_point);
00284     
00285     Eigen::Vector3f normal = convex->getNormal();
00286     Eigen::Vector3f old_normal;
00287     old_normal[0] = pose(0, 2);
00288     old_normal[1] = pose(1, 2);
00289     old_normal[2] = pose(2, 2);
00290     Eigen::Quaternionf rot;
00291     if (normal.dot(old_normal) < 0) {
00292       normal = - normal;
00293     }
00294     rot.setFromTwoVectors(old_normal, normal);
00295     aligned_pose = aligned_pose * rot;
00296     aligned_pose.translation() = projected_point;
00297     Eigen::Affine3d aligned_posed;
00298     convertEigenAffine3(aligned_pose, aligned_posed);
00299     geometry_msgs::PoseStamped ret;
00300     tf::poseEigenToMsg(aligned_posed, ret.pose);
00301     return ret;
00302   }
00303   
00304   std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> SnapIt::createConvexes(
00305     const std::string& frame_id, const ros::Time& stamp,
00306     jsk_recognition_msgs::PolygonArray::ConstPtr polygons)
00307   {
00308     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> result;
00309     try
00310     {
00311       for (size_t i = 0; i < polygons->polygons.size(); i++) {
00312         geometry_msgs::PolygonStamped polygon = polygons->polygons[i];
00313         jsk_recognition_utils::Vertices vertices;
00314         
00315         tf::StampedTransform transform = lookupTransformWithDuration(
00316           tf_listener_,
00317           polygon.header.frame_id, frame_id, stamp, ros::Duration(5.0));
00318         for (size_t j = 0; j < polygon.polygon.points.size(); j++) {
00319           Eigen::Vector4d p;
00320           p[0] = polygon.polygon.points[j].x;
00321           p[1] = polygon.polygon.points[j].y;
00322           p[2] = polygon.polygon.points[j].z;
00323           p[3] = 1;
00324           Eigen::Affine3d eigen_transform;
00325           tf::transformTFToEigen(transform, eigen_transform);
00326           Eigen::Vector4d transformed_pointd = eigen_transform.inverse() * p;
00327           Eigen::Vector3f transformed_point;
00328           transformed_point[0] = transformed_pointd[0];
00329           transformed_point[1] = transformed_pointd[1];
00330           transformed_point[2] = transformed_pointd[2];
00331           vertices.push_back(transformed_point);
00332         }
00333         std::reverse(vertices.begin(), vertices.end());
00334         jsk_recognition_utils::ConvexPolygon::Ptr convex(new jsk_recognition_utils::ConvexPolygon(vertices));
00335         result.push_back(convex);
00336       }
00337     }
00338     catch (tf2::ConnectivityException &e)
00339     {
00340       NODELET_ERROR("Transform error: %s", e.what());
00341     }
00342     catch (tf2::InvalidArgumentException &e)
00343     {
00344       NODELET_ERROR("Transform error: %s", e.what());
00345     }
00346     catch (tf2::ExtrapolationException &e)
00347     {
00348       NODELET_ERROR("Transform error: %s", e.what());
00349     }
00350     return result;
00351   }
00352 }
00353 
00354 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::SnapIt, nodelet::Nodelet);


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