00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
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){
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{
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
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
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;
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
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