handle_estimator_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, 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 
00036 #include "jsk_pcl_ros/handle_estimator.h"
00037 #include <pluginlib/class_list_macros.h>
00038 #include <geometry_msgs/PoseArray.h>
00039 #include <geometry_msgs/PoseStamped.h>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include "jsk_recognition_utils/geo_util.h"
00042 
00043 namespace jsk_pcl_ros
00044 {
00045   void HandleEstimator::onInit()
00046   {
00047     ConnectionBasedNodelet::onInit();
00048     output_buf.resize(100);
00049 
00050     pnh_->param("gripper_size", gripper_size_, 0.08); // defaults to pr2 gripper size
00051     pnh_->param("approach_offset", approach_offset_, 0.1); // default to 10 cm
00052     pnh_->param("angle_divide_num", angle_divide_num_, 6); // even will be better
00053     pub_ = advertise<geometry_msgs::PoseArray>(*pnh_, "output", 1);
00054     pub_best_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output_best", 1);
00055     pub_selected_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output_selected", 1);
00056 
00057     pub_preapproach_ = advertise<geometry_msgs::PoseArray>(*pnh_, "output_preapproach", 1);
00058     pub_selected_preapproach_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output_selected_preapproach", 1);
00059 
00060     onInitPostProcess();
00061   }
00062 
00063   void HandleEstimator::subscribe()
00064   {
00065     sub_index_ = pnh_->subscribe<jsk_recognition_msgs::Int32Stamped>("selected_index", 1, boost::bind( &HandleEstimator::selectedIndexCallback, this, _1));
00066     sub_input_.subscribe(*pnh_, "input", 1);
00067     sub_box_.subscribe(*pnh_, "input_box", 1);
00068     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00069     sync_->connectInput(sub_input_, sub_box_);
00070     sync_->registerCallback(boost::bind(&HandleEstimator::estimate, this, _1, _2));
00071   }
00072 
00073   void HandleEstimator::unsubscribe()
00074   {
00075     sub_index_.shutdown();
00076     sub_input_.unsubscribe();
00077     sub_box_.unsubscribe();
00078   }
00079   
00080   void HandleEstimator::estimate(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00081                                  const jsk_recognition_msgs::BoundingBox::ConstPtr& box_msg)
00082   {
00083     // pack dimensions into vector
00084     std::vector<double> dimensions;
00085     dimensions.push_back(box_msg->dimensions.x);
00086     dimensions.push_back(box_msg->dimensions.y);
00087     dimensions.push_back(box_msg->dimensions.z);
00088     size_t longest_index = 0;
00089     for (size_t i = 1; i < 3; i++) {
00090       if (dimensions[i] > dimensions[longest_index]) {
00091         longest_index = i;
00092       }
00093     }
00094     NODELET_INFO_STREAM("longest index is: " << longest_index);
00095     HandleType handle_type;
00096     // detect the handle type
00097     if (longest_index == 2) {
00098       if (dimensions[0] < gripper_size_ || dimensions[1] < gripper_size_) {
00099         handle_type = HANDLE_SMALL_ENOUGH_STAND_ON_PLANE;
00100       }
00101       else {
00102         handle_type = NO_HANDLE;
00103       }
00104     }
00105     else {
00106       if (longest_index == 0) {
00107         if (dimensions[1] < gripper_size_ || dimensions[2] < gripper_size_) {
00108           handle_type = HANDLE_SMALL_ENOUGH_LIE_ON_PLANE_X_LONGEST;
00109         }
00110         else {
00111           handle_type = NO_HANDLE;
00112         }
00113       }
00114       else {                    // 1
00115         if (dimensions[0] < gripper_size_ || dimensions[2] < gripper_size_) {
00116           handle_type = HANDLE_SMALL_ENOUGH_LIE_ON_PLANE_Y_LONGEST;
00117         }
00118         else {
00119           handle_type = NO_HANDLE;
00120         }
00121       }
00122       
00123     }
00124     estimateHandle(handle_type, cloud_msg, box_msg);
00125   }
00126 
00127   void HandleEstimator::estimateHandle(const HandleType& handle_type,
00128                                        const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00129                                        const jsk_recognition_msgs::BoundingBox::ConstPtr& box_msg)
00130   {
00131     if (handle_type == NO_HANDLE) {
00132       NODELET_ERROR("failed to estimate handle");
00133     }
00134     else if (handle_type == HANDLE_SMALL_ENOUGH_LIE_ON_PLANE_Y_LONGEST) {
00135       handleSmallEnoughLieOnPlane(cloud_msg, box_msg, true);
00136     }
00137     else if (handle_type == HANDLE_SMALL_ENOUGH_LIE_ON_PLANE_X_LONGEST) {
00138       handleSmallEnoughLieOnPlane(cloud_msg, box_msg, false);
00139     }
00140     else if (handle_type == HANDLE_SMALL_ENOUGH_STAND_ON_PLANE) {
00141       handleSmallEnoughStandOnPlane(cloud_msg, box_msg);
00142     }
00143   }
00144 
00145   void HandleEstimator::handleSmallEnoughStandOnPlane(
00146     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00147     const jsk_recognition_msgs::BoundingBox::ConstPtr& box_msg)
00148   {
00149     geometry_msgs::PoseArray pose_array;
00150     geometry_msgs::PoseArray prepose_array;
00151     pose_array.header = box_msg->header;
00152     prepose_array.header = box_msg->header;
00153     Eigen::Affine3d center;
00154     tf::poseMsgToEigen(box_msg->pose, center);
00155     Eigen::Vector3d z = center.rotation().col(2);
00156     Plane p(Eigen::Vector3f(z[0], z[1], z[2]), 0);
00157     Eigen::Vector3d ray = center.translation().normalized();
00158     Eigen::Vector3d ray_projected;
00159     p.project(ray, ray_projected);
00160     ray_projected.normalize();
00161     
00162     Eigen::Matrix3d center_refined_mat;
00163     center_refined_mat.col(0) = ray_projected;
00164     center_refined_mat.col(2) = z;
00165     center_refined_mat.col(1) = z.cross(ray_projected);
00166     Eigen::Affine3d center_refined
00167       = Eigen::Translation3d(center.translation()) * Eigen::Quaterniond(center_refined_mat);
00168 
00169     const double angle_margin = 0.2;
00170     const double start_angle = M_PI / 2.0 + angle_margin;
00171     const double end_angle = M_PI + M_PI / 2.0 - angle_margin;
00172     for (size_t i = 0; i < angle_divide_num_; i++) {
00173       const double theta = (end_angle - start_angle) / angle_divide_num_ * i + start_angle;
00174       Eigen::Vector3d offset_vec = Eigen::Vector3d(cos(theta), sin(theta), 0);
00175       Eigen::Vector3d pre_approach_pos
00176         = (center_refined * (approach_offset_ * offset_vec));
00177       Eigen::Matrix3d orientation;
00178       Eigen::Vector3d x_vec
00179         = (center_refined.translation() - pre_approach_pos).normalized();
00180       Eigen::Vector3d y_vec = z.cross(x_vec);
00181       
00182       orientation.col(0) = x_vec;
00183       orientation.col(1) = y_vec;
00184       orientation.col(2) = z;
00185 
00186       Eigen::Affine3d pre_grasp_pose = Eigen::Translation3d(pre_approach_pos) * Eigen::Quaterniond(orientation);
00187       Eigen::Affine3d grasp_pose = Eigen::Translation3d(center.translation()) * Eigen::Quaterniond(orientation);
00188       geometry_msgs::Pose ros_prepose;
00189       geometry_msgs::Pose ros_pose;
00190       tf::poseEigenToMsg(pre_grasp_pose, ros_prepose);
00191       tf::poseEigenToMsg(grasp_pose, ros_pose);
00192       prepose_array.poses.push_back(ros_prepose);
00193       pose_array.poses.push_back(ros_pose);
00194     }
00195     pub_.publish(pose_array);
00196     pub_preapproach_.publish(prepose_array);
00197     geometry_msgs::PoseStamped best;
00198     best.header = pose_array.header;
00199     best.pose = pose_array.poses[pose_array.poses.size() / 2];
00200     pub_best_.publish(best);
00201 
00202     output_buf.push_front(boost::make_tuple(pose_array, prepose_array));
00203   }
00204   
00205   void HandleEstimator::handleSmallEnoughLieOnPlane(
00206     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00207     const jsk_recognition_msgs::BoundingBox::ConstPtr& box_msg,
00208     bool y_longest)
00209   {
00210     geometry_msgs::PoseArray pose_array;
00211     geometry_msgs::PoseArray prepose_array;
00212     pose_array.header = box_msg->header;
00213     prepose_array.header = box_msg->header;
00214     // center
00215     Eigen::Affine3d center;
00216     tf::poseMsgToEigen(box_msg->pose, center);
00217     const double angle_margin = 0.2;
00218     const double start_angle = angle_margin;
00219     const double end_angle = M_PI - angle_margin;
00220     
00221     for (size_t i = 0; i <= angle_divide_num_; i++) { // angle_divide_num samples
00222       const double theta = (end_angle - start_angle) / angle_divide_num_ * i + start_angle;
00223       // x-z plane
00224       Eigen::Vector3d offset_vec;
00225       if (y_longest) {
00226         offset_vec = Eigen::Vector3d(sin(theta), 0, cos(theta));
00227       }
00228       else {
00229         offset_vec = Eigen::Vector3d(0, cos(theta), sin(theta));
00230       }
00231       Eigen::Vector3d pre_approach_pos
00232         = (center * (approach_offset_ * offset_vec));
00233       // compute unit vectors to construct orientation
00234       Eigen::Matrix3d orientation;
00235       if (y_longest) {
00236         Eigen::Vector3d x_vec
00237           = (center.translation() - pre_approach_pos).normalized();
00238         Eigen::Vector3d z_vec = center.rotation().col(1);
00239         Eigen::Vector3d y_vec = z_vec.cross(x_vec);
00240         orientation.col(0) = x_vec;
00241         orientation.col(1) = y_vec;
00242         orientation.col(2) = z_vec;
00243       }
00244       else {
00245         Eigen::Vector3d x_vec = (center.translation() - pre_approach_pos).normalized();
00246         Eigen::Vector3d z_vec = center.rotation().col(0);
00247         Eigen::Vector3d y_vec = z_vec.cross(x_vec);
00248         orientation.col(0) = x_vec;
00249         orientation.col(1) = y_vec;
00250         orientation.col(2) = z_vec;
00251       }
00252       
00253       Eigen::Affine3d pre_grasp_pose = Eigen::Translation3d(pre_approach_pos) * Eigen::Quaterniond(orientation);
00254       Eigen::Affine3d grasp_pose = Eigen::Translation3d(center.translation()) * Eigen::Quaterniond(orientation);
00255       geometry_msgs::Pose ros_prepose, ros_pose;
00256       tf::poseEigenToMsg(pre_grasp_pose, ros_prepose);
00257       tf::poseEigenToMsg(grasp_pose, ros_pose);
00258       pose_array.poses.push_back(ros_pose);
00259       prepose_array.poses.push_back(ros_prepose);
00260     }
00261     pub_.publish(pose_array);
00262     pub_preapproach_.publish(prepose_array);
00263     geometry_msgs::PoseStamped best;
00264     best.header = pose_array.header;
00265     best.pose = pose_array.poses[pose_array.poses.size() / 2];
00266     pub_best_.publish(best);
00267 
00268     output_buf.push_front(boost::make_tuple(pose_array, prepose_array));
00269   }
00270   
00271   void HandleEstimator::selectedIndexCallback( const jsk_recognition_msgs::Int32StampedConstPtr &index){
00272     boost::circular_buffer<boost::tuple<geometry_msgs::PoseArray, geometry_msgs::PoseArray> >::iterator it = output_buf.begin();
00273     while (it != output_buf.end()) {
00274       geometry_msgs::PoseArray pose_array = it->get<0>();
00275       geometry_msgs::PoseArray prepose_array = it->get<1>();
00276 
00277       if(pose_array.header.stamp == index->header.stamp){
00278         geometry_msgs::PoseStamped ps;
00279         ps.header = pose_array.header;
00280         ps.pose = pose_array.poses[index->data];
00281         pub_selected_.publish(ps);
00282 
00283         ps.header = prepose_array.header;
00284         ps.pose = prepose_array.poses[index->data];
00285         pub_selected_preapproach_.publish(ps);
00286         break;
00287       }
00288       ++it;
00289     }
00290   }
00291 }
00292 
00293 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::HandleEstimator, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:43