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