$search
00001 /* 00002 * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu> 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include <ias_drawer_executive/Perception3d.h> 00031 #include <ias_drawer_executive/Geometry.h> 00032 #include <pcl/ros/conversions.h> 00033 00034 boost::mutex Perception3d::handle_mutex; 00035 boost::mutex Perception3d::plane_mutex; 00036 00037 std::vector<tf::Stamped<tf::Pose> *> Perception3d::handlePoses = *(new std::vector<tf::Stamped<tf::Pose> *> ()); 00038 std::vector<tf::Stamped<tf::Pose> *> Perception3d::planePoses = *(new std::vector<tf::Stamped<tf::Pose> *> ()); 00039 00040 boost::mutex Perception3d::handle_cloud_mutex; 00041 PointCloudT Perception3d::lastCloud; 00042 int op_handle_cloud_cnt = 0; 00043 00044 btVector3 Perception3d::handleHint; 00045 btVector3 Perception3d::handleResult; 00046 double Perception3d::handleMinDist; 00047 ros::Time Perception3d::cloud_time; 00048 ros::Time Perception3d::query_time; 00049 00050 00051 void Perception3d::handleCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) 00052 { 00053 //make this a boost shared ptr! 00054 tf::Stamped<tf::Pose> *newPose = new tf::Stamped<tf::Pose>(); 00055 newPose->frame_id_ = msg->header.frame_id; 00056 newPose->stamp_ = msg->header.stamp; 00057 newPose->setOrigin(btVector3(msg->pose.position.x,msg->pose.position.y,msg->pose.position.z)); 00058 newPose->setRotation(btQuaternion(msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z,msg->pose.orientation.w)); 00059 ROS_INFO("CALLBACK GOT HANDLE AT %f %f %f", msg->pose.position.x,msg->pose.position.y,msg->pose.position.z); 00060 handle_mutex.lock(); 00061 handlePoses.push_back(newPose); 00062 handle_mutex.unlock(); 00063 } 00064 00065 //ostopic echo /stereo_table_cluster_detector/pointcloud_minmax_3d_node/cluster 00066 00067 tf::Stamped<tf::Pose> Perception3d::getHandlePoseFromLaser(int pos) 00068 { 00069 00070 00071 system("rosservice call laser_tilt_controller/set_periodic_cmd '{ command: { header: { stamp: 0 }, profile: \"linear\" , period: 10 , amplitude: 0.8 , offset: 0.3 }}'"); 00072 00073 handlePoses.clear(); 00074 ros::NodeHandle n_; 00075 ros::Subscriber sub_= n_.subscribe("/handle_detector/pointcloud_line_pose_node/handle_pose", 100, &Perception3d::handleCallback); 00076 00077 size_t numHandles = 0; 00078 ros::Rate rate(20); 00079 while (ros::ok() && (numHandles < 10)) 00080 { 00081 handle_mutex.lock(); 00082 if (Perception3d::handlePoses.size() > numHandles) 00083 { 00084 ROS_INFO("LAST HANDLE: %f %f %f ", handlePoses[numHandles]->getOrigin().x(),handlePoses[numHandles]->getOrigin().y(),handlePoses[numHandles]->getOrigin().z()); 00085 numHandles = handlePoses.size(); 00086 } 00087 handle_mutex.unlock(); 00088 rate.sleep(); 00089 ros::spinOnce(); 00090 } 00091 00092 return *handlePoses[0]; 00093 00094 /*tf::Stamped<tf::Pose> decid; 00095 decid.frame_id_ = "none"; 00096 decid.stamp_ = ros::Time(); 00097 decid.setOrigin(btVector3(1,0,0)); 00098 decid.setRotation(btQuaternion(0,0,0,1)); 00099 btTransform classify = transform * decid; 00100 00101 tf::Stamped<tf::Pose> decid2; 00102 decid2.frame_id_ = "none"; 00103 decid2.stamp_ = ros::Time(); 00104 decid2.setOrigin(btVector3(0,1,0)); 00105 decid2.setRotation(btQuaternion(0,0,0,1)); 00106 00107 btTransform classify2 = transform * decid2; 00108 00109 ROS_INFO("DECID :c %f c1 %f m %f", classify.getOrigin().z(), classify2.getOrigin().z(), transform.getOrigin().z()); 00110 00111 if (classify.getOrigin().z() < classify2.getOrigin().z()) 00112 { 00113 //numh++; 00114 ROS_INFO("HORIZONTAL %i");//, numh); 00115 } 00116 else 00117 { 00118 //numv++; 00119 ROS_INFO("VERTICAL %i");//, numv); 00120 }*/ 00121 } 00122 00123 btQuaternion resOri; 00124 00125 void Perception3d::handleCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) 00126 { 00127 00128 ros::Time t(msg->header.stamp); 00129 cloud_time = t; 00130 // ignore clouds taken before query time, since robot may have been moving, which results in problems since clouds are assembled in base_link 00131 if (t < query_time) 00132 return; 00133 //make this a boost shared ptr! 00134 handle_cloud_mutex.lock(); 00135 pcl::fromROSMsg(*msg,lastCloud); 00136 00137 bool takethisone = false; 00138 for (size_t k = 0; k < lastCloud.points.size(); k++) 00139 { 00140 btVector3 act(lastCloud.points[k].x,lastCloud.points[k].y,lastCloud.points[k].z); 00141 double dist = btVector3(act - handleHint).length(); 00142 if (dist < handleMinDist) 00143 { 00144 handleMinDist = dist; 00145 handleResult = act; 00146 takethisone = true; 00147 } 00148 } 00149 00150 // if this handle made it, calculate an orientation for it 00151 if (takethisone) { 00152 btVector3 l(lastCloud.points[0].x,lastCloud.points[0].y,lastCloud.points[0].z); 00153 btVector3 r(lastCloud.points[lastCloud.points.size() / 2].x,lastCloud.points[lastCloud.points.size() / 2].y,lastCloud.points[lastCloud.points.size() / 2].z); 00154 btVector3 rel = r - l; 00155 double at2 = atan2(rel.y(), rel.z()); 00156 btQuaternion ori(btVector3(1,0,0), at2); 00157 resOri = ori; 00158 } 00159 00160 //if (takethisone) { 00161 // btVector3 l(lastCloud.points[0].x,lastCloud.points[0].y,lastCloud.points[0].z); 00162 // btVector3 r(lastCloud.points[lastCloud.points.size() / 2].x,lastCloud.points[lastCloud.points.size() / 2].y,lastCloud.points[lastCloud.points.size() / 2].z); 00163 // btVector3 rel = r - l; 00164 // double at2 = atan2(rel.y(), rel.x()); 00165 // btQuaternion ori(btVector3(0,0,1), at2); 00166 // resOri = ori; 00167 //} 00168 00169 00170 //btVector3 rel = leftEdge.getOrigin() - rightEdge.getOrigin(); 00171 //double at2 = atan2(rel.y(), rel.x()); 00172 //btQuaternion ori(btVector3(0,0,1), at2); 00173 00174 00175 //lastCloud = msg; 00176 op_handle_cloud_cnt++; 00177 00178 ROS_INFO("GOT CLOUD %i", op_handle_cloud_cnt); 00179 handle_cloud_mutex.unlock(); 00180 } 00181 00182 tf::Stamped<tf::Pose> Perception3d::getHandlePoseFromLaser(tf::Stamped<tf::Pose> hint, double timeout) 00183 { 00184 00185 system("rosservice call laser_tilt_controller/set_periodic_cmd '{ command: { header: { stamp: 0 }, profile: \"linear\" , period: 10 , amplitude: 0.8 , offset: 0.3 }}'"); 00186 00187 query_time = ros::Time::now(); 00188 cloud_time = ros::Time(0); 00189 00190 tf::Stamped<tf::Pose> inBase = Geometry::getPoseIn("base_link", hint); 00191 00192 ROS_INFO("HINT in map"); 00193 Geometry::printPose(hint); 00194 ROS_INFO("HINT in base"); 00195 Geometry::printPose(inBase); 00196 00197 ros::NodeHandle n_; 00198 ros::Subscriber subHandleInliers = n_.subscribe("/handle_detector/handle_projected_inliers/output", 10, Perception3d::handleCloudCallback); 00199 // ros::Subscriber subHandleInliers = n_.subscribe("/handle_detector/handle_candidates_protrude/output", 10, Perception3d::handleCloudCallback); 00200 00201 bool found = false; 00202 ros::Rate rate(3); 00203 00204 int maxNum = 5; 00205 00206 btVector3 best(0,0,0); 00207 00208 handleHint = inBase.getOrigin(); 00209 handleMinDist = 100; 00210 double curBestDist = 100; 00211 int goodcloud = op_handle_cloud_cnt + 100000; 00212 00213 ROS_INFO("Waiting for handles published after query"); 00214 00215 while (cloud_time < query_time) { 00216 rate.sleep(); 00217 ros::spinOnce(); 00218 00219 if ((timeout > 0) && (ros::Time::now() > query_time + ros::Duration(timeout))) 00220 return hint; 00221 } 00222 00223 ros::Time current_timeslice = cloud_time; 00224 00225 ROS_INFO("Waiting done"); 00226 00227 //while (((!found) || (op_handle_cloud_cnt < goodcloud + 2)) && (maxNum > 0)) 00228 00229 //while (!found && (maxNum > 0)) 00230 while (cloud_time == current_timeslice) 00231 { 00232 rate.sleep(); 00233 ros::spinOnce(); 00234 if (handleMinDist < curBestDist) 00235 { 00236 // if we got better by at least 2 cm look for another 10 clouds 00237 //if (handleMinDist < curBestDist + 0.02) 00238 //goodcloud = op_handle_cloud_cnt; 00239 found = true; 00240 best = handleResult; 00241 curBestDist = handleMinDist; 00242 } 00243 ROS_INFO("Dist : %f", handleMinDist); 00244 00245 if ((timeout > 0) && (ros::Time::now() > query_time + ros::Duration(timeout))) 00246 return hint; 00247 } 00248 00249 tf::Stamped<tf::Pose> ret; 00250 ret.frame_id_ = "base_link"; 00251 ret.setOrigin(best); 00252 //ret.setRotation(btQuaternion(0,0,0,1)); 00253 ret.setRotation(resOri); 00254 00255 if (maxNum <= 0) 00256 ret.setOrigin(btVector3(0,0,-1)); 00257 00258 ROS_INFO("RET"); 00259 Geometry::printPose(ret); 00260 00261 ret = Geometry::getPoseIn(hint.frame_id_.c_str(), ret); 00262 00263 return ret; 00264 } 00265 00266 00267 void Perception3d::bottleCallback(const ias_table_msgs::TableCluster::ConstPtr& msg) 00268 { 00269 //make this a boost shared ptr! 00270 tf::Stamped<tf::Pose> *newPose = new tf::Stamped<tf::Pose>(); 00271 newPose->frame_id_ = msg->header.frame_id; 00272 newPose->stamp_ = msg->header.stamp; 00273 newPose->setOrigin(btVector3(msg->center.x,msg->center.y,msg->center.z)); 00274 //newPose->setRotation(btQuaternion(msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z,msg->pose.orientation.w)); 00275 ROS_INFO("CALLBACK GOT HANDLE AT %f %f %f", msg->center.x,msg->center.y,msg->center.z); 00276 handle_mutex.lock(); 00277 handlePoses.push_back(newPose); 00278 handle_mutex.unlock(); 00279 } 00280 00281 00282 tf::Stamped<tf::Pose> Perception3d::getBottlePose() 00283 { 00284 00285 //system("rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node projector_mode 3"); 00286 //system("rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node narrow_stereo_trig_mode 3"); 00287 int sysret = system("rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node '{projector_mode: 3, narrow_stereo_trig_mode: 3}'"); 00288 ROS_INFO("%i rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node '{projector_mode: 3, narrow_stereo_trig_mode: 3}'", sysret); 00289 //handlePoses.clear(); 00290 00291 //tf::Stamped<tf::Pose> *newPose = new tf::Stamped<tf::Pose>(); 00292 //handlePoses.push_back(newPose); 00293 00294 ros::NodeHandle n_; 00295 ros::Subscriber sub_= n_.subscribe("/stereo_table_cluster_detector/pointcloud_minmax_3d_node/cluster", 100, &Perception3d::bottleCallback); 00296 00297 size_t numHandles = 0; 00298 ros::Rate rate(20); 00299 00300 btVector3 average(0,0,0); 00301 double numav = 0; 00302 00303 tf::Stamped<tf::Pose> ret; 00304 00305 while (ros::ok() && (numHandles < 1)) 00306 { 00307 handle_mutex.lock(); 00308 if (Perception3d::handlePoses.size() > numHandles) 00309 { 00310 ROS_INFO("LAST HANDLE: %f %f %f ", handlePoses[numHandles]->getOrigin().x(),handlePoses[numHandles]->getOrigin().y(),handlePoses[numHandles]->getOrigin().z()); 00311 average += btVector3(handlePoses[numHandles]->getOrigin().x(),handlePoses[numHandles]->getOrigin().y(),handlePoses[numHandles]->getOrigin().z()); 00312 ret.stamp_ = handlePoses[numHandles]->stamp_; 00313 ret.frame_id_ = handlePoses[numHandles]->frame_id_; 00314 numav++; 00315 numHandles = handlePoses.size(); 00316 } 00317 00318 handle_mutex.unlock(); 00319 rate.sleep(); 00320 ros::spinOnce(); 00321 } 00322 00323 average *= 1.0f / numav; 00324 ret.setOrigin(average); 00325 00326 ret = Geometry::getPoseIn("map", ret); 00327 00328 system("rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node projector_mode 1"); 00329 00330 return ret; 00331 } 00332 00333 void Perception3d::fridgePlaneCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) 00334 { 00335 //make this a boost shared ptr! 00336 tf::Stamped<tf::Pose> *newPose = new tf::Stamped<tf::Pose>(); 00337 newPose->frame_id_ = msg->header.frame_id; 00338 newPose->stamp_ = msg->header.stamp; 00339 pcl::PointXYZ point_max, point_min, point_center; 00340 pcl::PointCloud<pcl::PointXYZ> cloud; 00341 pcl::fromROSMsg(*msg, cloud); 00342 pcl::getMinMax3D (cloud, point_min, point_max); 00343 //Calculate the centroid of the hull 00344 point_center.x = (point_max.x + point_min.x)/2; 00345 point_center.y = (point_max.y + point_min.y)/2; 00346 point_center.z = (point_max.z + point_min.z)/2; 00347 00348 newPose->setOrigin(btVector3(point_center.x,point_center.y,point_center.z)); 00349 //newPose->setRotation(btQuaternion(msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z,msg->pose.orientation.w)); 00350 ROS_INFO("CALLBACK GOT PLANE CENTER AT %f %f %f", point_center.x,point_center.y,point_center.z); 00351 plane_mutex.lock(); 00352 planePoses.push_back(newPose); 00353 plane_mutex.unlock(); 00354 } 00355 00356 tf::Stamped<tf::Pose> Perception3d::getFridgePlaneCenterPose() 00357 { 00358 int sysret = system("rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node '{projector_mode: 3, narrow_stereo_trig_mode: 3}'"); 00359 ROS_INFO("%i rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node '{projector_mode: 3, narrow_stereo_trig_mode: 3}'", sysret); 00360 00361 ros::NodeHandle n_; 00362 ros::Subscriber sub_= n_.subscribe("/stereo_table_cluster_detector/extract_clusters_on_table/table_inliers", 100, &Perception3d::fridgePlaneCloudCallback); 00363 00364 size_t numPlanes = 0; 00365 ros::Rate rate(20); 00366 00367 btVector3 average(0,0,0); 00368 double numav = 0; 00369 00370 tf::Stamped<tf::Pose> ret; 00371 00372 while (ros::ok() && (numPlanes < 1)) 00373 { 00374 plane_mutex.lock(); 00375 if (Perception3d::planePoses.size() > numPlanes) 00376 { 00377 ROS_INFO("LAST PLANE: %f %f %f ", planePoses[numPlanes]->getOrigin().x(),planePoses[numPlanes]->getOrigin().y(),planePoses[numPlanes]->getOrigin().z()); 00378 average += btVector3(planePoses[numPlanes]->getOrigin().x(),planePoses[numPlanes]->getOrigin().y(),planePoses[numPlanes]->getOrigin().z()); 00379 ret.stamp_ = planePoses[numPlanes]->stamp_; 00380 ret.frame_id_ = planePoses[numPlanes]->frame_id_; 00381 numav++; 00382 numPlanes = planePoses.size(); 00383 } 00384 00385 plane_mutex.unlock(); 00386 rate.sleep(); 00387 ros::spinOnce(); 00388 } 00389 00390 average *= 1.0f / numav; 00391 ret.setOrigin(average); 00392 00393 ret = Geometry::getPoseIn("map", ret); 00394 00395 system("rosrun dynamic_reconfigure dynparam set /camera_synchronizer_node projector_mode 1"); 00396 00397 return ret; 00398 }