hinted_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) 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 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include "jsk_pcl_ros/hinted_handle_estimator.h"
00038 #include "jsk_recognition_utils/pcl_conversion_util.h"
00039 #include <pcl/features/normal_3d.h>
00040 #include <pcl/kdtree/kdtree_flann.h>
00041 #include <pcl/common/transforms.h>
00042 #include <pcl/filters/passthrough.h>
00043 #include <jsk_recognition_utils/pcl_conversion_util.h>
00044 #include <eigen_conversions/eigen_msg.h>
00045 #include <tf/tf.h>
00046 #include <math.h>
00047 
00048 using namespace std;
00049 
00050 inline float NORM(float vx, float vy, float vz){return sqrt(vx*vx+vy*vy+vz*vz);}
00051 
00052 namespace jsk_pcl_ros
00053 {
00054   void HintedHandleEstimator::onInit()
00055   {
00056     DiagnosticNodelet::onInit();
00057     pub_pose_ = advertise<geometry_msgs::PoseStamped>(
00058                                                       *pnh_, "handle_pose", 1);
00059     pub_length_ = advertise<std_msgs::Float64>(
00060                                                *pnh_, "handle_length", 1);
00061     pub_handle_ = advertise<jsk_recognition_msgs::SimpleHandle>(
00062                                                *pnh_, "handle", 1);
00063     pub_debug_marker_ = advertise<visualization_msgs::Marker>(*pnh_, "debug_marker", 1);
00064     pub_debug_marker_array_ = advertise<visualization_msgs::MarkerArray>(*pnh_, "debug_marker_array", 1);
00065     handle = handle_model();
00066     pnh_->param("finger_l", handle.finger_l, 0.03);
00067     pnh_->param("finger_d", handle.finger_d, 0.02);
00068     pnh_->param("finger_w", handle.finger_w, 0.01);
00069     pnh_->param("arm_l", handle.arm_l, 0.05);
00070     pnh_->param("arm_d", handle.arm_d, 0.02);
00071     pnh_->param("arm_w", handle.arm_w, 0.1);
00072     onInitPostProcess();
00073   }
00074 
00075   void HintedHandleEstimator::subscribe()
00076   {
00077     sub_cloud_.subscribe(*pnh_, "cloud", 1);
00078     sub_point_.subscribe(*pnh_, "point", 1); 
00079     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00080     sync_->connectInput(sub_cloud_, sub_point_);
00081     sync_->registerCallback(boost::bind(&HintedHandleEstimator::estimate, this, _1, _2));
00082   }
00083   
00084   void HintedHandleEstimator::unsubscribe()
00085   {
00086     sub_cloud_.unsubscribe();
00087     sub_point_.unsubscribe();
00088   }
00089   
00090   void HintedHandleEstimator::estimate(
00091     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00092     const geometry_msgs::PointStampedConstPtr &point_msg)
00093   {
00094     boost::mutex::scoped_lock lock(mutex_);
00095     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00096     pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
00097     pcl::PassThrough<pcl::PointXYZ> pass;
00098     int K = 1;
00099     std::vector<int> pointIdxNKNSearch(K);
00100     std::vector<float> pointNKNSquaredDistance(K);
00101     pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>);
00102 
00103     pcl::fromROSMsg(*cloud_msg, *cloud);
00104     geometry_msgs::PointStamped transed_point;
00105     ros::Time now = ros::Time::now();
00106     try
00107       {
00108       listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0));
00109       listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point);
00110     }
00111     catch(tf::TransformException ex)
00112       {
00113       ROS_ERROR("%s", ex.what());
00114       return;
00115     }
00116     pcl::PointXYZ searchPoint;
00117     searchPoint.x = transed_point.point.x;
00118     searchPoint.y = transed_point.point.y;
00119     searchPoint.z = transed_point.point.z;
00120 
00121     //remove too far cloud
00122     pass.setInputCloud(cloud);
00123     pass.setFilterFieldName("x");
00124     pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w);
00125     pass.filter(*cloud);
00126     pass.setInputCloud(cloud);
00127     pass.setFilterFieldName("y");
00128     pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w);
00129     pass.filter(*cloud);
00130     pass.setInputCloud(cloud);
00131     pass.setFilterFieldName("z");
00132     pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w);
00133     pass.filter(*cloud);
00134 
00135     if(cloud->points.size() < 10){
00136       ROS_INFO("points are too small");
00137       return;
00138     }
00139     if(1){ //estimate_normal
00140       pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
00141       ne.setInputCloud(cloud);
00142       ne.setSearchMethod(kd_tree);
00143       ne.setRadiusSearch(0.02);
00144       ne.setViewPoint(0, 0, 0);
00145       ne.compute(*cloud_normals);
00146     }
00147     else{ //use normal of msg
00148       
00149     }
00150     if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){
00151       ROS_INFO("kdtree failed");
00152       return;
00153     }
00154     float x = cloud->points[pointIdxNKNSearch[0]].x;
00155     float y = cloud->points[pointIdxNKNSearch[0]].y;
00156     float z = cloud->points[pointIdxNKNSearch[0]].z;
00157     float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x;
00158     float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y;
00159     float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z;
00160     double theta = acos(v_x);
00161     // use normal for estimating handle direction
00162     tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2));
00163     tf::Quaternion final_quaternion = normal;
00164     double min_theta_index = 0;
00165     double min_width = 100;
00166     tf::Quaternion min_qua(0, 0, 0, 1);
00167     visualization_msgs::Marker debug_hand_marker;
00168     debug_hand_marker.header = cloud_msg->header;
00169     debug_hand_marker.ns = string("debug_grasp");
00170     debug_hand_marker.id = 0;
00171     debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST;
00172     debug_hand_marker.pose.orientation.w = 1;
00173     debug_hand_marker.scale.x=0.003;
00174     tf::Matrix3x3 best_mat;
00175     //search 180 degree and calc the shortest direction
00176     for(double theta_=0; theta_<3.14/2; 
00177         theta_+=3.14/2/30){
00178       tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_));
00179       tf::Quaternion temp_qua = normal * rotate_;
00180       tf::Matrix3x3 temp_mat(temp_qua);
00181       geometry_msgs::Pose pose_respected_to_tf;
00182       pose_respected_to_tf.position.x = x;
00183       pose_respected_to_tf.position.y = y;
00184       pose_respected_to_tf.position.z = z;
00185       pose_respected_to_tf.orientation.x = temp_qua.getX();
00186       pose_respected_to_tf.orientation.y = temp_qua.getY();
00187       pose_respected_to_tf.orientation.z = temp_qua.getZ();
00188       pose_respected_to_tf.orientation.w = temp_qua.getW();
00189       Eigen::Affine3d box_pose_respected_to_cloud_eigend;
00190       tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend);
00191       Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
00192         = box_pose_respected_to_cloud_eigend.inverse();
00193       Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
00194       Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
00195         = box_pose_respected_to_cloud_eigend_inversed.matrix();
00196       jsk_recognition_utils::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
00197                                                                     box_pose_respected_to_cloud_eigen_inversed_matrixd,
00198                                                                     box_pose_respected_to_cloud_eigen_inversed_matrixf);
00199       Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
00200       pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
00201       pcl::transformPointCloud(*cloud, *output_cloud, offset);
00202 
00203       pcl::PassThrough<pcl::PointXYZ> pass;
00204       pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>);
00205       pass.setInputCloud(output_cloud);
00206       pass.setFilterFieldName("y");
00207       pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2);
00208       pass.filter(*points_z);
00209       pass.setInputCloud(points_z);
00210       pass.setFilterFieldName("z");
00211       pass.setFilterLimits(-handle.finger_d, handle.finger_d);
00212       pass.filter(*points_yz);
00213       pass.setInputCloud(points_yz);
00214       pass.setFilterFieldName("x");
00215       pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l);
00216       pass.filter(*points_xyz);
00217       pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
00218       for(size_t index=0; index<points_xyz->size(); index++){
00219         points_xyz->points[index].x = points_xyz->points[index].z = 0;
00220       }
00221       if(points_xyz->points.size() == 0){ROS_INFO("points are empty");return;}
00222       kdtree.setInputCloud(points_xyz);
00223       std::vector<int> pointIdxRadiusSearch;
00224       std::vector<float> pointRadiusSquaredDistance;
00225       pcl::PointXYZ search_point_tree;
00226       search_point_tree.x=search_point_tree.y=search_point_tree.z=0;
00227       if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
00228         double before_w=10, temp_w;
00229         for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){
00230           temp_w =sqrt(pointRadiusSquaredDistance[index]);
00231           if(temp_w - before_w > handle.finger_w*2){
00232             break; // there are small space for finger
00233           }
00234           before_w=temp_w;
00235         }
00236         if(before_w < min_width){
00237           min_theta_index = theta_;
00238           min_width = before_w;
00239           min_qua = temp_qua;
00240           best_mat = temp_mat;
00241         }
00242         //for debug view
00243         geometry_msgs::Point temp_point;
00244         std_msgs::ColorRGBA temp_color;
00245         temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1;
00246         temp_point.x=x-temp_mat.getColumn(1)[0] * before_w;
00247         temp_point.y=y-temp_mat.getColumn(1)[1] * before_w;
00248         temp_point.z=z-temp_mat.getColumn(1)[2] * before_w;
00249         debug_hand_marker.points.push_back(temp_point);
00250         debug_hand_marker.colors.push_back(temp_color);
00251         temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w;
00252         temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w;
00253         temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w;
00254         debug_hand_marker.points.push_back(temp_point);
00255         debug_hand_marker.colors.push_back(temp_color);
00256       }
00257     }
00258     geometry_msgs::PoseStamped handle_pose_stamped;
00259     handle_pose_stamped.header = cloud_msg->header;
00260     handle_pose_stamped.pose.position.x = x;
00261     handle_pose_stamped.pose.position.y = y;
00262     handle_pose_stamped.pose.position.z = z;
00263     handle_pose_stamped.pose.orientation.x = min_qua.getX();
00264     handle_pose_stamped.pose.orientation.y = min_qua.getY();
00265     handle_pose_stamped.pose.orientation.z = min_qua.getZ();
00266     handle_pose_stamped.pose.orientation.w = min_qua.getW();
00267     std_msgs::Float64 min_width_msg;
00268     min_width_msg.data = min_width;
00269     pub_pose_.publish(handle_pose_stamped);
00270     pub_debug_marker_.publish(debug_hand_marker);
00271     pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle));
00272     jsk_recognition_msgs::SimpleHandle simple_handle;
00273     simple_handle.header = handle_pose_stamped.header;
00274     simple_handle.pose = handle_pose_stamped.pose;
00275     simple_handle.handle_width = min_width;
00276     pub_handle_.publish(simple_handle);
00277   }
00278 }
00279 
00280 #include <pluginlib/class_list_macros.h>
00281 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::HintedHandleEstimator, nodelet::Nodelet);
00282 
00283 


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